diff --git a/include/IMU/sensor/sensor_IMU.h b/include/IMU/sensor/sensor_IMU.h index c5da4386a10bc06e40609f16eba9ee9d31fef084..c8e13a2d8bf608c61aef462ea5f81e58946b2a9f 100644 --- a/include/IMU/sensor/sensor_IMU.h +++ b/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); }; diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp index 393af0da7bd0f09ce4f5b5999f2af8c6042ce799..1036b78214a0ee9d4d15c2ab8f0b677f0da96d6f 100644 --- a/src/sensor/sensor_IMU.cpp +++ b/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; diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp index ab67315b6e3c09bd431b0f88c3bcfb62bdc29078..89d7c382f48d8202fa439009ba9ff3a5191cb3e9 100644 --- a/src/yaml/sensor_IMU_yaml.cpp +++ b/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] diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 266bdfb1e5e5e3faf8adf566570285b0be5eee57..a6a1d9cd781eeb9d1c3349e7db9794e5c57d1aea 100644 --- a/test/CMakeLists.txt +++ b/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) diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index 6d120c6298eb9c39b9234f2bc1ec2106480a042a..ba537cc567b516cbc4ee5470644b2d059296e372 100644 --- a/test/gtest_feature_IMU.cpp +++ b/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;