Skip to content
Snippets Groups Projects
Commit aec2ac91 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge remote-tracking branch 'origin/devel' into 454-implementation-of-new-nodes-creation

parents 643643ce b79ddca8
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #13517 failed
Stage: deploy_plugins
    Stage: deploy_ros
      Hide whitespace changes
      Inline Side-by-side
      Showing
      with 62 additions and 93 deletions
      .gitlab-ci.yml
      + 28
      − 0
      View file @ aec2ac91
      ......@@ -152,6 +152,8 @@ deploy_imu:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH != "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      DEPLOY_CI_ROS: "false"
      ......@@ -164,6 +166,8 @@ deploy_gnss:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH != "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
      ......@@ -177,6 +181,8 @@ deploy_vision:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH != "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      DEPLOY_CI_ROS: "false"
      ......@@ -189,6 +195,8 @@ deploy_laser:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH != "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
      ......@@ -202,6 +210,8 @@ deploy_apriltag:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH != "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
      ......@@ -215,6 +225,8 @@ deploy_bodydynamics:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH != "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
      ......@@ -229,6 +241,8 @@ deploy_imu_main:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH == "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: main
      DEPLOY_CI_ROS: "false"
      ......@@ -241,6 +255,8 @@ deploy_gnss_main:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH == "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: main
      GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
      ......@@ -254,6 +270,8 @@ deploy_vision_main:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH == "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: main
      DEPLOY_CI_ROS: "false"
      ......@@ -266,6 +284,8 @@ deploy_laser_main:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH == "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: main
      LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
      ......@@ -279,6 +299,8 @@ deploy_apriltag_main:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH == "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: main
      WOLF_VISION_BRANCH: main
      ......@@ -292,6 +314,8 @@ deploy_bodydynamics_main:
      stage: deploy_plugins
      rules:
      - if: $CI_COMMIT_BRANCH == "main"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: main
      WOLF_IMU_BRANCH: main
      ......@@ -312,6 +336,8 @@ deploy_wolf_ros_node:
      stage: deploy_ros
      rules:
      - if: $CI_COMMIT_BRANCH != "main" && $DEPLOY_CI_ROS == "true"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
      ......@@ -337,6 +363,8 @@ deploy_wolf_ros_node_main:
      stage: deploy_ros
      rules:
      - if: $CI_COMMIT_BRANCH == "main" && $DEPLOY_CI_ROS == "true"
      - if: $CI_PIPELINE_SOURCE == "merge_request_event"
      when: never
      variables:
      WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
      WOLF_IMU_BRANCH: main
      ......
      include/core/sensor/sensor_base.h
      + 1
      − 2
      View file @ aec2ac91
      ......@@ -26,7 +26,6 @@ namespace wolf{
      class HardwareBase;
      class ProcessorBase;
      class StateBlock;
      class ParamsServer;
      }
      //Wolf includes
      ......@@ -98,7 +97,7 @@ struct ParamsSensorBase: public ParamsBase
      name = _input_node["name"].as<std::string>();
      }
      ~ParamsSensorBase() override = default;
      using ParamsBase::ParamsBase;
      std::string print() const override
      {
      return "name: " + name;
      ......
      include/core/sensor/sensor_pose.h
      + 15
      − 35
      View file @ aec2ac91
      ......@@ -31,21 +31,24 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
      struct ParamsSensorPose : public ParamsSensorBase
      {
      double std_p = 1; // m
      double std_o = 1; // rad
      Eigen::VectorXd std_noise;
      // double std_p = 1; // m
      // double std_o = 1; // rad
      ParamsSensorPose() = default;
      ParamsSensorPose(const YAML::Node& _input_node):
      ParamsSensorBase(_input_node)
      {
      std_p = _input_node["std_p"].as<double>();
      std_o = _input_node["std_o"].as<double>();
      std_noise = _input_node["std_noise"].as<Eigen::VectorXd>();
      // std_p = _input_node["std_p"].as<double>();
      // std_o = _input_node["std_o"].as<double>();
      }
      ~ParamsSensorPose() override = default;
      std::string print() const override
      {
      return ParamsSensorBase::print() + "\n"
      + "std_p: " + toString(std_p) + "\n"
      + "std_o: " + toString(std_o) + "\n";
      + "std_p: " + toString(std_noise) + "\n";
      // + "std_p: " + toString(std_p) + "\n"
      // + "std_o: " + toString(std_o) + "\n";
      }
      };
      ......@@ -65,45 +68,22 @@ class SensorPose : public SensorBase
      params_pose_(_params)
      {
      static_assert(DIM == 2 or DIM == 3);
      if (DIM == 2 and params_pose_->std_noise.size() != 3)
      throw std::runtime_error("SensorBase: wrong std_noise size for 2D: " + toString(params_pose_->std_noise.size()) + "(should be 3)");
      if (DIM == 3 and params_pose_->std_noise.size() != 6)
      throw std::runtime_error("SensorBase: wrong std_noise size for 3D: " + toString(params_pose_->std_noise.size()) + "(should be 6)");
      }
      WOLF_SENSOR_CREATE(SensorPose<DIM>, ParamsSensorPose);
      ~SensorPose() override = default;
      double getStdP() const;
      double getStdO() const;
      Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
      };
      template <unsigned int DIM>
      inline double SensorPose<DIM>::getStdP() const
      {
      return params_pose_->std_p;
      }
      template <unsigned int DIM>
      inline double SensorPose<DIM>::getStdO() const
      {
      return params_pose_->std_o;
      }
      template <>
      inline Eigen::MatrixXd SensorPose<2>::computeNoiseCov(const Eigen::VectorXd & _data) const
      {
      return (Eigen::Vector3d() << params_pose_->std_p,
      params_pose_->std_p,
      params_pose_->std_o).finished().cwiseAbs2().asDiagonal();
      }
      template <>
      inline Eigen::MatrixXd SensorPose<3>::computeNoiseCov(const Eigen::VectorXd & _data) const
      inline Eigen::MatrixXd SensorPose<DIM>::computeNoiseCov(const Eigen::VectorXd & _data) const
      {
      return (Eigen::Vector6d() << params_pose_->std_p,
      params_pose_->std_p,
      params_pose_->std_p,
      params_pose_->std_o,
      params_pose_->std_o,
      params_pose_->std_o).finished().cwiseAbs2().asDiagonal();
      return params_pose_->std_noise.cwiseAbs2().asDiagonal();
      }
      typedef SensorPose<2> SensorPose2d;
      ......
      schema/sensor/SensorPose2d.schema
      + 3
      − 8
      View file @ aec2ac91
      follow: SensorBase.schema
      std_p:
      std_noise:
      _mandatory: true
      _type: double
      _doc: standard deviation of the position measurement.
      std_o:
      _mandatory: true
      _type: double
      _doc: standard deviation of the orientation measurement.
      _type: Eigen::Vector3d
      _doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
      states:
      P:
      ......
      schema/sensor/SensorPose3d.schema
      + 3
      − 8
      View file @ aec2ac91
      follow: SensorBase.schema
      std_p:
      std_noise:
      _mandatory: true
      _type: double
      _doc: standard deviation of the position measurement.
      std_o:
      _mandatory: true
      _type: double
      _doc: standard deviation of the orientation measurement.
      _type: Eigen::Vector6d
      _doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
      states:
      P:
      ......
      test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
      + 3
      − 1
      View file @ aec2ac91
      ......@@ -116,8 +116,10 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
      solver = std::make_shared<SolverCeres>(problem);
      // Create sensor to be able to initialize
      auto S_params = std::make_shared<ParamsSensorPose>();
      S_params->std_noise = Eigen::Vector6d::Ones();
      S = SensorBase::emplace<SensorPose3d>(problem->getHardware(),
      std::make_shared<ParamsSensorPose>(),
      S_params,
      SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",pos_camera)},
      {'O',SpecStateSensor("StateQuaternion",vquat_camera)}});
      ......
      test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
      + 1
      − 2
      View file @ aec2ac91
      ......@@ -120,8 +120,7 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
      // pose sensor and proc (to get extrinsics in the prb)
      auto params_sp = std::make_shared<ParamsSensorPose>();
      params_sp->name = "pose sensor";
      params_sp->std_p = 1;
      params_sp->std_o = 1;
      params_sp->std_noise = Vector6d::Ones();
      sensor_pose_ = SensorBase::emplace<SensorPose3d>(problem_->getHardware(),
      params_sp,
      SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",b_p_bm_,"initial_guess")},
      ......
      test/gtest_sensor_pose.cpp
      + 6
      − 32
      View file @ aec2ac91
      ......@@ -19,12 +19,6 @@
      // along with this program. If not, see <http://www.gnu.org/licenses/>.
      //
      //--------LICENSE_END--------
      /**
      * \file gtest_sensor_pose.cpp
      *
      * Created on: Feb 18, 2020
      * \author: mfourmy
      */
      #include "core/sensor/sensor_pose.h"
      #include "core/sensor/factory_sensor.h"
      ......@@ -42,17 +36,16 @@ Vector2d p_state_2D = (Vector2d() << 1,2).finished();
      Vector3d p_state_3D = (Vector3d() << 1,2,3).finished();
      Vector1d o_state_2D = (Vector1d() << 3).finished();
      Vector4d o_state_3D = (Vector4d() << .5,.5,.5,.5).finished();
      double std_p = 0.2;
      double std_o = 0.1;
      Matrix3d noise_cov_2D = (Vector3d() << std_p, std_p, std_o).finished().cwiseAbs2().asDiagonal();
      Matrix6d noise_cov_3D = (Vector6d() << std_p, std_p, std_p, std_o, std_o, std_o).finished().cwiseAbs2().asDiagonal();
      Vector3d std_noise_2D = (Vector3d() << 0.1, 0.2, 0.3).finished();
      Vector6d std_noise_3D = (Vector6d() << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
      Matrix3d noise_cov_2D = std_noise_2D.cwiseAbs2().asDiagonal();
      Matrix6d noise_cov_3D = std_noise_3D.cwiseAbs2().asDiagonal();
      TEST(Pose, constructor_priors_2D)
      {
      auto param = std::make_shared<ParamsSensorPose>();
      param->name = "a not so cool sensor";
      param->std_p = std_p;
      param->std_o = std_o;
      param->std_noise = std_noise_2D;
      SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic
      {'O',SpecStateSensor("StateAngle", o_state_2D)}}; //default "fix", not dynamic
      ......@@ -67,9 +60,6 @@ TEST(Pose, constructor_priors_2D)
      ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
      ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
      ASSERT_EQ(sen->getStdP(), std_p);
      ASSERT_EQ(sen->getStdO(), std_o);
      ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
      }
      ......@@ -77,8 +67,7 @@ TEST(Pose, constructor_priors_3D)
      {
      auto param = std::make_shared<ParamsSensorPose>();
      param->name = "a not so cool sensor";
      param->std_p = std_p;
      param->std_o = std_o;
      param->std_noise = std_noise_3D;
      SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic
      {'O',SpecStateSensor("StateQuaternion", o_state_3D)}}; //default "fix", not dynamic
      ......@@ -93,9 +82,6 @@ TEST(Pose, constructor_priors_3D)
      ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
      ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
      ASSERT_EQ(sen->getStdP(), std_p);
      ASSERT_EQ(sen->getStdO(), std_o);
      ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
      }
      ......@@ -117,9 +103,6 @@ TEST(Pose, factory_2D)
      ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
      ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
      ASSERT_EQ(sen->getStdP(), std_p);
      ASSERT_EQ(sen->getStdO(), std_o);
      ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
      }
      ......@@ -141,9 +124,6 @@ TEST(Pose, factory_3D)
      ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
      ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
      ASSERT_EQ(sen->getStdP(), std_p);
      ASSERT_EQ(sen->getStdO(), std_o);
      ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
      }
      ......@@ -161,9 +141,6 @@ TEST(Pose, factory_yaml_2D)
      ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
      ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
      ASSERT_EQ(sen->getStdP(), std_p);
      ASSERT_EQ(sen->getStdO(), std_o);
      ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
      }
      ......@@ -181,9 +158,6 @@ TEST(Pose, factory_yaml_3D)
      ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
      ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
      ASSERT_EQ(sen->getStdP(), std_p);
      ASSERT_EQ(sen->getStdO(), std_o);
      ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
      }
      ......
      test/yaml/sensor_pose_2d.yaml
      + 1
      − 3
      View file @ aec2ac91
      ......@@ -9,6 +9,4 @@ states:
      state: [3]
      dynamic: false
      std_p: 0.2
      std_o: 0.1
      std_noise: [0.1, 0.2, 0.3]
      test/yaml/sensor_pose_3d.yaml
      + 1
      − 2
      View file @ aec2ac91
      ......@@ -9,5 +9,4 @@ states:
      state: [0.5, 0.5, 0.5, 0.5]
      dynamic: false
      std_p: 0.2
      std_o: 0.1
      std_noise: [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
      0% or .
      You are about to add 0 people to the discussion. Proceed with caution.
      Finish editing this message first!
      Please register or to comment