Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/imu into devel

include/IMU/sensor/sensor_IMU.h

@@ -8,10 +8,10 @@
 
 namespace wolf {
 
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorIMU);
 
 
-struct IntrinsicsIMU : public IntrinsicsBase
+struct ParamsSensorIMU : public ParamsSensorBase
 {
         //noise std dev
         double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
@@ -25,13 +25,13 @@ struct IntrinsicsIMU : public IntrinsicsBase
         double ab_rate_stdev = 0.00001;
         double wb_rate_stdev = 0.00001;
 
-        virtual ~IntrinsicsIMU() = default;
-    IntrinsicsIMU()
+        virtual ~ParamsSensorIMU() = default;
+    ParamsSensorIMU()
     {
         //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
     }
-    IntrinsicsIMU(std::string _unique_name, const ParamsServer& _server):
-        IntrinsicsBase(_unique_name, _server)
+    ParamsSensorIMU(std::string _unique_name, const ParamsServer& _server):
+        ParamsSensorBase(_unique_name, _server)
     {
         w_noise             = _server.getParam<double>(_unique_name + "/w_noise");
         a_noise             = _server.getParam<double>(_unique_name + "/a_noise");
@@ -42,7 +42,7 @@ struct IntrinsicsIMU : public IntrinsicsBase
     }
     std::string print() const
     {
-        return "\n" + IntrinsicsBase::print()                           + "\n"
+        return "\n" + ParamsSensorBase::print()                           + "\n"
             + "w_noise: "           + std::to_string(w_noise)           + "\n"
             + "a_noise: "           + std::to_string(a_noise)           + "\n"
             + "ab_initial_stdev: "  + std::to_string(ab_initial_stdev)  + "\n"
@@ -69,8 +69,8 @@ class SensorIMU : public SensorBase
 
     public:
 
-        SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params);
-        SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params);
+        SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params);
+        SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params);
 
         double getGyroNoise() const;
         double getAccelNoise() const;
@@ -82,7 +82,7 @@ class SensorIMU : public SensorBase
         virtual ~SensorIMU();
 
     public:
-        static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics = nullptr);
+        static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorBasePtr _intrinsics = nullptr);
 
 };
 

src/sensor/sensor_IMU.cpp

@@ -4,13 +4,13 @@
 
 namespace wolf {
 
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params) :
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params) :
         SensorIMU(_extrinsics, *_params)
 {
     //
 }
 
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params) :
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params) :
         SensorBase("SensorIMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, false, true),
         a_noise(_params.a_noise),
         w_noise(_params.w_noise),
@@ -29,12 +29,12 @@ SensorIMU::~SensorIMU()
 
 // Define the factory method
 SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq,
-                              const IntrinsicsBasePtr _intrinsics)
+                              const ParamsSensorBasePtr _intrinsics)
 {
     // decode extrinsics vector
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
 
-    IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
+    ParamsSensorIMUPtr params = std::static_pointer_cast<ParamsSensorIMU>(_intrinsics);
     SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
     sen->setName(_unique_name);
     return sen;

src/yaml/sensor_IMU_yaml.cpp

@@ -21,7 +21,7 @@ namespace wolf
 namespace
 {
 
-static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_yaml)
+static ParamsSensorBasePtr createParamsSensorIMU(const std::string & _filename_dot_yaml)
 {
     YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
 
@@ -30,7 +30,7 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
         YAML::Node variances        = config["motion_variances"];
         YAML::Node kf_vote          = config["keyframe_vote"];
 
-        IntrinsicsIMUPtr params = std::make_shared<IntrinsicsIMU>();
+        ParamsSensorIMUPtr params = std::make_shared<ParamsSensorIMU>();
 
         params->a_noise             = variances["a_noise"]          .as<double>();
         params->w_noise             = variances["w_noise"]          .as<double>();
@@ -47,7 +47,7 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
 }
 
 // Register in the SensorFactory
-const bool WOLF_UNUSED registered_imu_intr = IntrinsicsFactory::get().registerCreator("SensorIMU", createIntrinsicsIMU);
+const bool WOLF_UNUSED registered_imu_intr = ParamsSensorFactory::get().registerCreator("SensorIMU", createParamsSensorIMU);
 
 } // namespace [unnamed]
 

test/CMakeLists.txt

@@ -14,6 +14,7 @@ target_link_libraries(gtest_example ${PLUGIN_NAME})      #
 ###########################################################
 
 wolf_add_gtest(gtest_processor_IMU gtest_processor_IMU.cpp)
+message("WHAT IS HERE ${PLUGIN_NAME} ${wolf_LIBRARY}")
 target_link_libraries(gtest_processor_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
 
 wolf_add_gtest(gtest_IMU gtest_IMU.cpp)

test/gtest_feature_IMU.cpp

@@ -38,7 +38,7 @@ class FeatureIMU_test : public testing::Test
         problem = Problem::create("POV", 3);
         Eigen::VectorXd IMU_extrinsics(7);
         IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-        IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
+        ParamsSensorIMUPtr sen_imu_params = std::make_shared<ParamsSensorIMU>();
         SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", IMU_extrinsics, sen_imu_params);
         ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
         prc_imu_params->max_time_span   = 0.5;