diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 5e55b6171b199d9b4228ce5495d910f248f42290..9eff4752348505f42b7f26484de945533e33802f 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -61,6 +61,9 @@ using namespace std;
// SOME CONSTANTS
const double Acc1 = 1.0;
const double Acc2 = 2.5;
+const Vector3d zero3 = Vector3d::Zero();
+const Vector6d zero6 = Vector6d::Zero();
+
Matrix9d computeKinJac(const Vector3d& w_unb,
@@ -120,7 +123,9 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
}
else
{
- Vector3d pert = Vector3d::Random();
+ VectorXd pert;
+ pert.resize(KF->getStateBlock(sb_name)->getSize());
+ pert.setRandom();
KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
}
}
@@ -133,6 +138,18 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
}
}
+FrameBasePtr createKFWithCDLBI(const ProblemPtr& problem, const TimeStamp& t, const Vector10d& x_origin,
+ const Vector3d& c, const Vector3d& cd, const Vector3d& Lc, const Vector3d& bias_p, const Vector6d& bias_imu)
+{
+ FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), "POV", 3, wolf::KEY, t, x_origin);
+ StateBlockPtr sbc = make_shared<StateBlock>(c); KF->addStateBlock("C", sbc); problem->notifyStateBlock(sbc,ADD);
+ StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock("D", sbd); problem->notifyStateBlock(sbd,ADD);
+ StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock("L", sbL); problem->notifyStateBlock(sbL,ADD);
+ StateBlockPtr sbb = make_shared<StateBlock>(bias_p); KF->addStateBlock("B", sbb); problem->notifyStateBlock(sbb,ADD);
+ StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock("I", sbbimu); problem->notifyStateBlock(sbbimu,ADD);
+ return KF;
+}
+
class FactorInertialKinematics_2KF : public testing::Test
{
@@ -147,13 +164,15 @@ class FactorInertialKinematics_2KF : public testing::Test
FrameBasePtr KFA_;
FrameBasePtr KFB_;
Matrix3d Qp_, Qv_, Qw_;
- Vector3d bias_p_, bias_imu_;
+ Vector3d bias_p_;
+ Vector6d bias_imu_;
FeatureInertialKinematicsPtr feat_ikinA_;
FeatureInertialKinematicsPtr feat_ikinB_;
FeatureForceTorquePtr feat_ftAB_;
// SensorIMUPtr sen_imu_;
// ProcessorIMUPtr processor_imu_;
-
+
+ protected:
virtual void SetUp()
{
@@ -195,10 +214,8 @@ class FactorInertialKinematics_2KF : public testing::Test
// 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;
+ bias_p_ = zero3;
+ bias_imu_ = zero6;
StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("C", sbcA); problem_->notifyStateBlock(sbcA,ADD);
StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("D", sbdA); problem_->notifyStateBlock(sbdA,ADD);
StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("L", sbLA); problem_->notifyStateBlock(sbLA,ADD);
@@ -238,16 +255,11 @@ class FactorInertialKinematics_2KF : public testing::Test
// =============== NEW KFB WITH CORRESPONDING STATEBLOCKS
tB_.set(1);
- KFB_ = FrameBase::emplace<FrameBase>(problem_->getTrajectory(), "POV", 3, wolf::KEY, tB_, x_origin_);
- StateBlockPtr sbcB = make_shared<StateBlock>(zero3); KFB_->addStateBlock("C", sbcB); problem_->notifyStateBlock(sbcB,ADD);
- StateBlockPtr sbdB = make_shared<StateBlock>(zero3); KFB_->addStateBlock("D", sbdB); problem_->notifyStateBlock(sbdB,ADD);
- StateBlockPtr sbLB = make_shared<StateBlock>(zero3); KFB_->addStateBlock("L", sbLB); problem_->notifyStateBlock(sbLB,ADD);
- StateBlockPtr sbbB = make_shared<StateBlock>(bias_p_); KFB_->addStateBlock("B", sbbB); problem_->notifyStateBlock(sbbB,ADD);
- StateBlockPtr sbbimuB = make_shared<StateBlock>(bias_imu_); KFB_->addStateBlock("I", sbbimuB); problem_->notifyStateBlock(sbbimuB,ADD);
-
+ KFB_ = createKFWithCDLBI(problem_, tB_, x_origin_,
+ zero3, zero3, zero3, bias_p_, bias_imu_);
// Fix the one we cannot estimate
// KFB_->getP()->fix();
- KFB_->getO()->fix();
+ KFB_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
KFB_->getStateBlock("I")->fix();
KFB_->getStateBlock("B")->fix();
@@ -268,7 +280,7 @@ class FactorInertialKinematics_2KF : public testing::Test
CaptureBasePtr cap_ikinB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureInertialKinematics", tB_, nullptr);
auto feat_ikinB = FeatureBase::emplace<FeatureInertialKinematics>(cap_ikinB, meas_ikinB, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
feat_ikinB_ = static_pointer_cast<FeatureInertialKinematics>(feat_ikinB); // !! auto feat_ikinB_ creates no compilation error but a segfault
- auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(feat_ikinB_, feat_ikinB_, sbbimuB, nullptr, false);
+ auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(feat_ikinB_, feat_ikinB_, KFB_->getStateBlock("I"), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -302,8 +314,9 @@ class FactorInertialKinematics_2KF : public testing::Test
};
-class FactorInertialKinematics_2KF_FX : public FactorInertialKinematics_2KF
+class FactorInertialKinematics_2KF_ForceX : public FactorInertialKinematics_2KF
{
+ protected:
virtual void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
@@ -314,8 +327,9 @@ class FactorInertialKinematics_2KF_FX : public FactorInertialKinematics_2KF
}
};
-class FactorInertialKinematics_2KF_FY : public FactorInertialKinematics_2KF
+class FactorInertialKinematics_2KF_ForceY : public FactorInertialKinematics_2KF
{
+ protected:
virtual void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
@@ -326,6 +340,87 @@ class FactorInertialKinematics_2KF_FY : public FactorInertialKinematics_2KF
}
};
+class FactorInertialKinematics_2KF_ForceZ : public FactorInertialKinematics_2KF
+{
+ protected:
+ virtual void SetUp() override
+ {
+ FactorInertialKinematics_2KF::SetUp();
+ Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
+ Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
+ Vector6d fmeas; fmeas << f1, f2;
+ feat_ftAB_->setForcesMeas(fmeas);
+ }
+};
+
+class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_ForceX
+{
+ public:
+ FrameBasePtr KFC_;
+
+ protected:
+ virtual void SetUp() override
+ {
+ FactorInertialKinematics_2KF_ForceX::SetUp();
+ TimeStamp tC;
+ tC.set(2);
+
+ std::cout << "\n\n\n" << feat_ftAB_->getForcesMeas().transpose() << endl;
+ // WHY NOT SAME VALUE AS
+
+ // KFB_->getStateBlock("O")->unfix();
+
+ KFC_ = createKFWithCDLBI(problem_, tC, x_origin_,
+ zero3, zero3, zero3, bias_p_, bias_imu_);
+ // Fix the one we cannot estimate
+ // 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("B")->fix();
+
+ // ================ FACTOR INERTIAL KINEMATICS ON KFB
+ Vector3d pBC_measC = zero3;
+ Vector3d vBC_measC = zero3;
+ Vector3d w_measC = zero3;
+ Vector9d meas_ikinC; meas_ikinC << pBC_measC, vBC_measC, w_measC;
+ Matrix3d Iq; Iq.setIdentity();
+ Vector3d Lq = zero3;
+
+ Matrix9d QkinC = computeKinCov(Qp_, Qv_, Qw_, pBC_measC-bias_p_, w_measC-bias_imu_.tail(3), Iq);
+ CaptureBasePtr cap_ikinB = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureInertialKinematics", tC, nullptr);
+ auto feat_BC = FeatureBase::emplace<FeatureInertialKinematics>(cap_ikinB, meas_ikinC, QkinC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ auto feat_ikinBC = static_pointer_cast<FeatureInertialKinematics>(feat_BC); // !! auto feat_ikinB_ creates no compilation error but a segfault
+ auto fac_inkinBC = FactorBase::emplace<FactorInertialKinematics>(feat_ikinBC, feat_ikinBC, KFC_->getStateBlock("I"), nullptr, false);
+
+
+ // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
+ Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
+ Vector3d tau1; tau1 << 0,0,0;
+ Vector3d pbl1; pbl1 << 0,0,0;
+ Vector4d bql1; bql1 << 0,0,0,1;
+ Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
+ Vector3d tau2; tau2 << 0,0,0;
+ Vector3d pbl2; pbl2 << 0,0,0;
+ Vector4d bql2; bql2 << 0,0,0,1;
+ Vector3d pbc; pbc << 0,0,0;
+ // aggregated meas
+ Vector6d f_meas; f_meas << f1, f2;
+ Vector6d tau_meas; tau_meas << tau1, tau2;
+ Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
+
+ Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
+ Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
+ Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
+
+ CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC, nullptr);
+ auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(cap_ftC,
+ tC - tB_, mass_,
+ f_meas, tau_meas, pbc, kin_meas,
+ cov_f, cov_tau, cov_pbc);
+ FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, nullptr, false);
+ }
+};
////////////////////////////////////////////////////////
// =============== TEST FONCTIONS ======================
@@ -363,7 +458,7 @@ TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
}
-TEST_F(FactorInertialKinematics_2KF_FX,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
{
string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -389,7 +484,7 @@ TEST_F(FactorInertialKinematics_2KF_FX,ZeroMvt)
}
-TEST_F(FactorInertialKinematics_2KF_FY,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
{
string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -414,6 +509,63 @@ TEST_F(FactorInertialKinematics_2KF_FY,ZeroMvt)
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
}
+
+TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
+{
+ string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ perturbateAllIfUnFixed(KFA_);
+ perturbateAllIfUnFixed(KFB_);
+
+ problem_->print(4,true,true,true);
+ report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::cout << report << std::endl;
+ problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, 0, Acc1/2).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, 0, Acc1).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+}
+
+TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
+{
+ string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+ perturbateAllIfUnFixed(KFA_);
+ perturbateAllIfUnFixed(KFB_);
+ perturbateAllIfUnFixed(KFC_);
+
+ problem_->print(4,true,true,true);
+ report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::cout << report << std::endl;
+ problem_->print(4,true,true,true);
+
+ ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
+ // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFC_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2 + Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
+ ASSERT_MATRIX_APPROX(KFC_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
+ ASSERT_MATRIX_APPROX(KFC_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(KFC_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);