diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index bebfa15f6b75e9b668ea8b91e7fcdf977bcaeb03..095e702fac013b2cea83817f0902aca981b02fa7 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -58,6 +58,8 @@ using namespace wolf;
using namespace Eigen;
using namespace std;
+const Vector3d ZERO3 = Vector3d::Zero();
+const Vector6d ZERO6 = Vector6d::Zero();
class FactorInertialKinematics_1KF : public testing::Test
{
@@ -74,9 +76,10 @@ class FactorInertialKinematics_1KF : public testing::Test
SensorInertialKinematicsPtr SIK_;
CaptureInertialKinematicsPtr CIK0_;
Eigen::Matrix3d Qp_, Qv_, Qw_;
- Eigen::Vector3d bias_p_, bias_imu_;
FeatureInertialKinematicsPtr feat_in_;
StateBlockPtr sbbimu_;
+ Vector3d bias_p_;
+ Vector6d bias_imu_;
virtual void SetUp()
{
@@ -101,13 +104,11 @@ class FactorInertialKinematics_1KF : public testing::Test
SIK_ = std::make_shared<SensorInertialKinematics>(Eigen::VectorXd(0), intr_ik);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
- Vector3d zero3; zero3 << 0,0,0;
- Vector6d zero6; zero6 << zero3, zero3;
- Vector3d bias_p_ = zero3;
- Vector6d bias_imu_ = zero6;
- StateBlockPtr sbc = make_shared<StateBlock>(zero3);
- StateBlockPtr sbd = make_shared<StateBlock>(zero3);
- StateBlockPtr sbL = make_shared<StateBlock>(zero3);
+ bias_p_ = ZERO3;
+ bias_imu_ = ZERO6;
+ StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
+ StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
+ StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
sbbimu_ = make_shared<StateBlock>(bias_imu_);
@@ -121,13 +122,13 @@ class FactorInertialKinematics_1KF : public testing::Test
// =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
// Measurements
- Vector3d pBC_meas = zero3;
- Vector3d vBC_meas = zero3;
- Vector3d w_meas = zero3;
+ Vector3d pBC_meas = ZERO3;
+ Vector3d vBC_meas = ZERO3;
+ Vector3d w_meas = ZERO3;
// momentum parameters
Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
+ Vector3d Lq = ZERO3;
Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
@@ -150,15 +151,15 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
// Fix the bp bias
CIK0_->getStateBlock("I")->fix();
KF0_->getStateBlock("C")->unfix();
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
- Eigen::Vector3d pBC_meas = zero3;
- Eigen::Vector3d vBC_meas = zero3;
- Eigen::Vector3d w_meas = zero3;
+ Eigen::Vector3d pBC_meas = ZERO3;
+ Eigen::Vector3d vBC_meas = ZERO3;
+ Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
+ Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
@@ -175,15 +176,15 @@ class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1K
// Fix the bp bias
CIK0_->getStateBlock("I")->fix();
KF0_->getStateBlock("C")->unfix();
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
- Eigen::Vector3d vBC_meas = zero3;
- Eigen::Vector3d w_meas = zero3;
+ Eigen::Vector3d vBC_meas = ZERO3;
+ Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
+ Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
@@ -200,16 +201,16 @@ class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1K
// Fix the COM position
CIK0_->getStateBlock("I")->unfix();
KF0_->getStateBlock("C")->fix();
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
- Eigen::Vector3d vBC_meas = zero3;
- Eigen::Vector3d w_meas = zero3;
+ Eigen::Vector3d vBC_meas = ZERO3;
+ Eigen::Vector3d w_meas = ZERO3;
bias_p_ << 1,0,0;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
+ Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
@@ -224,19 +225,19 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
+ Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
CIK0_->getStateBlock("I")->fix();
KF0_->getStateBlock("C")->unfix();
- bias_p_ = zero3;
+ bias_p_ = ZERO3;
CIK0_->getStateBlock("I")->setState(bias_p_);
- Eigen::Vector3d pBC_meas = zero3;
+ Eigen::Vector3d pBC_meas = ZERO3;
Eigen::Vector3d vBC_meas; vBC_meas << 1,0,0;
- Eigen::Vector3d w_meas = zero3;
+ Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
- Eigen::Vector3d Lq = zero3;
+ Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
@@ -303,13 +304,13 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
//
//
// // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
-// Vector3d zero3; zero3 << 0,0,0;
-// Vector6d zero6; zero6 << zero3, zero3;
-// Vector3d bias_p_ = zero3;
-// Vector6d bias_imu_ = zero6;
-// StateBlockPtr sbc = make_shared<StateBlock>(zero3);
-// StateBlockPtr sbd = make_shared<StateBlock>(zero3);
-// StateBlockPtr sbL = make_shared<StateBlock>(zero3);
+// Vector3d ZERO3; ZERO3 << 0,0,0;
+// Vector6d ZERO6; ZERO6 << ZERO3, ZERO3;
+// Vector3d bias_p_ = ZERO3;
+// Vector6d bias_imu_ = ZERO6;
+// StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
+// StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
+// StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
//
// KF0_->addStateBlock("C", sbcproblem_);
@@ -323,13 +324,13 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
//
// // =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
// // Measurements
-// Vector3d pBC_meas = zero3;
-// Vector3d vBC_meas = zero3;
-// Vector3d w_meas = zero3;
+// Vector3d pBC_meas = ZERO3;
+// Vector3d vBC_meas = ZERO3;
+// Vector3d w_meas = ZERO3;
//
// // momentum parameters
// Matrix3d Iq; Iq.setIdentity();
-// Vector3d Lq = zero3;
+// Vector3d Lq = ZERO3;
//
// Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
// Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();