From ef7d19da084adda0ced2b408a7faf920f36066ff Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 16 Jun 2020 11:34:34 +0200
Subject: [PATCH] Key string --> char
---
.../bodydynamics/factor/factor_force_torque.h | 14 +-
.../factor/factor_force_torque_preint.h | 12 +-
.../factor/factor_inertial_kinematics.h | 8 +-
.../processor_inertial_kinematics.cpp | 2 +-
src/sensor/sensor_inertial_kinematics.cpp | 2 +-
test/gtest_factor_force_torque.cpp | 268 +++++++++---------
test/gtest_factor_inertial_kinematics.cpp | 56 ++--
test/gtest_processor_force_torque_preint.cpp | 196 ++++++-------
test/gtest_processor_inertial_kinematics.cpp | 22 +-
9 files changed, 290 insertions(+), 290 deletions(-)
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index a9ee309..c04ecaa 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -105,14 +105,14 @@ FactorForceTorque::FactorForceTorque(
_processor_ptr,
_apply_loss_function,
_status,
- _feat->getFrame()->getStateBlock("C"), // COM position, current
- _feat->getFrame()->getStateBlock("D"), // COM velocity (bad name), current
- _feat->getFrame()->getStateBlock("L"), // Angular momentum, current
- _frame_other->getStateBlock("C"), // COM position, previous
- _frame_other->getStateBlock("D"), // COM velocity (bad name), previous
- _frame_other->getStateBlock("L"), // Angular momentum, previous
+ _feat->getFrame()->getStateBlock('C'), // COM position, current
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum, current
+ _frame_other->getStateBlock('C'), // COM position, previous
+ _frame_other->getStateBlock('D'), // COM velocity (bad name), previous
+ _frame_other->getStateBlock('L'), // Angular momentum, previous
_sb_bp_other, // BC relative position bias, previous
- _frame_other->getStateBlock("O") // Base orientation, previous
+ _frame_other->getStateBlock('O') // Base orientation, previous
),
sb_bp_other_(_sb_bp_other)
{
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
index 99fb9e3..080a31b 100644
--- a/include/bodydynamics/factor/factor_force_torque_preint.h
+++ b/include/bodydynamics/factor/factor_force_torque_preint.h
@@ -169,15 +169,15 @@ inline FactorForceTorquePreint::FactorForceTorquePreint(
_processor_ptr,
_apply_loss_function,
_status,
- _cap_origin_ptr->getFrame()->getStateBlock("C"),
- _cap_origin_ptr->getFrame()->getStateBlock("D"),
- _cap_origin_ptr->getFrame()->getStateBlock("L"),
+ _cap_origin_ptr->getFrame()->getStateBlock('C'),
+ _cap_origin_ptr->getFrame()->getStateBlock('D'),
+ _cap_origin_ptr->getFrame()->getStateBlock('L'),
_cap_origin_ptr->getFrame()->getO(),
_cap_ikin_other->getSensorIntrinsic(),
_cap_gyro_other->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getStateBlock("C"),
- _ftr_ptr->getFrame()->getStateBlock("D"),
- _ftr_ptr->getFrame()->getStateBlock("L"),
+ _ftr_ptr->getFrame()->getStateBlock('C'),
+ _ftr_ptr->getFrame()->getStateBlock('D'),
+ _ftr_ptr->getFrame()->getStateBlock('L'),
_ftr_ptr->getFrame()->getO()
),
dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index cf36623..082737f 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -79,10 +79,10 @@ FactorInertialKinematics::FactorInertialKinematics(
_feat->getFrame()->getP(),
_feat->getFrame()->getO(),
_feat->getFrame()->getV(),
- _feat->getFrame()->getStateBlock("C"), // COM position
- _feat->getFrame()->getStateBlock("D"), // COM velocity (bad name)
- _feat->getFrame()->getStateBlock("L"), // Angular momentum
- _feat->getCapture()->getStateBlock("I"), // body-to-COM relative position bias
+ _feat->getFrame()->getStateBlock('C'), // COM position
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name)
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum
+ _feat->getCapture()->getStateBlock('I'), // body-to-COM relative position bias
_sb_bias_imu
)
{
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
index 1fe5892..f19ebad 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -63,7 +63,7 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
if (time_ok) {
CaptureBasePtr cap_angvel = buffer_pack_kf_it->second->key_frame->getCaptureOf(sensor_angvel);
auto min_ts = (buffer_pack_kf_it->first < buffer_capture_it->first) ? buffer_pack_kf_it->first : buffer_capture_it->first;
- if (cap_angvel && cap_angvel->getStateBlock("I")){ // TODO: or only cap_angvel?
+ if (cap_angvel && cap_angvel->getStateBlock('I')){ // TODO: or only cap_angvel?
// cast incoming capture to the range-and-bearing type, add it to the keyframe
auto kf = buffer_pack_kf_it->second->key_frame;
auto cap_ikin = std::static_pointer_cast<CaptureInertialKinematics>(buffer_capture_it->second);
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index ab6d15e..86c9084 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -17,7 +17,7 @@ SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extri
{
assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0.");
assert(getIntrinsic()->getState().size() == 3 && "Wrong extrinsics size! Should be 3.");
- setStateBlockDynamic("I", true);
+ setStateBlockDynamic('I', true);
}
SensorInertialKinematics::~SensorInertialKinematics()
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 8af2609..a36fb85 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -121,15 +121,15 @@ Matrix9d computeKinCov(const Matrix3d& Qp,
}
-void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
+void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
if(!KF->getStateBlock(sb_name)->isFixed())
{
- if (sb_name == "O")
+ if (sb_name == 'O')
{
double max_rad_pert = M_PI / 3;
Vector3d do_pert = max_rad_pert*Vector3d::Random();
Quaterniond curr_state_q(KF->getO()->getState().data());
- KF->getStateBlock("O")->setState((curr_state_q * exp_q(do_pert)).coeffs());
+ KF->getStateBlock('O')->setState((curr_state_q * exp_q(do_pert)).coeffs());
}
else
{
@@ -152,10 +152,10 @@ FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, Vec
Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
{
FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), wolf::KEY, t, "POV", x_origin);
- StateBlockPtr sbc = make_shared<StateBlock>(c); KF->addStateBlock("C", sbc, problem);
- StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock("D", sbd, problem);
- StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock("L", sbL, problem);
- StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock("I", sbbimu, problem); // Simulating IMU
+ StateBlockPtr sbc = make_shared<StateBlock>(c); KF->addStateBlock('C', sbc, problem);
+ StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock('D', sbd, problem);
+ StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock('L', sbL, problem);
+ StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
return KF;
}
@@ -226,16 +226,16 @@ class FactorInertialKinematics_2KF : public testing::Test
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_ = zero3;
bias_imu_ = zero6;
- StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("C", sbcA, problem_);
- StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("D", sbdA, problem_);
- StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("L", sbLA, problem_);
- StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock("I", sbbimuA, problem_);
+ StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
+ StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
+ StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
+ StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
// Fix the one we cannot estimate
// KFA_->getP()->fix();
// KFA_->getO()->fix();
// KFA_->getV()->fix();
- KFA_->getStateBlock("I")->fix(); // Imu
+ KFA_->getStateBlock('I')->fix(); // Imu
// INERTIAL KINEMATICS FACTOR
// Measurements
@@ -253,7 +253,7 @@ class FactorInertialKinematics_2KF : public testing::Test
Eigen::Matrix9d Q_ikin_err = computeKinCov(Qp_, Qv_, Qw_, meas_ikinA.head<3>()-bias_p_, meas_ikinA.tail<3>()-bias_imu_.tail<3>(), Iq);
CIKA_ = CaptureBase::emplace<CaptureInertialKinematics>(KFA_, tA_, SIK_, meas_ikinA, Iq, Lq, bias_p_);
- CIKA_->getStateBlock("I")->fix(); // IK bias
+ CIKA_->getStateBlock('I')->fix(); // IK bias
FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
FactorBase::emplace<FactorInertialKinematics>(FIKA_, FIKA_, sbbimuA, nullptr, false);
@@ -267,7 +267,7 @@ class FactorInertialKinematics_2KF : public testing::Test
// KFB_->getP()->fix();
KFB_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
- KFB_->getStateBlock("I")->fix(); // Imu
+ KFB_->getStateBlock('I')->fix(); // Imu
// // ================ PROCESS Imu DATA
@@ -285,7 +285,7 @@ class FactorInertialKinematics_2KF : public testing::Test
CIKB_ = CaptureBase::emplace<CaptureInertialKinematics>(KFB_, tB_, SIK_, meas_ikinB, Iq, Lq, bias_p_);
CIKB_->getSensorIntrinsic()->fix(); // IK bias
FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock("I"), nullptr, false);
+ auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -457,7 +457,7 @@ class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_
// KFB_->getP()->fix();
// KFC_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
- KFC_->getStateBlock("I")->fix();
+ KFC_->getStateBlock('I')->fix();
// ================ FACTOR INERTIAL KINEMATICS ON KFB
Vector3d pBC_measC = zero3;
@@ -469,9 +469,9 @@ class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_
Eigen::Matrix9d Q_ikin_errC = computeKinCov(Qp_, Qv_, Qw_, meas_ikinC.head<3>()-bias_p_, meas_ikinC.tail<3>()-bias_imu_.tail<3>(), Iq);
CIKC_ = CaptureBase::emplace<CaptureInertialKinematics>(KFC_, tC_, SIK_, meas_ikinC, Iq, Lq, bias_p_);
- CIKC_->getStateBlock("I")->fix(); // IK bias
+ CIKC_->getStateBlock('I')->fix(); // IK bias
auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock("I"), nullptr, false);
+ FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -519,9 +519,9 @@ class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinemati
FactorBase::emplace<FactorOdom3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false);
// With Odom3d the bias on relative base COM position is observable, we unfix the KFB state block
- CIKB_->getStateBlock("I")->unfix();
+ CIKB_->getStateBlock('I')->unfix();
// However this test is not sufficient to observe the bias at KFA
- // CIKA_->getStateBlock("I")->unfix(); // this is not observable
+ // CIKA_->getStateBlock('I')->unfix(); // this is not observable
}
};
@@ -555,17 +555,17 @@ TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
@@ -585,17 +585,17 @@ TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
@@ -615,17 +615,17 @@ TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
@@ -646,17 +646,17 @@ TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
@@ -677,17 +677,17 @@ TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
@@ -705,17 +705,17 @@ TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
@@ -731,17 +731,17 @@ TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
@@ -757,17 +757,17 @@ TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
// TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
@@ -783,21 +783,21 @@ TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
// // std::cout << report << std::endl;
// // problem_->print(4,true,true,true);
-// ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
-// // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock("C")->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock("D")->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock("L")->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock("I")->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+// // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock('I')->getState(), zero3, 1e-5);
// }
@@ -813,17 +813,17 @@ TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at("P"), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at("O"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at("V"), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock("I")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock("I")->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+ ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index f263d81..8dfb0f5 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -111,13 +111,13 @@ class FactorInertialKinematics_1KF : public testing::Test
StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
sbbimu_ = make_shared<StateBlock>(bias_imu_);
- KF0_->addStateBlock("C", sbc, problem_);
- KF0_->addStateBlock("D", sbd, problem_);
- KF0_->addStateBlock("L", sbL, problem_);
- KF0_->addStateBlock("I", sbbimu_, problem_); // IMU bias
+ KF0_->addStateBlock('C', sbc, problem_);
+ KF0_->addStateBlock('D', sbd, problem_);
+ KF0_->addStateBlock('L', sbL, problem_);
+ KF0_->addStateBlock('I', sbbimu_, problem_); // IMU bias
// Fix the one we cannot estimate
- KF0_->getStateBlock("I")->fix(); // Imu
+ KF0_->getStateBlock('I')->fix(); // Imu
// =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
// Measurements
@@ -148,8 +148,8 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- CIK0_->getStateBlock("I")->fix();
- KF0_->getStateBlock("C")->unfix();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas = ZERO3;
@@ -173,8 +173,8 @@ class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1K
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
- CIK0_->getStateBlock("I")->fix();
- KF0_->getStateBlock("C")->unfix();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
@@ -198,8 +198,8 @@ class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1K
{
FactorInertialKinematics_1KF::SetUp();
// Fix the COM position
- CIK0_->getStateBlock("I")->unfix();
- KF0_->getStateBlock("C")->fix();
+ CIK0_->getStateBlock('I')->unfix();
+ KF0_->getStateBlock('C')->fix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
@@ -225,10 +225,10 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
- CIK0_->getStateBlock("I")->fix();
- KF0_->getStateBlock("C")->unfix();
+ CIK0_->getStateBlock('I')->fix();
+ KF0_->getStateBlock('C')->unfix();
bias_p_ = ZERO3;
- CIK0_->getStateBlock("I")->setState(bias_p_);
+ CIK0_->getStateBlock('I')->setState(bias_p_);
Eigen::Vector3d pBC_meas = ZERO3;
Eigen::Vector3d vBC_meas; vBC_meas << 1,0,0;
@@ -373,10 +373,10 @@ TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt)
KF0_->perturb();
string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.at("P"), 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.at('P'), 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
@@ -390,9 +390,9 @@ TEST_F(FactorInertialKinematics_1KF_1p_bpfix,ZeroMvt)
KF0_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt)
@@ -405,10 +405,10 @@ TEST_F(FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt)
KF0_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIK0_->getStateBlock("I")->getState(), bp_exp, 1e-5);
+ // ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(CIK0_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
TEST_F(FactorInertialKinematics_1KF_1v_bfix,ZeroMvt)
@@ -421,9 +421,9 @@ TEST_F(FactorInertialKinematics_1KF_1v_bfix,ZeroMvt)
KF0_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 1a861db..58ee5df 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -541,38 +541,38 @@ public:
TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
{
// // TEST FIRST PURE INTEGRATION
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// THEN SOLVE
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
@@ -586,18 +586,18 @@ TEST_F(Cst2KFZeroMotionBias, ZeroMotionBias)
// problem_->print(4,true,true,true);
// std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
@@ -611,19 +611,19 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), posi_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
problem_->perturb();
@@ -631,19 +631,19 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
// problem_->print(4, true, true, true);
// std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), ZERO3.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), ZERO3.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
@@ -656,38 +656,38 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), ZERO3.head(2), 1e-6);
@@ -706,19 +706,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBias, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
@@ -737,19 +737,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
@@ -762,14 +762,14 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
// Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
// Vector4d q_diff; q_diff << 1,0,0,0;
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
-// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
-
-// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("O")->getState(), q_diff, 1e-6);
-// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), Lc_diff, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('O')->getState(), q_diff, 1e-6);
+// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), Lc_diff, 1e-6);
// }
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 1fa6c8b..8653902 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -200,17 +200,17 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
FrameBasePtr KF1 = problem_->getTrajectory()->getLastKeyFrame();
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("V")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), ZERO3, 1e-6);
- ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), ZERO3, 1e-6);
-
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("P")->getState(), pdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("V")->getState(), vdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("C")->getState(), pdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("D")->getState(), vdiff, 1e-6);
- ASSERT_MATRIX_APPROX(KF1->getStateBlock("L")->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('P')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('V')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), ZERO3, 1e-6);
+ ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), ZERO3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('P')->getState(), pdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('V')->getState(), vdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('C')->getState(), pdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('D')->getState(), vdiff, 1e-6);
+ ASSERT_MATRIX_APPROX(KF1->getStateBlock('L')->getState(), ZERO3, 1e-6);
}
--
GitLab