diff --git a/demos/processor_odom_3d.yaml b/demos/processor_odom_3d.yaml
index c0dc6463ade663803424f87e1cc1898c61b17561..491e9c8d2db5f258e96d60d0a2ce8321834e9fa0 100644
--- a/demos/processor_odom_3d.yaml
+++ b/demos/processor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "ProcessorOdom3D" # 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
diff --git a/demos/sensor_odom_3d.yaml b/demos/sensor_odom_3d.yaml
index 9fb43d4c30c0f5c01757362f6122d0cba98b14c5..58db1c088fbc80339a78ba815fddbaf69674d3b6 100644
--- a/demos/sensor_odom_3d.yaml
+++ b/demos/sensor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom3D" # 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/capture/capture_imu.h b/include/imu/capture/capture_imu.h
index ef150f8b9916696d990718a7558feb45f46fd812..d080a04f2e512b6ed40fe76737e3eb2df0bc718c 100644
--- a/include/imu/capture/capture_imu.h
+++ b/include/imu/capture/capture_imu.h
@@ -7,36 +7,36 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(CaptureIMU);
+WOLF_PTR_TYPEDEFS(CaptureImu);
-class CaptureIMU : public CaptureMotion
+class CaptureImu : public CaptureMotion
{
public:
- CaptureIMU(const TimeStamp& _init_ts,
+ CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
- CaptureIMU(const TimeStamp& _init_ts,
+ CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _data,
const Eigen::MatrixXd& _data_cov,
const Vector6d& _bias,
CaptureBasePtr _capture_origin_ptr = nullptr);
- virtual ~CaptureIMU();
+ virtual ~CaptureImu();
virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
};
-inline Eigen::VectorXd CaptureIMU::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+inline Eigen::VectorXd CaptureImu::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
{
return imu::plus(_delta, _delta_error);
}
} // namespace wolf
-#endif // CAPTURE_IMU_H
+#endif // CAPTURE_Imu_H
diff --git a/include/imu/factor/factor_fix_bias.h b/include/imu/factor/factor_fix_bias.h
index 9292e92374a862a41b67eb37f4a1a05669a88602..85a30d524d57efeda7f5704d4f708e074ac27db1 100644
--- a/include/imu/factor/factor_fix_bias.h
+++ b/include/imu/factor/factor_fix_bias.h
@@ -21,8 +21,8 @@ class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
public:
FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
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())
+ 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())
{
// std::cout << "created FactorFixBias " << std::endl;
}
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 63df0350702725c2ee127a8a1f52d579f61fba73..bb81e4a653027feb75efd9eefd9ac189cbcf5c08 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -11,19 +11,19 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorIMU);
+WOLF_PTR_TYPEDEFS(FactorImu);
//class
-class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
+class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
{
public:
- FactorIMU(const FeatureIMUPtr& _ftr_ptr,
- const CaptureIMUPtr& _capture_origin_ptr,
+ FactorImu(const FeatureImuPtr& _ftr_ptr,
+ const CaptureImuPtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
- virtual ~FactorIMU() = default;
+ virtual ~FactorImu() = default;
virtual std::string getTopology() const override
{
@@ -153,13 +153,13 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
///////////////////// IMPLEMENTAITON ////////////////////////////
-inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
- const CaptureIMUPtr& _cap_origin_ptr,
+inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
+ const CaptureImuPtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
- "FactorIMU",
+ FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
+ "FactorImu",
_cap_origin_ptr->getFrame(),
_cap_origin_ptr,
nullptr,
@@ -186,8 +186,8 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
- wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
+ ab_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
+ wb_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
sqrt_A_r_dt_inv((Eigen::Matrix3d::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
sqrt_W_r_dt_inv((Eigen::Matrix3d::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
{
@@ -195,7 +195,7 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
}
template<typename T>
-inline bool FactorIMU::operator ()(const T* const _p1,
+inline bool FactorImu::operator ()(const T* const _p1,
const T* const _q1,
const T* const _v1,
const T* const _b1,
@@ -227,7 +227,7 @@ inline bool FactorIMU::operator ()(const T* const _p1,
return true;
}
-Eigen::Vector9d FactorIMU::error()
+Eigen::Vector9d FactorImu::error()
{
Vector6d bias = capture_other_ptr_.lock()->getCalibration();
@@ -252,7 +252,7 @@ Eigen::Vector9d FactorIMU::error()
}
template<typename D1, typename D2, typename D3>
-inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
+inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _v1,
const Eigen::MatrixBase<D1> & _ab1,
@@ -265,7 +265,7 @@ inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
Eigen::MatrixBase<D3> & _res) const
{
- /* Help for the IMU residual function
+ /* Help for the Imu residual function
*
* Notations:
* D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv]
@@ -428,17 +428,17 @@ inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
return true;
}
-inline Eigen::VectorXd FactorIMU::expectation() const
+inline Eigen::VectorXd FactorImu::expectation() const
{
FrameBasePtr frm_current = getFeature()->getFrame();
FrameBasePtr frm_previous = getFrameOther();
- //get information on current_frame in the FactorIMU
+ //get information on current_frame in the FactorImu
const Eigen::Vector3d frame_current_pos = (frm_current->getP()->getState());
const Eigen::Quaterniond frame_current_ori (frm_current->getO()->getState().data());
const Eigen::Vector3d frame_current_vel = (frm_current->getV()->getState());
- // get info on previous frame in the FactorIMU
+ // get info on previous frame in the FactorImu
const Eigen::Vector3d frame_previous_pos = (frm_previous->getP()->getState());
const Eigen::Quaterniond frame_previous_ori (frm_previous->getO()->getState().data());
const Eigen::Vector3d frame_previous_vel = (frm_previous->getV()->getState());
@@ -458,7 +458,7 @@ inline Eigen::VectorXd FactorIMU::expectation() const
}
template<typename D1, typename D2, typename D3, typename D4>
-inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
+inline void FactorImu::expectation(const Eigen::MatrixBase<D1> & _p1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _v1,
const Eigen::MatrixBase<D1> & _p2,
@@ -480,7 +480,7 @@ inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
* Optimization results
* ================================================
*
- * Using gtest_IMU.cpp
+ * Using gtest_Imu.cpp
*
* Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations.
*
@@ -488,48 +488,48 @@ inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
*
* With Method 1:
*
-[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
-[trace][10:58:16] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][10:58:16] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
-[----------] 3 tests from Process_Factor_IMU (159 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][10:58:16] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][10:58:16] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (120 ms total)
+[ RUN ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
+[trace][10:58:16] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
+[trace][10:58:16] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
+[ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
+[ RUN ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
+[trace][10:58:16] [gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
+[ RUN ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
+[trace][10:58:16] [gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
+[----------] 3 tests from Process_Factor_Imu (159 ms total)
+
+[----------] 2 tests from Process_Factor_Imu_ODO
+[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
+[trace][10:58:16] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
+[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
+[trace][10:58:16] [gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
+[----------] 2 tests from Process_Factor_Imu_ODO (120 ms total)
*
* With Method 2:
*
-[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
-[trace][11:15:43] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][11:15:43] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
-[----------] 3 tests from Process_Factor_IMU (133 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][11:15:43] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][11:15:43] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (127 ms total)
+[ RUN ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
+[trace][11:15:43] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
+[trace][11:15:43] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
+[ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
+[ RUN ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
+[trace][11:15:43] [gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
+[ RUN ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
+[trace][11:15:43] [gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
+[----------] 3 tests from Process_Factor_Imu (133 ms total)
+
+[----------] 2 tests from Process_Factor_Imu_ODO
+[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
+[trace][11:15:43] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
+[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
+[trace][11:15:43] [gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
+[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
+[----------] 2 tests from Process_Factor_Imu_ODO (127 ms total)
*
*/
diff --git a/include/imu/feature/feature_imu.h b/include/imu/feature/feature_imu.h
index 3f45dd14aa613248b59211122567c72117eb77b3..c4ecb458aa0621addc30e540c73a6f0e44a92fec 100644
--- a/include/imu/feature/feature_imu.h
+++ b/include/imu/feature/feature_imu.h
@@ -10,10 +10,10 @@
namespace wolf {
-//WOLF_PTR_TYPEDEFS(CaptureIMU);
-WOLF_PTR_TYPEDEFS(FeatureIMU);
+//WOLF_PTR_TYPEDEFS(CaptureImu);
+WOLF_PTR_TYPEDEFS(FeatureImu);
-class FeatureIMU : public FeatureBase
+class FeatureImu : public FeatureBase
{
public:
@@ -21,12 +21,12 @@ class FeatureIMU : public FeatureBase
*
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
- * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
+ * \param _dD_db_jacobians Jacobians of preintegrated delta wrt Imu biases
* \param acc_bias accelerometer bias of origin frame
* \param gyro_bias gyroscope bias of origin frame
* \param _cap_imu_ptr pointer to parent CaptureMotion
*/
- FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
+ FeatureImu(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::Vector6d& _bias,
const Eigen::Matrix<double,9,6>& _dD_db_jacobians,
@@ -36,9 +36,9 @@ class FeatureIMU : public FeatureBase
*
* \param _cap_imu_ptr pointer to parent CaptureMotion
*/
- FeatureIMU(CaptureMotionPtr _cap_imu_ptr);
+ FeatureImu(CaptureMotionPtr _cap_imu_ptr);
- virtual ~FeatureIMU();
+ virtual ~FeatureImu();
const Eigen::Vector3d& getAccBiasPreint() const;
const Eigen::Vector3d& getGyroBiasPreint() const;
@@ -56,17 +56,17 @@ class FeatureIMU : public FeatureBase
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
-inline const Eigen::Vector3d& FeatureIMU::getAccBiasPreint() const
+inline const Eigen::Vector3d& FeatureImu::getAccBiasPreint() const
{
return acc_bias_preint_;
}
-inline const Eigen::Vector3d& FeatureIMU::getGyroBiasPreint() const
+inline const Eigen::Vector3d& FeatureImu::getGyroBiasPreint() const
{
return gyro_bias_preint_;
}
-inline const Eigen::Matrix<double, 9, 6>& FeatureIMU::getJacobianBias() const
+inline const Eigen::Matrix<double, 9, 6>& FeatureImu::getJacobianBias() const
{
return jacobian_bias_;
}
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index 76a6f5cef00e14e8035204e4f0bd9a4f9e4622a5..5df0828e7a9702531231e07de358b59dab4e0852 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -14,9 +14,9 @@
/*
* Most functions in this file are explained in the document:
*
- * Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC
+ * Joan Sola, "Imu pre-integration", 2015-2017 IRI-CSIC
*
- * They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
+ * They relate manipulations of Delta motion magnitudes used for Imu pre-integration.
*
* The Delta is defined as
* Delta = [Dp, Dq, Dv]
@@ -34,9 +34,9 @@
* - composeOverState: x2 = x1 (+) D
* - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
* - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
- * - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 (+) exp_IMU(d)
- * - diff: d = log_IMU( D2 (-) D1 )
+ * - exp_Imu: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 (+) exp_Imu(d)
+ * - diff: d = log_Imu( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/
@@ -340,7 +340,7 @@ inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 9, 1> log_Imu(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<10, 1>::check(delta_in);
@@ -361,7 +361,7 @@ Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 10, 1> exp_Imu(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<9, 1>::check(d_in);
@@ -578,13 +578,13 @@ void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const Quatern
w_m = w + w_b;
}
-/* Create IMU data from body motion
+/* Create Imu data from body motion
* Input:
* motion : [ax, ay, az, wx, wy, wz] the motion in body frame
* q : the current orientation wrt horizontal
- * bias : the bias of the IMU
+ * bias : the bias of the Imu
* Output:
- * return : the data vector as created by the IMU (with motion, gravity, and bias)
+ * return : the data vector as created by the Imu (with motion, gravity, and bias)
*/
template<typename D1, typename D2, typename D3>
Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias)
@@ -607,4 +607,4 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons
} // namespace imu
} // namespace wolf
-#endif /* IMU_TOOLS_H_ */
+#endif /* Imu_TOOLS_H_ */
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 3d3a04390065ce0d523c78db0a27e6057437dd7e..bf3d20d7a08c22be8256e38a502d0c5e0aacea68 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -92,4 +92,4 @@ inline Eigen::VectorXd ProcessorImu::deltaZero() const
} // namespace wolf
-#endif // PROCESSOR_IMU_H
+#endif // PROCESSOR_Imu_H
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index b2cd4dbcae2b16949b52312f911e50e3f4cd3506..45cd49c7d1c233e6592e9221ae5fd26bf3a6e053 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -116,4 +116,4 @@ inline double SensorImu::getAbRateStdev() const
} // namespace wolf
-#endif // SENSOR_IMU_H
+#endif // SENSOR_Imu_H
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index 08e7dbc3cc90b9f81707f8677be6a5c6219601f8..90d80f0a8275243d99696891cb70708f114e4fd8 100644
--- a/src/capture/capture_imu.cpp
+++ b/src/capture/capture_imu.cpp
@@ -4,7 +4,7 @@
namespace wolf {
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
+CaptureImu::CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _acc_gyro_data,
const Eigen::MatrixXd& _data_cov,
@@ -14,7 +14,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
//
}
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
+CaptureImu::CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _acc_gyro_data,
const Eigen::MatrixXd& _data_cov,
@@ -25,7 +25,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
//
}
-CaptureIMU::~CaptureIMU()
+CaptureImu::~CaptureImu()
{
//
}
diff --git a/src/feature/feature_imu.cpp b/src/feature/feature_imu.cpp
index 405c1403b71a75df4f93d3ce402b8828b5697dfa..d3547a52c2c6794ed2d795af780ca2063ef14e36 100644
--- a/src/feature/feature_imu.cpp
+++ b/src/feature/feature_imu.cpp
@@ -2,7 +2,7 @@
namespace wolf {
-FeatureIMU::FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
+FeatureImu::FeatureImu(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::Vector6d& _bias,
const Eigen::Matrix<double,9,6>& _dD_db_jacobians,
@@ -14,7 +14,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
{
}
-FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
+FeatureImu::FeatureImu(CaptureMotionPtr _cap_imu_ptr):
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>()),
@@ -22,7 +22,7 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
{
}
-FeatureIMU::~FeatureIMU()
+FeatureImu::~FeatureImu()
{
//
}
diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp
index 7e7082899ff4924749c4b402f163ac96afc2b688..03e3e232f989e0127f4d9769474c6c4bcd875be4 100644
--- a/src/yaml/processor_imu_yaml.cpp
+++ b/src/yaml/processor_imu_yaml.cpp
@@ -20,16 +20,16 @@ namespace wolf
namespace
{
-static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _filename_dot_yaml)
+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>() == "ProcessorIMU")
+ if (config["type"].as<std::string>() == "ProcessorImu")
{
YAML::Node kf_vote = config["keyframe_vote"];
- ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
+ ProcessorParamsImuPtr params = std::make_shared<ProcessorParamsImu>();
params->time_tolerance = config["time_tolerance"] .as<double>();
params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>();
@@ -47,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("ProcessorIMU", 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 575ec70054732295e3f6126d93918e11d406baf5..9268169c6c13835cce8f8f6d656da8910664103d 100644
--- a/src/yaml/sensor_imu_yaml.cpp
+++ b/src/yaml/sensor_imu_yaml.cpp
@@ -21,16 +21,16 @@ namespace wolf
namespace
{
-static ParamsSensorBasePtr createParamsSensorIMU(const std::string & _filename_dot_yaml)
+static ParamsSensorBasePtr createParamsSensorImu(const std::string & _filename_dot_yaml)
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["type"].as<std::string>() == "SensorIMU")
+ if (config["type"].as<std::string>() == "SensorImu")
{
YAML::Node variances = config["motion_variances"];
YAML::Node kf_vote = config["keyframe_vote"];
- ParamsSensorIMUPtr params = std::make_shared<ParamsSensorIMU>();
+ 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 ParamsSensorBasePtr createParamsSensorIMU(const std::string & _filename_d
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_imu_intr = ParamsSensorFactory::get().registerCreator("SensorIMU", createParamsSensorIMU);
+const bool WOLF_UNUSED registered_imu_intr = ParamsSensorFactory::get().registerCreator("SensorImu", createParamsSensorImu);
} // namespace [unnamed]
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index b1d7254b8ef39178d63f9e0217011dcf408a9fb2..f6f26a4aee404d7ea579beece22ebedcbea60112 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -8,7 +8,7 @@
#include <core/utils/utils_gtest.h>
#include <core/utils/logging.h>
-// IMU
+// Imu
#include "imu/internal/config.h"
#include "imu/capture/capture_imu.h"
#include "imu/processor/processor_imu.h"
@@ -30,22 +30,22 @@ using namespace std;
using namespace wolf;
/*
- * This test is designed to test IMU biases in a particular case : perfect IMU, not moving.
+ * This test is designed to test Imu biases in a particular case : perfect Imu, not moving.
* var(b1,b2,p2,v2,q2), inv(p1,q1,v1); fac1: imu+(b1=b2)
* So there is no odometry data.
- * IMU data file should first contain initial conditions, then the time_step between each new imu and the last one (in seconds) data,
- * and finally the last stateafter integration and the last timestamp, Then it should contain all IMU data and related timestamps
+ * Imu data file should first contain initial conditions, then the time_step between each new imu and the last one (in seconds) data,
+ * and finally the last stateafter integration and the last timestamp, Then it should contain all Imu data and related timestamps
*/
-class FactorIMU_biasTest_Static_NullBias : public testing::Test
+class FactorImu_biasTest_Static_NullBias : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr KF0;
FrameBasePtr KF1;
Eigen::Vector6d origin_bias;
@@ -72,11 +72,11 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
@@ -103,7 +103,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
Eigen::Vector6d data_imu;
data_imu << -wolf::gravity(), 0,0,0;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -126,15 +126,15 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
+class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr KF0;
FrameBasePtr KF1;
Eigen::Vector6d origin_bias;
@@ -160,11 +160,11 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -188,7 +188,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on Imu (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -210,15 +210,15 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
+class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -244,11 +244,11 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
// ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -272,7 +272,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on IMU (measure only gravity here)
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on Imu (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -294,15 +294,15 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
+class FactorImu_biasTest_Static_NonNullBias : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -328,11 +328,11 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -358,7 +358,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero()); //set data on IMU (measure only gravity here)
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero()); //set data on Imu (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -380,15 +380,15 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_biasTest_Move_NullBias : public testing::Test
+class FactorImu_biasTest_Move_NullBias : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -414,11 +414,11 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
@@ -445,7 +445,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -467,15 +467,15 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
+class FactorImu_biasTest_Move_NonNullBias : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -501,11 +501,11 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -528,7 +528,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -550,15 +550,15 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
+class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -584,11 +584,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -617,12 +617,12 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
//===================================================== END{INITIALIZATION}
//===================================================== PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
- //gravity measure depends on current IMU orientation + bias
- //use data_imu_initial to measure gravity from real orientation of IMU then add biases
+ //gravity measure depends on current Imu orientation + bias
+ //use data_imu_initial to measure gravity from real orientation of Imu then add biases
data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
t.set(t.get() + 0.001); //increment of 1 ms
imu_ptr->setData(data_imu);
@@ -643,15 +643,15 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
+class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -677,11 +677,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -716,12 +716,12 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
- //gravity measure depends on current IMU orientation + bias
- //use data_imu_initial to measure gravity from real orientation of IMU then add biases
+ //gravity measure depends on current Imu orientation + bias
+ //use data_imu_initial to measure gravity from real orientation of Imu then add biases
data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
t.set(t.get() + 0.001); //increment of 1 ms
imu_ptr->setData(data_imu);
@@ -744,15 +744,15 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
// var(b1,b2), inv(p1,q1,v1,p2,q2,v2); fac1: imu(p,q,v)+(b1=b2)
-class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
+class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
Eigen::Vector6d origin_bias;
@@ -778,11 +778,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
//===================================================== END{SETTING PROBLEM}
//===================================================== INITIALIZATION
@@ -806,11 +806,11 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
double dt(0.001);
TimeStamp ts(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
while( ts.get() < 1 )
{
- // PROCESS IMU DATA
+ // PROCESS Imu DATA
// Time and data variables
ts += dt;
@@ -841,21 +841,21 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
+class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
wolf::TimeStamp t;
ProblemPtr problem;
CeresManagerPtr ceres_manager;
SensorBasePtr sensor;
- SensorIMUPtr sensor_imu;
+ SensorImuPtr sensor_imu;
SensorOdom3dPtr sensor_odo;
ProcessorBasePtr processor;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
ProcessorOdom3dPtr processor_odo;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- CaptureIMUPtr capture_imu;
+ CaptureImuPtr capture_imu;
CaptureOdom3dPtr capture_odo;
Eigen::Vector6d origin_bias;
Eigen::VectorXd expected_final_state;
@@ -878,11 +878,11 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_options.minimizer_type = ceres::TRUST_REGION;
ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- sensor = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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("ProcessorImu", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
- processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
+ // SENSOR + PROCESSOR Imu
+ sensor = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
+ processor_imu = std::static_pointer_cast<ProcessorImu>(processor);
// SENSOR + PROCESSOR ODOM 3d
sensor = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
@@ -890,7 +890,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
sensor_odo->setNoiseStd((sensor_odo->getNoiseStd()/10.0).eval());
- WOLF_TRACE("IMU cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
+ WOLF_TRACE("Imu cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose());
ProcessorParamsOdom3dPtr prc_odom3d_params = std::make_shared<ProcessorParamsOdom3d>();
@@ -927,7 +927,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
TimeStamp t_imu(0.0), t_odo(0.0);
double dt_imu(0.001), dt_odo(.01);
- capture_imu = std::make_shared<CaptureIMU> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
+ capture_imu = std::make_shared<CaptureImu> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
capture_odo = std::make_shared<CaptureOdom3d>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
sensor_odo->process(capture_odo);
@@ -943,7 +943,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
for(unsigned int i = 1; i<=1000; i++)
{
- // PROCESS IMU DATA
+ // PROCESS Imu DATA
// Time and data variables
t_imu += dt_imu;
@@ -965,7 +965,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
+ //when we find a Imu timestamp corresponding with this odometry timestamp then we process odometry measurement
if(t_imu.get() >= t_odo.get())
{
WOLF_TRACE("====== create ODOM KF ========");
@@ -1025,16 +1025,16 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
+class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
CeresManager* ceres_manager_wolf_diff;
ProcessorBasePtr processor_;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
ProcessorOdom3dPtr processor_odom3d;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
@@ -1061,11 +1061,11 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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_);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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_);
// SENSOR + PROCESSOR ODOM 3d
SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
@@ -1106,7 +1106,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
double dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(0.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
wolf::CaptureOdom3dPtr mot_ptr = std::make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, sen_odom3d->getNoiseCov(), nullptr);
sen_odom3d->process(mot_ptr);
//first odometry data will be processed at this timestamp
@@ -1114,10 +1114,10 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
+ //when we find a Imu timestamp corresponding with this odometry timestamp then we process odometry measurement
for(unsigned int i = 1; i<=1000; i++)
{
- // PROCESS IMU DATA
+ // PROCESS Imu DATA
// Time and data variables
ts.set(i*dt);
data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
@@ -1166,16 +1166,16 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
virtual void TearDown(){}
};
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
+class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
{
public:
wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
+ SensorImuPtr sen_imu;
SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
CeresManagerPtr ceres_manager_wolf_diff;
ProcessorBasePtr processor;
- ProcessorIMUPtr processor_imu;
+ ProcessorImuPtr processor_imu;
ProcessorOdom3dPtr processor_odom3d;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
@@ -1203,11 +1203,11 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
ceres_options.max_num_iterations = 1e4;
ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("SensorIMU", "Main IMU", (Vector7d()<<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);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<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);
// SENSOR + PROCESSOR ODOM 3d
SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml");
@@ -1248,7 +1248,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
double dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(1.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
+ wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
wolf::CaptureOdom3dPtr mot_ptr = std::make_shared<CaptureOdom3d>(t, sen_odom3d, data_odom3d, nullptr);
sen_odom3d->process(mot_ptr);
//first odometry data will be processed at this timestamp
@@ -1257,10 +1257,10 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
Eigen::Vector2d randomPart;
data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
+ //when we find a Imu timestamp corresponding with this odometry timestamp then we process odometry measurement
for(unsigned int i = 1; i<=500; i++)
{
- // PROCESS IMU DATA
+ // PROCESS Imu DATA
// Time and data variables
ts.set(i*dt);
@@ -1296,7 +1296,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
for(unsigned int j = 1; j<=500; j++)
{
- // PROCESS IMU DATA
+ // PROCESS Imu DATA
// Time and data variables
ts.set((500 + j)*dt);
@@ -1345,7 +1345,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
// tests with following conditions :
// var(b1,b2), invar(p1,q1,v1,p2,q2,v2), factor : imu(p,q,v)
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
KF0->getP()->fix();
@@ -1372,7 +1372,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
}
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
KF0->getP()->fix();
@@ -1463,7 +1463,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
}
}
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
KF0->getP()->fix();
@@ -1488,7 +1488,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK
ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
}
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
KF0->getP()->fix();
@@ -1579,7 +1579,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
}
}
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1601,7 +1601,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
}
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1681,7 +1681,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
}
}
-TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1762,7 +1762,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
}
}
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1783,7 +1783,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
}
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1861,7 +1861,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
}
}
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1882,7 +1882,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
}
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1961,7 +1961,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
}
}
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -1982,7 +1982,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initO
}
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2061,7 +2061,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
}
}
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2082,7 +2082,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
}
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2161,7 +2161,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
}
}
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2182,7 +2182,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
}
-//TEST_F(FactorIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+//TEST_F(FactorImu_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
//{
// //prepare problem for solving
// origin_KF->getP()->fix();
@@ -2203,7 +2203,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
//
//}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2248,7 +2248,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2295,7 +2295,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2324,7 +2324,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2354,7 +2354,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2382,7 +2382,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2411,7 +2411,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2461,7 +2461,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
{
//Add fix factor on yaw to make the problem observable
Eigen::MatrixXd featureFix_cov(6,6);
@@ -2519,7 +2519,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
{
//Add fix factor on yaw to make the problem observable
Eigen::MatrixXd featureFix_cov(6,6);
@@ -2580,7 +2580,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2601,7 +2601,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
// WOLF_TRACE("real bias : ", origin_bias.transpose());
// WOLF_TRACE("origin bias : ", origin_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
// WOLF_TRACE("last bias : ", last_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
-// WOLF_TRACE("jacob bias : ", std::static_pointer_cast<CaptureIMU>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
+// WOLF_TRACE("jacob bias : ", std::static_pointer_cast<CaptureImu>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
std::cout << report << std::endl;
@@ -2631,7 +2631,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2677,7 +2677,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2704,7 +2704,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_in
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2733,7 +2733,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_in
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2760,7 +2760,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_in
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2789,7 +2789,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_in
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
+TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
{
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2844,10 +2844,10 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
+// ::testing::GTEST_FLAG(filter) = "FactorImu_biasTest_Move_NonNullBiasRot.*:FactorImu_biasTest_Static_NullBias.*:FactorImu_biasTest_Static_NonNullAccBias.*:FactorImu_biasTest_Static_NonNullGyroBias.*";
+// ::testing::GTEST_FLAG(filter) = "FactorImu_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
+// ::testing::GTEST_FLAG(filter) = "FactorImu_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
+// ::testing::GTEST_FLAG(filter) = "FactorImu_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp
index 41a5599ec3032fda5ee408368afd9c7f0541f7d4..6e801064b2d91fccd011a9637a01ee15b6185856 100644
--- a/test/gtest_feature_imu.cpp
+++ b/test/gtest_feature_imu.cpp
@@ -9,17 +9,17 @@
#include <core/utils/utils_gtest.h>
#include "core/utils/logging.h"
-class FeatureIMU_test : public testing::Test
+class FeatureImu_test : public testing::Test
{
public: //These can be accessed in fixtures
wolf::ProblemPtr problem;
wolf::TimeStamp ts;
- wolf::CaptureIMUPtr imu_ptr;
+ wolf::CaptureImuPtr imu_ptr;
Eigen::VectorXd state_vec;
Eigen::VectorXd delta_preint;
Eigen::Matrix<double,9,9> delta_preint_cov;
- std::shared_ptr<wolf::FeatureIMU> feat_imu;
+ std::shared_ptr<wolf::FeatureImu> feat_imu;
wolf::FrameBasePtr last_frame;
wolf::FrameBasePtr origin_frame;
Eigen::MatrixXd dD_db_jacobians;
@@ -36,16 +36,16 @@ class FeatureIMU_test : public testing::Test
// Wolf problem
problem = Problem::create("POV", 3);
- Eigen::VectorXd IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- 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>();
+ Eigen::VectorXd Imu_extrinsics(7);
+ Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
+ 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;
prc_imu_params->max_buff_length = 10;
prc_imu_params->dist_traveled = 5;
prc_imu_params->angle_turned = 0.5;
- processor_ptr_ = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
+ processor_ptr_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr_);
// Time and data variables
@@ -60,10 +60,10 @@ class FeatureIMU_test : public testing::Test
MatrixXd P0; P0.setIdentity(9,9);
origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
- // Emplace one capture to store the IMU data arriving from (sensor / callback / file / etc.)
+ // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- imu_ptr = std::static_pointer_cast<CaptureIMU>(
- CaptureBase::emplace<CaptureIMU>(origin_frame,
+ imu_ptr = std::static_pointer_cast<CaptureImu>(
+ CaptureBase::emplace<CaptureImu>(origin_frame,
t,
sensor_ptr,
data_,
@@ -90,8 +90,8 @@ class FeatureIMU_test : public testing::Test
delta_preint_cov = processor_motion_ptr_->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
VectorXd calib_preint = processor_motion_ptr_->getLast()->getCalibrationPreint();
dD_db_jacobians = processor_motion_ptr_->getMotion().jacobian_calib_;
- feat_imu = std::static_pointer_cast<FeatureIMU>(
- FeatureBase::emplace<FeatureIMU>(imu_ptr,
+ feat_imu = std::static_pointer_cast<FeatureImu>(
+ FeatureBase::emplace<FeatureImu>(imu_ptr,
delta_preint,
delta_preint_cov,
calib_preint,
@@ -113,7 +113,7 @@ class FeatureIMU_test : public testing::Test
}
};
-TEST_F(FeatureIMU_test, check_frame)
+TEST_F(FeatureImu_test, check_frame)
{
// set variables
using namespace wolf;
@@ -144,7 +144,7 @@ TEST_F(FeatureIMU_test, check_frame)
ASSERT_EQ(origin_frame->id(), left_frame->id());
}
-TEST_F(FeatureIMU_test, access_members)
+TEST_F(FeatureImu_test, access_members)
{
using namespace wolf;
@@ -154,13 +154,13 @@ TEST_F(FeatureIMU_test, access_members)
ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
}
-//TEST_F(FeatureIMU_test, addFactor)
+//TEST_F(FeatureImu_test, addFactor)
//{
// using namespace wolf;
//
// FrameBasePtr frm_imu = std::static_pointer_cast<FrameBase>(last_frame);
// auto cap_imu = last_frame->getCaptureList().back();
-// FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, std::static_pointer_cast<CaptureIMU>(cap_imu), processor_ptr_);
+// FactorImuPtr factor_imu = std::make_shared<FactorImu>(feat_imu, std::static_pointer_cast<CaptureImu>(cap_imu), processor_ptr_);
// feat_imu->addFactor(factor_imu);
// origin_frame->addConstrainedBy(factor_imu);
//}
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 16b7d30838c92aa5ac3a490f673c924a54478e44..37bef1d89728b9590f9db3b3b4574d1d60889fd7 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -1,5 +1,5 @@
/*
- * gtest_IMU.cpp
+ * gtest_Imu.cpp
*
* Created on: Nov 14, 2017
* Author: jsola
@@ -27,15 +27,15 @@ using std::make_shared;
using std::static_pointer_cast;
using std::string;
-class Process_Factor_IMU : public testing::Test
+class Process_Factor_Imu : public testing::Test
{
public:
// Wolf objects
ProblemPtr problem;
CeresManagerPtr ceres_manager;
- SensorIMUPtr sensor_imu;
- ProcessorIMUPtr processor_imu;
- CaptureIMUPtr capture_imu;
+ SensorImuPtr sensor_imu;
+ ProcessorImuPtr processor_imu;
+ CaptureImuPtr capture_imu;
FrameBasePtr KF_0, KF_1; // keyframes
CaptureBasePtr C_0 , C_1; // base captures
CaptureMotionPtr CM_0, CM_1; // motion captures
@@ -56,9 +56,9 @@ class Process_Factor_IMU : public testing::Test
Vector6d bias_0, bias_1; // optimized bias at KF's 0 and 1
// input
- Matrix<double, 6, Dynamic> motion; // Motion in IMU frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
- Matrix<double, 3, Dynamic> a, w; // True acc and angvel in IMU frame. Each column is a motion step. Used to create motion with `motion << a,w;`
- Vector6d data; // IMU data. It's the motion with gravity and bias. See imu::motion2data().
+ Matrix<double, 6, Dynamic> motion; // Motion in Imu frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
+ Matrix<double, 3, Dynamic> a, w; // True acc and angvel in Imu frame. Each column is a motion step. Used to create motion with `motion << a,w;`
+ Vector6d data; // Imu data. It's the motion with gravity and bias. See imu::motion2data().
// Deltas and states (exact, integrated, corrected, solved, etc)
VectorXd D_exact, x1_exact; // exact or ground truth
@@ -96,11 +96,11 @@ class Process_Factor_IMU : public testing::Test
ceres::Solver::Options ceres_options;
ceres_manager = make_shared<CeresManager>(problem, ceres_options);
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sensor = problem->installSensor ("SensorIMU", "Main IMU", (Vector7d()<<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);
+ // SENSOR + PROCESSOR Imu
+ SensorBasePtr sensor = problem->installSensor ("SensorImu", "Main Imu", (Vector7d()<<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);
dt = 0.01;
processor_imu->setTimeTolerance(dt/2);
@@ -120,7 +120,7 @@ class Process_Factor_IMU : public testing::Test
* Input:
* q: current orientation
* motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
+ * bias_real: the real bias of the Imu
* bias_preint: the bias used for Delta pre-integration
* Input/output
* Delta: the preintegrated delta
@@ -158,7 +158,7 @@ class Process_Factor_IMU : public testing::Test
* N: number of steps
* q0: initial orientation
* motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
+ * bias_real: the real bias of the Imu
* bias_preint: the bias used for Delta pre-integration
* Output:
* return: the preintegrated delta
@@ -180,7 +180,7 @@ class Process_Factor_IMU : public testing::Test
* N: number of steps
* q0: initial orientation
* motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
+ * bias_real: the real bias of the Imu
* bias_preint: the bias used for Delta pre-integration
* Output:
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -205,7 +205,7 @@ class Process_Factor_IMU : public testing::Test
* Input:
* q0: initial orientation
* motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
- * bias_real: the real bias of the IMU
+ * bias_real: the real bias of the Imu
* bias_preint: the bias used for Delta pre-integration
* Output:
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -230,7 +230,7 @@ class Process_Factor_IMU : public testing::Test
* Input:
* q0: initial orientation
* motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
- * bias_real: the real bias of the IMU
+ * bias_real: the real bias of the Imu
* bias_preint: the bias used for Delta pre-integration
* Output:
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -261,7 +261,7 @@ class Process_Factor_IMU : public testing::Test
{
Vector6d motion_now;
data = imu::motion2data(motion.col(0), q0, bias_real);
- capture_imu = make_shared<CaptureIMU>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
+ capture_imu = make_shared<CaptureImu>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
q = q0;
t = t0;
for (int i= 0; i < N; i++)
@@ -324,14 +324,14 @@ class Process_Factor_IMU : public testing::Test
// Integrate using all methods
virtual void integrateAll()
{
- // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
+ // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
if (motion.cols() == 1)
D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
else
D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
x1_exact = imu::composeOverState(x0, D_exact, DT );
- // ===================================== INTEGRATE USING IMU_TOOLS
+ // ===================================== INTEGRATE USING Imu_TOOLS
// pre-integrate
if (motion.cols() == 1)
D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias);
@@ -346,7 +346,7 @@ class Process_Factor_IMU : public testing::Test
x1_preint_imu = imu::composeOverState(x0, D_preint_imu , DT );
x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
- // ===================================== INTEGRATE USING PROCESSOR_IMU
+ // ===================================== INTEGRATE USING PROCESSOR_Imu
integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
@@ -362,7 +362,7 @@ class Process_Factor_IMU : public testing::Test
Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
- // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
+ // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
// Build exact trajectories
@@ -380,7 +380,7 @@ class Process_Factor_IMU : public testing::Test
D_exact = Trj_D_exact.col(cols-1);
x1_exact = Trj_x_exact.col(cols-1);
- // ===================================== INTEGRATE USING IMU_TOOLS
+ // ===================================== INTEGRATE USING Imu_TOOLS
// pre-integrate
MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);
@@ -411,11 +411,11 @@ class Process_Factor_IMU : public testing::Test
x1_preint_imu = imu::composeOverState(x0, D_preint_imu , DT );
x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
- // ===================================== INTEGRATE USING PROCESSOR_IMU
+ // ===================================== INTEGRATE USING PROCESSOR_Imu
MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
- // Build trajectories preintegrated and corrected with processorIMU
+ // Build trajectories preintegrated and corrected with processorImu
Dt = 0;
col = 0;
Buf_Jac_preint_prc.clear();
@@ -485,16 +485,16 @@ class Process_Factor_IMU : public testing::Test
// ===================================== SET KF in Wolf tree
FrameBasePtr KF = problem->emplaceFrame(KEY, x1_exact, t);
- // ===================================== IMU CALLBACK
+ // ===================================== Imu CALLBACK
problem->keyFrameCallback(KF, nullptr, dt/2);
- // Process IMU for the callback to take effect
+ // Process Imu for the callback to take effect
data = Vector6d::Zero();
- capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
+ capture_imu = make_shared<CaptureImu>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
sensor_imu->process(capture_imu);
KF_1 = problem->getLastKeyFrame();
- C_1 = KF_1->getCaptureList().front(); // front is IMU
+ C_1 = KF_1->getCaptureList().front(); // front is Imu
CM_1 = static_pointer_cast<CaptureMotion>(C_1);
// ===================================== SET BOUNDARY CONDITIONS
@@ -586,7 +586,7 @@ class Process_Factor_IMU : public testing::Test
};
-class Process_Factor_IMU_ODO : public Process_Factor_IMU
+class Process_Factor_Imu_ODO : public Process_Factor_Imu
{
public:
// Wolf objects
@@ -597,8 +597,8 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
virtual void SetUp( ) override
{
- // ===================================== IMU
- Process_Factor_IMU::SetUp();
+ // ===================================== Imu
+ Process_Factor_Imu::SetUp();
// ===================================== ODO
string wolf_root = _WOLF_IMU_ROOT_DIR;
@@ -615,8 +615,8 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
virtual void integrateAll() override
{
- // ===================================== IMU
- Process_Factor_IMU::integrateAll();
+ // ===================================== Imu
+ Process_Factor_Imu::integrateAll();
// ===================================== ODO
Vector6d data;
@@ -633,8 +633,8 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
virtual void integrateAllTrajectories() override
{
- // ===================================== IMU
- Process_Factor_IMU::integrateAllTrajectories();
+ // ===================================== Imu
+ Process_Factor_Imu::integrateAllTrajectories();
// ===================================== ODO
Vector6d data;
@@ -651,8 +651,8 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
virtual void buildProblem() override
{
- // ===================================== IMU
- Process_Factor_IMU::buildProblem();
+ // ===================================== Imu
+ Process_Factor_Imu::buildProblem();
// ===================================== ODO
// Process ODO for the callback to take effect
@@ -664,7 +664,7 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
};
-TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -708,7 +708,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, test_capture) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -754,7 +754,7 @@ TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 );
}
-TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -798,7 +798,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -842,7 +842,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -886,7 +886,7 @@ TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -930,7 +930,7 @@ TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -974,7 +974,7 @@ TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1018,7 +1018,7 @@ TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed__
}
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1062,7 +1062,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___
}
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1106,7 +1106,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___
}
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1150,7 +1150,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___
}
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1194,7 +1194,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___
}
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1238,7 +1238,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___
}
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1282,7 +1282,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimat
}
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1326,7 +1326,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimat
}
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1370,7 +1370,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1414,7 +1414,7 @@ TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
}
-TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1502,28 +1502,28 @@ TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
+ // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.*";
+ // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
+ // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
return RUN_ALL_TESTS();
}
/* Some notes :
*
- * - Process_Factor_IMU_ODO.MotionConstant_PQv_b__PQv_b :
+ * - Process_Factor_Imu_ODO.MotionConstant_PQv_b__PQv_b :
* this test will not work + jacobian is rank deficient because of estimating both initial
* and final velocities.
- * IMU data integration is done with correct biases (so this is the case of a calibrated IMU). Before solving the problem, we perturbate the initial bias.
+ * Imu data integration is done with correct biases (so this is the case of a calibrated Imu). Before solving the problem, we perturbate the initial bias.
* We solve the problem by fixing all states excepted V1 and V2. while creating the factors, both velocities are corrected using the difference between the actual
* bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the
* difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this
* solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
*
* - Bias evaluation with a precision of 1e-4 :
- * The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated IMU
+ * The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated Imu
* Because of cross relations between acc and gyro biases (respectively a_b and w_b) it is difficult to expect a better estimation.
* A small change in a_b can be cancelled by a small variation in w_b. in other words : there are local minima.
- * In addition, for Process_Factor_IMU tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
+ * In addition, for Process_Factor_Imu tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
* Errors tend to be distributed in different estimated variable when we get into a local minima (to minimize residuals in a least square sense).
*/
diff --git a/test/gtest_imu_tools.cpp b/test/gtest_imu_tools.cpp
index da88317f0e3dae17d26ee59670f2d25e8bc65518..c2e8306ce5b7d0c6cacafc1ac30f8be0474cf0e9 100644
--- a/test/gtest_imu_tools.cpp
+++ b/test/gtest_imu_tools.cpp
@@ -12,7 +12,7 @@ using namespace Eigen;
using namespace wolf;
using namespace imu;
-TEST(IMU_tools, identity)
+TEST(Imu_tools, identity)
{
VectorXd id_true;
id_true.setZero(10);
@@ -22,7 +22,7 @@ TEST(IMU_tools, identity)
ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
}
-TEST(IMU_tools, identity_split)
+TEST(Imu_tools, identity_split)
{
VectorXd p(3), qv(4), v(3);
Quaterniond q;
@@ -38,7 +38,7 @@ TEST(IMU_tools, identity_split)
ASSERT_MATRIX_APPROX(v , Vector3d::Zero(), 1e-10);
}
-TEST(IMU_tools, inverse)
+TEST(Imu_tools, inverse)
{
VectorXd d(10), id(10), iid(10), iiid(10), I(10);
Vector4d qv;
@@ -60,7 +60,7 @@ TEST(IMU_tools, inverse)
ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
}
-TEST(IMU_tools, compose_between)
+TEST(Imu_tools, compose_between)
{
VectorXd dx1(10), dx2(10), dx3(10);
Matrix<double, 10, 1> d1, d2, d3;
@@ -94,7 +94,7 @@ TEST(IMU_tools, compose_between)
ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10);
}
-TEST(IMU_tools, compose_between_with_state)
+TEST(Imu_tools, compose_between_with_state)
{
VectorXd x(10), d(10), x2(10), x3(10), d2(10), d3(10);
Vector4d qv;
@@ -123,12 +123,12 @@ TEST(IMU_tools, compose_between_with_state)
ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
}
-TEST(IMU_tools, lift_retract)
+TEST(Imu_tools, lift_retract)
{
VectorXd d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
// transform to delta
- VectorXd delta = exp_IMU(d_min);
+ VectorXd delta = exp_Imu(d_min);
// expected delta
Vector3d dp = d_min.head(3);
@@ -138,15 +138,15 @@ TEST(IMU_tools, lift_retract)
ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
// transform to a new tangent -- should be the original tangent
- VectorXd d_from_delta = log_IMU(delta);
+ VectorXd d_from_delta = log_Imu(delta);
ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
// transform to a new delta -- should be the previous delta
- VectorXd delta_from_d = exp_IMU(d_from_delta);
+ VectorXd delta_from_d = exp_Imu(d_from_delta);
ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
}
-TEST(IMU_tools, plus)
+TEST(Imu_tools, plus)
{
VectorXd d1(10), d2(10), d3(10);
VectorXd err(9);
@@ -162,7 +162,7 @@ TEST(IMU_tools, plus)
ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(9), 1e-10);
}
-TEST(IMU_tools, diff)
+TEST(Imu_tools, diff)
{
VectorXd d1(10), d2(10);
Vector4d qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
@@ -184,7 +184,7 @@ TEST(IMU_tools, diff)
}
-TEST(IMU_tools, compose_jacobians)
+TEST(Imu_tools, compose_jacobians)
{
VectorXd d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
VectorXd t1(9), t2(9); // tangents
@@ -224,7 +224,7 @@ TEST(IMU_tools, compose_jacobians)
ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
}
-TEST(IMU_tools, diff_jacobians)
+TEST(Imu_tools, diff_jacobians)
{
VectorXd d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
VectorXd t1(9), t2(9); // tangents
@@ -263,7 +263,7 @@ TEST(IMU_tools, diff_jacobians)
ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
}
-TEST(IMU_tools, body2delta_jacobians)
+TEST(Imu_tools, body2delta_jacobians)
{
VectorXd delta(10), delta_pert(10); // deltas
VectorXd body(6), pert(6);
@@ -393,7 +393,7 @@ TEST(motion2data, AllRandom)
* N: number of steps
* q0: initial orientaiton
* motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- * bias_real: the real bias of the IMU
+ * bias_real: the real bias of the Imu
* bias_preint: the bias used for Delta pre-integration
* Output:
* return: the preintegrated delta
@@ -423,7 +423,7 @@ VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, co
* N: number of steps
* q0: initial orientaiton
* motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- * bias_real: the real bias of the IMU
+ * bias_real: the real bias of the Imu
* bias_preint: the bias used for Delta pre-integration
* Output:
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
@@ -448,12 +448,12 @@ VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, co
data = motion2data(motion, q, bias_real);
q = q*exp_q(motion.tail(3)*dt);
// Motion::integrateOneStep()
- { // IMU::computeCurrentDelta
+ { // Imu::computeCurrentDelta
body = data - bias_preint;
body2delta(body, dt, delta, J_d_d);
J_d_b = - J_d_d;
}
- { // IMU::deltaPlusDelta
+ { // Imu::deltaPlusDelta
compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
}
// Motion:: jac calib
@@ -464,7 +464,7 @@ VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, co
return Delta;
}
-TEST(IMU_tools, integral_jacobian_bias)
+TEST(Imu_tools, integral_jacobian_bias)
{
VectorXd Delta(10), Delta_pert(10); // deltas
VectorXd bias_real(6), pert(6), bias_pert(6), bias_preint(6);
@@ -499,7 +499,7 @@ TEST(IMU_tools, integral_jacobian_bias)
ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
}
-TEST(IMU_tools, delta_correction)
+TEST(Imu_tools, delta_correction)
{
VectorXd Delta(10), Delta_preint(10), Delta_corr; // deltas
VectorXd bias(6), pert(6), bias_preint(6);
@@ -536,7 +536,7 @@ TEST(IMU_tools, delta_correction)
ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
}
-TEST(IMU_tools, full_delta_residual)
+TEST(Imu_tools, full_delta_residual)
{
VectorXd x1(10), x2(10);
VectorXd Delta(10), Delta_preint(10), Delta_corr(10);
@@ -623,7 +623,7 @@ TEST(IMU_tools, full_delta_residual)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
+// ::testing::GTEST_FLAG(filter) = "Imu_tools.delta_correction";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp
index 49e4cfd46dec4a01b3db2d4d3d03b40cc38299ab..90786bb6472b4ea5e5f085e05413cd3657dc1c72 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -24,7 +24,7 @@
using namespace Eigen;
-class ProcessorIMUt : public testing::Test
+class ProcessorImut : public testing::Test
{
public: //These can be accessed in fixtures
@@ -37,7 +37,7 @@ class ProcessorIMUt : public testing::Test
Vector6d data;
Matrix6d data_cov;
VectorXd x0;
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
+ std::shared_ptr<wolf::CaptureImu> cap_imu_ptr;
//a new of this will be created for each new test
virtual void SetUp()
@@ -54,8 +54,8 @@ class ProcessorIMUt : public testing::Test
// Wolf problem
problem = Problem::create("POV", 3);
Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished();
- 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");
+ 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");
processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
@@ -65,8 +65,8 @@ class ProcessorIMUt : public testing::Test
// Set the origin
x0.resize(10);
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
+ // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
+ cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
}
virtual void TearDown()
@@ -83,18 +83,18 @@ class ProcessorIMUt : public testing::Test
}
};
-TEST(ProcessorIMU_constructors, ALL)
+TEST(ProcessorImu_constructors, ALL)
{
using namespace wolf;
- //constructor with ProcessorIMUParamsPtr argument only
- ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
+ //constructor with ProcessorImuParamsPtr argument only
+ ProcessorParamsImuPtr param_ptr = std::make_shared<ProcessorParamsImu>();
param_ptr->max_time_span = 2.0;
param_ptr->max_buff_length = 20000;
param_ptr->dist_traveled = 2.0;
param_ptr->angle_turned = 2.0;
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
+ ProcessorImuPtr prc1 = std::make_shared<ProcessorImu>(param_ptr);
ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
@@ -104,23 +104,23 @@ TEST(ProcessorIMU_constructors, ALL)
std::string wolf_root = _WOLF_IMU_ROOT_DIR;
ProblemPtr problem = Problem::create("POV", 3);
Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
- 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("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);
+ 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("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("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);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
+ 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);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(), 0.2);
}
-TEST(ProcessorIMU, voteForKeyFrame)
+TEST(ProcessorImu, voteForKeyFrame)
{
using namespace wolf;
using namespace Eigen;
@@ -134,14 +134,14 @@ TEST(ProcessorIMU, voteForKeyFrame)
// Wolf problem
ProblemPtr problem = Problem::create("POV", 3);
Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
- ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
+ 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("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
+ ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
//setting origin
VectorXd x0(10);
@@ -156,17 +156,17 @@ TEST(ProcessorIMU, voteForKeyFrame)
Matrix6d data_cov(Matrix6d::Identity());
data_cov(0,0) = 0.5;
- // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
+ // Create the captureImu to store the Imu data arriving from (sensor / callback / file / etc.)
+ std::shared_ptr<wolf::CaptureImu> cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
// Time
// we want more than one data to integrate otherwise covariance will be 0
- double dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
+ double dt = std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan() - 0.1;
t.set(dt);
cap_imu_ptr->setTimeStamp(t);
sensor_ptr->process(cap_imu_ptr);
- dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
+ dt = std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan() + 0.1;
t.set(dt);
cap_imu_ptr->setTimeStamp(t);
sensor_ptr->process(cap_imu_ptr);
@@ -204,7 +204,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
}
-TEST_F(ProcessorIMUt, acc_x)
+TEST_F(ProcessorImut, acc_x)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
@@ -227,7 +227,7 @@ TEST_F(ProcessorIMUt, acc_x)
ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
}
-TEST_F(ProcessorIMUt, acc_y)
+TEST_F(ProcessorImut, acc_y)
{
// last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
// difference hier is that we integrate over 1ms periods
@@ -267,7 +267,7 @@ TEST_F(ProcessorIMUt, acc_y)
ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, acc_z)
+TEST_F(ProcessorImut, acc_z)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
@@ -304,7 +304,7 @@ TEST_F(ProcessorIMUt, acc_z)
ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, check_Covariance)
+TEST_F(ProcessorImut, check_Covariance)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
@@ -324,7 +324,7 @@ TEST_F(ProcessorIMUt, check_Covariance)
// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
}
-TEST_F(ProcessorIMUt, gyro_x)
+TEST_F(ProcessorImut, gyro_x)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -367,7 +367,7 @@ TEST_F(ProcessorIMUt, gyro_x)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
+TEST_F(ProcessorImut, gyro_x_biasedAbx)
{
// time
double dt(0.001);
@@ -428,7 +428,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
}
-TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
+TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -481,7 +481,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
// "\n expected state is : \n" << x.transpose() << std::endl;
}
-TEST_F(ProcessorIMUt, gyro_z)
+TEST_F(ProcessorImut, gyro_z)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -520,7 +520,7 @@ TEST_F(ProcessorIMUt, gyro_z)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_xyz)
+TEST_F(ProcessorImut, gyro_xyz)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
@@ -565,7 +565,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
+ // use processorImu
Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
data.tail(3) = tmp_vec;
@@ -578,7 +578,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
/* We focus on orientation here. position is supposed not to have moved
* we integrated using 2 ways :
- a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
+ - using methods implemented in processorImu
We check one against the other
*/
@@ -606,7 +606,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
+TEST_F(ProcessorImut, gyro_z_ConstVelocity)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -645,7 +645,7 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
+TEST_F(ProcessorImut, gyro_x_ConstVelocity)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -689,7 +689,7 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
+TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -733,7 +733,7 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
+TEST_F(ProcessorImut, gyro_y_ConstVelocity)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -777,7 +777,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
+TEST_F(ProcessorImut, gyro_xyz_ConstVelocity)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
@@ -822,7 +822,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
+ // use processorImu
Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
data.tail(3) = tmp_vec;
@@ -835,7 +835,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
/* We focus on orientation here. position is supposed not to have moved
* we integrated using 2 ways :
- a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
+ - using methods implemented in processorImu
We check one against the other
*/
@@ -865,7 +865,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
}
-TEST_F(ProcessorIMUt, gyro_x_acc_x)
+TEST_F(ProcessorImut, gyro_x_acc_x)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -914,7 +914,7 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_y_acc_y)
+TEST_F(ProcessorImut, gyro_y_acc_y)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -963,7 +963,7 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
}
-TEST_F(ProcessorIMUt, gyro_z_acc_z)
+TEST_F(ProcessorImut, gyro_z_acc_z)
{
double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
@@ -1014,7 +1014,7 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
+ //::testing::GTEST_FLAG(filter) = "ProcessorImut.check_Covariance";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_imu_jacobians.cpp b/test/gtest_processor_imu_jacobians.cpp
index 66fb7ae9f802477522622439e657ba7e549833bd..e9fd58e521a716cd783a93f0994023068551f201 100644
--- a/test/gtest_processor_imu_jacobians.cpp
+++ b/test/gtest_processor_imu_jacobians.cpp
@@ -28,7 +28,7 @@
using namespace wolf;
// A new one of these is created for each test
-class ProcessorIMU_jacobians : public testing::Test
+class ProcessorImu_jacobians : public testing::Test
{
public:
TimeStamp t;
@@ -38,16 +38,16 @@ class ProcessorIMU_jacobians : public testing::Test
double dt;
Eigen::Matrix<double,9,1> Delta_noise;
Eigen::Matrix<double,9,1> delta_noise;
- struct IMU_jac_bias bias_jac;
- struct IMU_jac_deltas deltas_jac;
+ struct Imu_jac_bias bias_jac;
+ struct Imu_jac_deltas deltas_jac;
- void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
+ void remapJacDeltas_quat0(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
}
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
+ void remapJacDeltas_quat(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
assert(place < _jac_delta.Delta_noisy_vect_.size());
new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
@@ -62,10 +62,10 @@ class ProcessorIMU_jacobians : public testing::Test
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
- Eigen::VectorXd IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+ Eigen::VectorXd Imu_extrinsics(7);
+ Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
- ProcessorIMU_UnitTester processor_imu;
+ ProcessorImu_UnitTester processor_imu;
ddelta_bias = 0.00000001;
dt = 0.001;
@@ -86,13 +86,13 @@ class ProcessorIMU_jacobians : public testing::Test
//std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
//get data to compute jacobians
- struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
+ struct Imu_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
bias_jac.copyfrom(bias_jac_c);
Delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
- struct IMU_jac_deltas deltas_jac_c = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
+ struct Imu_jac_deltas deltas_jac_c = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
deltas_jac = deltas_jac_c;
}
@@ -110,7 +110,7 @@ class ProcessorIMU_jacobians : public testing::Test
}
};
-class ProcessorIMU_jacobians_Dq : public testing::Test
+class ProcessorImu_jacobians_Dq : public testing::Test
{
public:
TimeStamp t;
@@ -118,15 +118,15 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
Eigen::Matrix<double,10,1> Delta0;
double ddelta_bias2;
double dt;
- struct IMU_jac_bias bias_jac2;
+ struct Imu_jac_bias bias_jac2;
- void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
+ void remapJacDeltas_quat0(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
}
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
+ void remapJacDeltas_quat(Imu_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
assert(place < _jac_delta.Delta_noisy_vect_.size());
new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
@@ -141,10 +141,10 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
- Eigen::VectorXd IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+ Eigen::VectorXd Imu_extrinsics(7);
+ Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
- ProcessorIMU_UnitTester processor_imu;
+ ProcessorImu_UnitTester processor_imu;
ddelta_bias2 = 0.0001;
dt = 0.001;
@@ -165,7 +165,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
//std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
//get data to compute jacobians
- struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
+ struct Imu_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
bias_jac2.copyfrom(bias_jac_c);
}
@@ -185,7 +185,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
///BIAS TESTS
-/* IMU_jac_deltas struct form :
+/* Imu_jac_deltas struct form :
contains vectors of size 7 :
Elements at place 0 are those not affected by the bias noise that we add (da_bx,..., dw_bx,... ).
place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
@@ -215,7 +215,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt
*/
-TEST_F(ProcessorIMU_jacobians, dDp_dab)
+TEST_F(ProcessorImu_jacobians, dDp_dab)
{
using namespace wolf;
Eigen::Matrix3d dDp_dab;
@@ -227,7 +227,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dab)
"\ndDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDv_dab)
+TEST_F(ProcessorImu_jacobians, dDv_dab)
{
using namespace wolf;
Eigen::Matrix3d dDv_dab;
@@ -239,7 +239,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dab)
"\ndDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDp_dwb)
+TEST_F(ProcessorImu_jacobians, dDp_dwb)
{
using namespace wolf;
Eigen::Matrix3d dDp_dwb;
@@ -251,7 +251,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dwb)
"\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
+TEST_F(ProcessorImu_jacobians, dDv_dwb_)
{
using namespace wolf;
Eigen::Matrix3d dDv_dwb;
@@ -263,7 +263,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
"\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDq_dab)
+TEST_F(ProcessorImu_jacobians, dDq_dab)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -278,7 +278,7 @@ TEST_F(ProcessorIMU_jacobians, dDq_dab)
ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
+TEST_F(ProcessorImu_jacobians, dDq_dwb_noise_1Em8_)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -300,7 +300,7 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
<< std::endl;
}
-TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
+TEST_F(ProcessorImu_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -394,7 +394,7 @@ TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
*/
-TEST_F(ProcessorIMU_jacobians, dDp_dP)
+TEST_F(ProcessorImu_jacobians, dDp_dP)
{
using namespace wolf;
Eigen::Matrix3d dDp_dP;
@@ -407,7 +407,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dP)
"\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDp_dV)
+TEST_F(ProcessorImu_jacobians, dDp_dV)
{
using namespace wolf;
Eigen::Matrix3d dDp_dV;
@@ -420,7 +420,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dV)
"\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDp_dO)
+TEST_F(ProcessorImu_jacobians, dDp_dO)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -437,7 +437,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dO)
"\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDv_dV)
+TEST_F(ProcessorImu_jacobians, dDv_dV)
{
using namespace wolf;
Eigen::Matrix3d dDv_dV;
@@ -450,7 +450,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dV)
"\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDv_dO)
+TEST_F(ProcessorImu_jacobians, dDv_dO)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -469,7 +469,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dO)
//dDo_dP = dDo_dV = [0, 0, 0]
-TEST_F(ProcessorIMU_jacobians, dDo_dO)
+TEST_F(ProcessorImu_jacobians, dDo_dO)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -486,7 +486,7 @@ TEST_F(ProcessorIMU_jacobians, dDo_dO)
"\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDp_dp)
+TEST_F(ProcessorImu_jacobians, dDp_dp)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -504,7 +504,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dp)
//dDv_dp = [0, 0, 0]
-TEST_F(ProcessorIMU_jacobians, dDv_dv)
+TEST_F(ProcessorImu_jacobians, dDv_dv)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -522,7 +522,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dv)
//dDo_dp = dDo_dv = [0, 0, 0]
-TEST_F(ProcessorIMU_jacobians, dDo_do)
+TEST_F(ProcessorImu_jacobians, dDo_do)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
diff --git a/test/processor_imu_UnitTester.cpp b/test/processor_imu_UnitTester.cpp
index 46881c51161d93cf31ad5907ce3b427438bc7c36..7da3afb9f5c98329ad469d564691adad9ab40dba 100644
--- a/test/processor_imu_UnitTester.cpp
+++ b/test/processor_imu_UnitTester.cpp
@@ -2,12 +2,12 @@
namespace wolf {
-ProcessorIMU_UnitTester::ProcessorIMU_UnitTester() :
- ProcessorIMU(std::make_shared<ProcessorParamsIMU>()),
+ProcessorImu_UnitTester::ProcessorImu_UnitTester() :
+ ProcessorImu(std::make_shared<ProcessorParamsImu>()),
Dq_out_(nullptr)
{}
-ProcessorIMU_UnitTester::~ProcessorIMU_UnitTester(){}
+ProcessorImu_UnitTester::~ProcessorImu_UnitTester(){}
} // namespace wolf
diff --git a/test/processor_imu_UnitTester.h b/test/processor_imu_UnitTester.h
index fb4de710d0b49e14012b5cdca61af01df5dcd9c2..2e9bbdd3a61a6619f7f84b47e202a4da848ff459 100644
--- a/test/processor_imu_UnitTester.h
+++ b/test/processor_imu_UnitTester.h
@@ -7,9 +7,9 @@
#include "core/processor/processor_motion.h"
namespace wolf {
- struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
+ struct Imu_jac_bias{ //struct used for checking jacobians by finite difference
- IMU_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
+ Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
Eigen::VectorXd _Delta0 ,
Eigen::Matrix3d _dDp_dab,
Eigen::Matrix3d _dDv_dab,
@@ -27,7 +27,7 @@ namespace wolf {
//
}
- IMU_jac_bias(){
+ Imu_jac_bias(){
for (int i=0; i<6; i++){
Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
@@ -41,7 +41,7 @@ namespace wolf {
dDq_dwb_ = Eigen::Matrix3d::Zero();
}
- IMU_jac_bias(IMU_jac_bias const & toCopy){
+ Imu_jac_bias(Imu_jac_bias const & toCopy){
Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
Delta0_ = toCopy.Delta0_;
@@ -66,7 +66,7 @@ namespace wolf {
Eigen::Matrix3d dDq_dwb_;
public:
- void copyfrom(IMU_jac_bias const& right){
+ void copyfrom(Imu_jac_bias const& right){
Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
Delta0_ = right.Delta0_;
@@ -78,9 +78,9 @@ namespace wolf {
}
};
- struct IMU_jac_deltas{
+ struct Imu_jac_deltas{
- IMU_jac_deltas(Eigen::VectorXd _Delta0,
+ Imu_jac_deltas(Eigen::VectorXd _Delta0,
Eigen::VectorXd _delta0,
Eigen::Matrix<Eigen::VectorXd,9,1> _Delta_noisy_vect,
Eigen::Matrix<Eigen::VectorXd,9,1> _delta_noisy_vect,
@@ -96,7 +96,7 @@ namespace wolf {
//
}
- IMU_jac_deltas(){
+ Imu_jac_deltas(){
for (int i=0; i<9; i++){
Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
@@ -108,7 +108,7 @@ namespace wolf {
jacobian_delta_ = Eigen::MatrixXd::Zero(9,9);
}
- IMU_jac_deltas(IMU_jac_deltas const & toCopy){
+ Imu_jac_deltas(Imu_jac_deltas const & toCopy){
Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
delta_noisy_vect_ = toCopy.delta_noisy_vect_;
@@ -136,7 +136,7 @@ namespace wolf {
Eigen::MatrixXd jacobian_delta_;
public:
- void copyfrom(IMU_jac_deltas const& right){
+ void copyfrom(Imu_jac_deltas const& right){
Delta_noisy_vect_ = right.Delta_noisy_vect_;
delta_noisy_vect_ = right.delta_noisy_vect_;
@@ -147,32 +147,32 @@ namespace wolf {
}
};
- class ProcessorIMU_UnitTester : public ProcessorIMU{
+ class ProcessorImu_UnitTester : public ProcessorImu{
public:
- ProcessorIMU_UnitTester();
- virtual ~ProcessorIMU_UnitTester();
+ ProcessorImu_UnitTester();
+ virtual ~ProcessorImu_UnitTester();
//Functions to test jacobians with finite difference method
/* params :
_data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
- _dt : time interval between 2 IMU measurements
+ _dt : time interval between 2 Imu measurements
da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one.
*/
- IMU_jac_bias finite_diff_ab(const double _dt,
+ Imu_jac_bias finite_diff_ab(const double _dt,
Eigen::Vector6d& _data,
const double& da_b,
const Eigen::Matrix<double,10,1>& _delta_preint0);
/* params :
_data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
- _dt : time interval between 2 IMU measurements
+ _dt : time interval between 2 Imu measurements
_Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
_delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
*/
- IMU_jac_deltas finite_diff_noise(const double& _dt,
+ Imu_jac_deltas finite_diff_noise(const double& _dt,
Eigen::Vector6d& _data,
const Eigen::Matrix<double,9,1>& _Delta_noise,
const Eigen::Matrix<double,9,1>& _delta_noise,
@@ -202,7 +202,7 @@ namespace wolf {
namespace wolf{
//Functions to test jacobians with finite difference method
-inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const double _dt,
+inline Imu_jac_bias ProcessorImu_UnitTester::finite_diff_ab(const double _dt,
Eigen::Vector6d& _data,
const double& da_b,
const Eigen::Matrix<double,10,1>& _delta_preint0)
@@ -265,11 +265,11 @@ inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const double _dt,
Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
}
- IMU_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
+ Imu_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
return bias_jacobians;
}
-inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+inline Imu_jac_deltas ProcessorImu_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
{
//we do not propagate any noise from data vector
Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
@@ -369,11 +369,11 @@ inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const double& _
Delta_noisy_vect(i+3) = Delta_;
}
- IMU_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
+ Imu_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
return jac_deltas;
}
} // namespace wolf
-#endif // PROCESSOR_IMU_UNITTESTER_H
+#endif // PROCESSOR_Imu_UNITTESTER_H