From 4536b824c2019b8510ca4de4fbd26b62693a4f8e Mon Sep 17 00:00:00 2001 From: jcasals <jcasals@iri.upc.edu> Date: Tue, 7 Jan 2020 10:52:23 +0100 Subject: [PATCH] Change factory keys and adapt code accordingly --- demos/processor_imu.yaml | 2 +- demos/processor_odom_3D.yaml | 4 +- demos/sensor_imu.yaml | 4 +- demos/sensor_odom_3D.yaml | 2 +- include/IMU/factor/factor_IMU.h | 2 +- include/IMU/factor/factor_fix_bias.h | 2 +- src/capture/capture_IMU.cpp | 4 +- src/feature/feature_IMU.cpp | 4 +- src/feature/feature_apriltag.cpp | 2 +- src/landmark/landmark_apriltag.cpp | 4 +- src/processor/processor_IMU.cpp | 4 +- .../processor_tracker_landmark_apriltag.cpp | 4 +- src/sensor/sensor_IMU.cpp | 4 +- src/yaml/processor_IMU_yaml.cpp | 5 +- src/yaml/sensor_IMU_yaml.cpp | 4 +- test/gtest_IMU.cpp | 8 ++-- test/gtest_factor_IMU.cpp | 48 +++++++++---------- test/gtest_feature_IMU.cpp | 4 +- test/gtest_processor_IMU.cpp | 14 +++--- 19 files changed, 63 insertions(+), 62 deletions(-) diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml index b86d47713..5c623b82d 100644 --- a/demos/processor_imu.yaml +++ b/demos/processor_imu.yaml @@ -1,4 +1,4 @@ -type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. +type: "ProcessorIMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. time_tolerance: 0.0025 # Time tolerance for joining KFs unmeasured_perturbation_std: 0.00001 diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3D.yaml index ef8f7fd14..c0dc6463a 100644 --- a/demos/processor_odom_3D.yaml +++ b/demos/processor_odom_3D.yaml @@ -1,4 +1,4 @@ -type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. +type: "ProcessorOdom3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. time_tolerance: 0.01 # seconds @@ -10,4 +10,4 @@ keyframe_vote: dist_traveled: 0.5 # meters angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) -unmeasured_perturbation_std: 0.001 \ No newline at end of file +unmeasured_perturbation_std: 0.001 diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml index 2716803c3..dd576c839 100644 --- a/demos/sensor_imu.yaml +++ b/demos/sensor_imu.yaml @@ -1,4 +1,4 @@ -type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. +type: "SensorIMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error. motion variances: a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2 @@ -6,4 +6,4 @@ motion variances: ab_initial_stdev: 0.800 # m/s2 - initial bias wb_initial_stdev: 0.350 # rad/sec - initial bias ab_rate_stdev: 0.1 # m/s2/sqrt(s) - wb_rate_stdev: 0.0400 # rad/s/sqrt(s) \ No newline at end of file + wb_rate_stdev: 0.0400 # rad/s/sqrt(s) diff --git a/demos/sensor_odom_3D.yaml b/demos/sensor_odom_3D.yaml index c45adb8ca..9fb43d4c3 100644 --- a/demos/sensor_odom_3D.yaml +++ b/demos/sensor_odom_3D.yaml @@ -1,4 +1,4 @@ -type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. +type: "SensorOdom3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. k_disp_to_disp: 0.02 # m^2 / m k_disp_to_rot: 0.02 # rad^2 / m diff --git a/include/IMU/factor/factor_IMU.h b/include/IMU/factor/factor_IMU.h index 0378abb66..91e573f6e 100644 --- a/include/IMU/factor/factor_IMU.h +++ b/include/IMU/factor/factor_IMU.h @@ -152,7 +152,7 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr, bool _apply_loss_function, FactorStatus _status) : FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ... - "IMU", + "FactorIMU", _cap_origin_ptr->getFrame(), _cap_origin_ptr, nullptr, diff --git a/include/IMU/factor/factor_fix_bias.h b/include/IMU/factor/factor_fix_bias.h index 876fa5220..6ab168472 100644 --- a/include/IMU/factor/factor_fix_bias.h +++ b/include/IMU/factor/factor_fix_bias.h @@ -20,7 +20,7 @@ class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3> { public: FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : - FactorAutodiff<FactorFixBias, 6, 3, 3>("FIX BIAS", + FactorAutodiff<FactorFixBias, 6, 3, 3>("FactorFixBias", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(), std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias()) { diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp index 23874f183..3fd9c4e71 100644 --- a/src/capture/capture_IMU.cpp +++ b/src/capture/capture_IMU.cpp @@ -9,7 +9,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, const Eigen::Vector6s& _acc_gyro_data, const Eigen::MatrixXs& _data_cov, CaptureBasePtr _capture_origin_ptr) : - CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _capture_origin_ptr, nullptr, nullptr, std::make_shared<StateBlock>(6, false)) + CaptureMotion("CaptureImu", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _capture_origin_ptr, nullptr, nullptr, std::make_shared<StateBlock>(6, false)) { // } @@ -20,7 +20,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, const Eigen::MatrixXs& _data_cov, const Vector6s& _bias, CaptureBasePtr _capture_origin_ptr) : - CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _capture_origin_ptr, nullptr, nullptr, std::make_shared<StateBlock>(_bias, false)) + CaptureMotion("CaptureImu", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _capture_origin_ptr, nullptr, nullptr, std::make_shared<StateBlock>(_bias, false)) { // } diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp index ee8d115e2..32339da25 100644 --- a/src/feature/feature_IMU.cpp +++ b/src/feature/feature_IMU.cpp @@ -7,7 +7,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::Vector6s& _bias, const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians, CaptureMotionPtr _cap_imu_ptr) : - FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance), + FeatureBase("FeatureImu", _delta_preintegrated, _delta_preintegrated_covariance), acc_bias_preint_(_bias.head<3>()), gyro_bias_preint_(_bias.tail<3>()), jacobian_bias_(_dD_db_jacobians) @@ -15,7 +15,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, } FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr): - FeatureBase("IMU", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()), + FeatureBase("FeatureImu", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()), acc_bias_preint_ (_cap_imu_ptr->getCalibrationPreint().head<3>()), gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()), jacobian_bias_(_cap_imu_ptr->getJacobianCalib()) diff --git a/src/feature/feature_apriltag.cpp b/src/feature/feature_apriltag.cpp index 18cb89cdf..cbde88a27 100644 --- a/src/feature/feature_apriltag.cpp +++ b/src/feature/feature_apriltag.cpp @@ -11,7 +11,7 @@ FeatureApriltag::FeatureApriltag(const Eigen::Vector7s & _measurement, Scalar _rep_error2, bool _use_rotation, UncertaintyType _uncertainty_type) : - FeatureBase("APRILTAG", _measurement, _meas_uncertainty, _uncertainty_type), + FeatureBase("FeatureApriltag", _measurement, _meas_uncertainty, _uncertainty_type), tag_id_ (_tag_id), tag_corners_(4), detection_ (_det), diff --git a/src/landmark/landmark_apriltag.cpp b/src/landmark/landmark_apriltag.cpp index 193c979b2..af72d5217 100644 --- a/src/landmark/landmark_apriltag.cpp +++ b/src/landmark/landmark_apriltag.cpp @@ -7,7 +7,7 @@ namespace wolf { LandmarkApriltag::LandmarkApriltag(Eigen::Vector7s& pose, const int& _tagid, const Scalar& _tag_width) : - LandmarkBase("APRILTAG", std::make_shared<StateBlock>(pose.head(3)), std::make_shared<StateQuaternion>(pose.tail(4))), + LandmarkBase("LandmarkApriltag", std::make_shared<StateBlock>(pose.head(3)), std::make_shared<StateQuaternion>(pose.tail(4))), tag_id_(_tagid), tag_width_(_tag_width) { @@ -86,7 +86,7 @@ YAML::Node LandmarkApriltag::saveToYaml() const // Register landmark creator namespace { -const bool WOLF_UNUSED registered_lmk_apriltag = LandmarkFactory::get().registerCreator("APRILTAG", LandmarkApriltag::create); +const bool WOLF_UNUSED registered_lmk_apriltag = LandmarkFactory::get().registerCreator("LandmarkApriltag", LandmarkApriltag::create); } diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index a410afaaf..84be708f8 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -6,7 +6,7 @@ namespace wolf { ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) : - ProcessorMotion("IMU", 10, 10, 9, 6, 6, _params_motion_IMU), + ProcessorMotion("ProcessorImu", 10, 10, 9, 6, 6, _params_motion_IMU), params_motion_IMU_(std::make_shared<ProcessorParamsIMU>(*_params_motion_IMU)) { // Set constant parts of Jacobians @@ -209,5 +209,5 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint, namespace wolf { -WOLF_REGISTER_PROCESSOR("IMU", ProcessorIMU) +WOLF_REGISTER_PROCESSOR("ProcessorIMU", ProcessorIMU) } diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp index 59e481187..d1223f960 100644 --- a/src/processor/processor_tracker_landmark_apriltag.cpp +++ b/src/processor/processor_tracker_landmark_apriltag.cpp @@ -33,7 +33,7 @@ namespace wolf { // Constructor ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) : - ProcessorTrackerLandmark("TRACKER LANDMARK APRILTAG", _params_tracker_landmark_apriltag ), + ProcessorTrackerLandmark("ProcessorTrackerLandmarkApriltag", _params_tracker_landmark_apriltag ), tag_widths_(_params_tracker_landmark_apriltag->tag_widths_), tag_width_default_(_params_tracker_landmark_apriltag->tag_width_default_), std_xy_ (_params_tracker_landmark_apriltag->std_xy_ ), @@ -664,6 +664,6 @@ std::string ProcessorTrackerLandmarkApriltag::getTagFamily() const namespace wolf { -WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG", ProcessorTrackerLandmarkApriltag) +WOLF_REGISTER_PROCESSOR("ProcessorTrackerLandmarkApriltag", ProcessorTrackerLandmarkApriltag) } diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp index 33cd3e16e..ce62575a5 100644 --- a/src/sensor/sensor_IMU.cpp +++ b/src/sensor/sensor_IMU.cpp @@ -11,7 +11,7 @@ SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _param } SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) : - SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true), + 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::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true), a_noise(_params.a_noise), w_noise(_params.w_noise), ab_initial_stdev(_params.ab_initial_stdev), @@ -45,5 +45,5 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve // Register in the SensorFactory #include "core/sensor/sensor_factory.h" namespace wolf { -WOLF_REGISTER_SENSOR("IMU", SensorIMU) +WOLF_REGISTER_SENSOR("SensorIMU", SensorIMU) } // namespace wolf diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp index a0ed78017..8bc55f707 100644 --- a/src/yaml/processor_IMU_yaml.cpp +++ b/src/yaml/processor_IMU_yaml.cpp @@ -23,8 +23,9 @@ namespace static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _filename_dot_yaml) { YAML::Node config = YAML::LoadFile(_filename_dot_yaml); + std::cout << _filename_dot_yaml << '\n'; - if (config["type"].as<std::string>() == "IMU") + if (config["type"].as<std::string>() == "ProcessorIMU") { YAML::Node kf_vote = config["keyframe_vote"]; @@ -46,7 +47,7 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file } // Register in the SensorFactory -const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("IMU", createProcessorIMUParams); +const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("ProcessorIMU", createProcessorIMUParams); } // namespace [unnamed] diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp index 0d6d4c5d4..d9a0a403c 100644 --- a/src/yaml/sensor_IMU_yaml.cpp +++ b/src/yaml/sensor_IMU_yaml.cpp @@ -25,7 +25,7 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y { YAML::Node config = YAML::LoadFile(_filename_dot_yaml); - if (config["type"].as<std::string>() == "IMU") + if (config["type"].as<std::string>() == "SensorIMU") { YAML::Node variances = config["motion variances"]; YAML::Node kf_vote = config["keyframe vote"]; @@ -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("IMU", createIntrinsicsIMU); +const bool WOLF_UNUSED registered_imu_intr = IntrinsicsFactory::get().registerCreator("SensorIMU", createIntrinsicsIMU); } // namespace [unnamed] diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp index 2e06cb014..a6d122cf3 100644 --- a/test/gtest_IMU.cpp +++ b/test/gtest_IMU.cpp @@ -97,8 +97,8 @@ class Process_Factor_IMU : public testing::Test ceres_manager = make_shared<CeresManager>(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sensor = problem->installSensor ("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - ProcessorBasePtr processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sensor = problem->installSensor ("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + ProcessorBasePtr processor = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sensor_imu = static_pointer_cast<SensorIMU> (sensor); processor_imu = static_pointer_cast<ProcessorIMU>(processor); @@ -603,8 +603,8 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU // ===================================== ODO string wolf_root = _WOLF_IMU_ROOT_DIR; - SensorBasePtr sensor = problem->installSensor ("ODOM 3D", "Odometer", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml" ); - ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/demos/processor_odom_3D.yaml"); + SensorBasePtr sensor = problem->installSensor ("SensorOdom3D", "Odometer", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml" ); + ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom3D", "Odometer", "Odometer" , wolf_root + "/demos/processor_odom_3D.yaml"); sensor_odo = static_pointer_cast<SensorOdom3D>(sensor); diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp index f8fa7b85b..098d85d51 100644 --- a/test/gtest_factor_IMU.cpp +++ b/test/gtest_factor_IMU.cpp @@ -73,8 +73,8 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -161,8 +161,8 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -245,8 +245,8 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -329,8 +329,8 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -415,8 +415,8 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -502,8 +502,8 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -585,8 +585,8 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -678,8 +678,8 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -779,8 +779,8 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -879,9 +879,9 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test ceres_manager = std::make_shared<CeresManager>(problem, ceres_options); // SENSOR + PROCESSOR IMU - sensor = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + sensor = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); sensor_imu = std::static_pointer_cast<SensorIMU>(sensor); - processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + processor = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor); // SENSOR + PROCESSOR ODOM 3D @@ -1062,8 +1062,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); @@ -1204,8 +1204,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); - processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor = problem->installProcessor("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor); diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index f5e9a0c2b..a6d6481ad 100644 --- a/test/gtest_feature_IMU.cpp +++ b/test/gtest_feature_IMU.cpp @@ -38,13 +38,13 @@ class FeatureIMU_test : public testing::Test Eigen::VectorXs 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>(); - SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params); + 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; prc_imu_params->max_buff_length = 10; prc_imu_params->dist_traveled = 5; prc_imu_params->angle_turned = 0.5; - processor_ptr_ = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params); + processor_ptr_ = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params); // Time and data variables TimeStamp t; diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp index 9c51e9c2b..2f9413f0f 100644 --- a/test/gtest_processor_IMU.cpp +++ b/test/gtest_processor_IMU.cpp @@ -53,8 +53,8 @@ class ProcessorIMUt : public testing::Test // Wolf problem problem = Problem::create("POV", 3); Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished(); - sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); - ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); // Time and data variables data = Vector6s::Zero(); @@ -102,16 +102,16 @@ TEST(ProcessorIMU_constructors, ALL) std::string wolf_root = _WOLF_IMU_ROOT_DIR; ProblemPtr problem = Problem::create("POV", 3); Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished(); - SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); + SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); ProcessorParamsIMUPtr params_default = std::make_shared<ProcessorParamsIMU>(); - ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, params_default); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, params_default); ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default->max_time_span); ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length); ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default->dist_traveled); ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default->angle_turned); //Factory constructor with yaml - processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); + processor_ptr = problem->installProcessor("ProcessorIMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0); ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000); ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0); @@ -132,14 +132,14 @@ TEST(ProcessorIMU, voteForKeyFrame) // Wolf problem ProblemPtr problem = Problem::create("POV", 3); Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished(); - SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); + SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>(); prc_imu_params->max_time_span = 1; prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass prc_imu_params->dist_traveled = 1000000000; prc_imu_params->angle_turned = 1000000000; prc_imu_params->voting_active = true; - ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params); //setting origin VectorXs x0(10); -- GitLab