diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque_preint.h
index 3e6a24840bfa8e6f9265b3fb2352e88c14c693e6..3436b5b8acb4b8a6bbea631c46e2b93b8aceaea2 100644
--- a/include/bodydynamics/capture/capture_force_torque_preint.h
+++ b/include/bodydynamics/capture/capture_force_torque_preint.h
@@ -7,6 +7,8 @@
#include "core/capture/capture_motion.h"
#include "bodydynamics/capture/capture_inertial_kinematics.h"
+#include <core/state_block/state_composite.h>
+
namespace wolf {
WOLF_PTR_TYPEDEFS(CaptureForceTorquePreint);
@@ -39,35 +41,14 @@ class CaptureForceTorquePreint : public CaptureMotion
virtual ~CaptureForceTorquePreint();
- virtual VectorXd getCalibration() const override;
-
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
-
CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_; }
CaptureBasePtr getGyroCaptureOther(){ return cap_gyro_other_;}
private:
CaptureBasePtr cap_ikin_other_;
CaptureBasePtr cap_gyro_other_;
-
- protected:
- SizeEigen computeCalibSize() const override;
-
-
};
-inline SizeEigen CaptureForceTorquePreint::computeCalibSize() const
-{
- // Hack: CaptureBase::computeCalibSize will compute a size 0 since no statblock is stored in this capture
- // To be able to use setCalib, we need to override this function
- return 6;
-}
-
-inline Eigen::VectorXd CaptureForceTorquePreint::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error_biases) const
-{
- return bodydynamics::plus(_delta, _delta_error_biases); // deltacorr = plus(deltapreint, J_deltapreint_biases * (biases - biasespreint)
-}
-
} // namespace wolf
#endif // CAPTURE_FORCE_TORQUE_PREINT_H
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
index 2b54d2b1214bb5366a3dd860e4ef776adb99ba74..c455c6d893b1419daba15a65921da84136f7e4d3 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque_preint.h
@@ -58,11 +58,16 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
Eigen::VectorXd& _delta_preint_plus_delta,
Eigen::MatrixXd& _jacobian_delta_preint,
Eigen::MatrixXd& _jacobian_delta) const override;
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXd& _x_plus_delta ) const override;
+ VectorComposite& _x_plus_delta ) const override;
virtual Eigen::VectorXd deltaZero() const override;
+ virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
virtual bool voteForKeyFrame() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/src/capture/capture_force_torque_preint.cpp b/src/capture/capture_force_torque_preint.cpp
index 264faa7be50cd26eb8983ab100dcdccb76f00507..8435c934a2a640f5b1ed41cecca889ac75edfc76 100644
--- a/src/capture/capture_force_torque_preint.cpp
+++ b/src/capture/capture_force_torque_preint.cpp
@@ -13,7 +13,7 @@ CaptureForceTorquePreint::CaptureForceTorquePreint(
const Eigen::Matrix<double, 32, 1>& _data,
const Eigen::MatrixXd& _data_cov, // TODO: no uncertainty from kinematics
CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("CaptureForceTorquePreint", _init_ts, _sensor_ptr, _data, _data_cov, 13, 12,
+ CaptureMotion("CaptureForceTorquePreint", _init_ts, _sensor_ptr, _data, _data_cov,
_capture_origin_ptr, nullptr, nullptr, nullptr),
cap_ikin_other_(_capture_IK_ptr),
cap_gyro_other_(_capture_motion_ptr)
@@ -25,14 +25,4 @@ CaptureForceTorquePreint::~CaptureForceTorquePreint()
}
-VectorXd CaptureForceTorquePreint::getCalibration() const
-{
- // Needs to be overriden since the calib vector is coming from 2 different stateblocks
- // that are NOT in the capture stateblock structure (contrary ti CaptureImu for instance)
- Eigen::Vector6d bias_vec;
- bias_vec.segment<3>(0) = cap_ikin_other_->getSensorIntrinsic()->getState();
- bias_vec.segment<3>(3) = cap_gyro_other_->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
- return bias_vec;
-}
-
} //namespace wolf
diff --git a/src/capture/capture_leg_odom.cpp b/src/capture/capture_leg_odom.cpp
index e45f6492f2c926422fcb22351ae7c710c9a7c3b0..69319a07d654b67cbe26adf2282851058f370766 100644
--- a/src/capture/capture_leg_odom.cpp
+++ b/src/capture/capture_leg_odom.cpp
@@ -20,7 +20,7 @@ CaptureLegOdom::CaptureLegOdom(const TimeStamp& _init_ts,
double dt) :
CaptureMotion("CaptureLegOdom", _init_ts, _sensor_ptr,
Eigen::Vector7d::Zero(), Eigen::Matrix6d::Identity(), // dummy data and data covariance filled in the constructor
- 7, 6, nullptr, nullptr, nullptr, nullptr),
+ nullptr, nullptr, nullptr, nullptr),
data_kin_(_data_kin)
{
Vector7d data;
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
index 35cd62515b9e66a5ea54c6216434944e106dc58f..6cb8586b9db0b016905af79cddf99293fdd3c281 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/src/processor/processor_force_torque_preint.cpp
@@ -84,8 +84,8 @@ CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr&
// which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
auto cap_ftpreint = std::static_pointer_cast<CaptureForceTorquePreint>(cap);
- Vector6d calib = cap_ftpreint->getCalibration();
- cap_ftpreint->setCalibration(calib);
+ Vector6d calib = getCalibration(cap_ftpreint);
+ setCalibration(cap_ftpreint, calib);
cap_ftpreint->setCalibrationPreint(calib);
return cap;
@@ -102,6 +102,40 @@ FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capt
return feature;
}
+Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ return bodydynamics::plus(delta_preint, delta_step);
+}
+
+VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBasePtr _capture) const
+{
+ VectorXd bias_vec(6);
+ CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
+
+ // get calib part from Ikin capture
+ bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
+
+ // get calib part from IMU capture
+ bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+
+ return bias_vec;
+}
+
+void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
+
+ // set calib part in Ikin capture
+ cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>());
+
+ // set calib part in IMU capture
+ Vector6d calib_imu = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState();
+ calib_imu.tail<3>() = _calibration.tail<3>();
+
+ cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
+}
+
FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
CaptureForceTorquePreintPtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorquePreint>(_capture_origin);
@@ -158,17 +192,21 @@ void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_pr
_delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
}
-void ProcessorForceTorquePreint::statePlusDelta(const Eigen::VectorXd& _x,
+void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXd& _x_plus_delta) const
+ VectorComposite& _x_plus_delta) const
{
- assert(_x.size() == 13 && "Wrong _x vector size");
assert(_delta.size() == 13 && "Wrong _delta vector size");
- assert(_x_plus_delta.size() == 13 && "Wrong _x_plus_delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
- _x_plus_delta = bodydynamics::composeOverState(_x, _delta, _dt);
+ // verbose way : create Eigen vectors, then compute, then convert back to Composite
+
+ auto x = _x.vector("CDLO");
+
+ VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
+
+ _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,3});
}
void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index dc0e94a1a1e08be6aad6285360025fada105e204..2adf0d7300248dd368115e565f26f49b8cd128a6 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -148,10 +148,10 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
}
}
-FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, Vector10d x_origin,
+FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, VectorComposite x_origin,
Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
{
- FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), "POV", 3, wolf::KEY, t, x_origin);
+ 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);
@@ -168,8 +168,8 @@ class FactorInertialKinematics_2KF : public testing::Test
wolf::TimeStamp tB_;
ProblemPtr problem_;
CeresManagerPtr ceres_manager_;
- VectorXd x_origin_;
- MatrixXd P_origin_;
+ VectorComposite x_origin_; // initial state
+ VectorComposite s_origin_; // initial sigmas for prior
SensorInertialKinematicsPtr SIK_;
CaptureInertialKinematicsPtr CIKA_, CIKB_;
FrameBasePtr KFA_;
@@ -219,17 +219,9 @@ class FactorInertialKinematics_2KF : public testing::Test
// ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
tA_.set(0);
- x_origin_.resize(10);
- x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
- P_origin_ = pow(1e-3, 2) * Matrix9d::Identity();
-
- // Set origin of the problem
- //KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005);
- // Specific factor for origin velocity (not covered by setPrior)
- //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KFA_, "Vel0", tA_);
- //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KFA_->getV());
- KFA_ = problem_->setPriorFactor(x_origin_, P_origin_, tA_, 0.005);
+ x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+ s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
+ KFA_ = problem_->setPriorFactor(x_origin_, s_origin_, tA_, 0.005);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
@@ -325,8 +317,6 @@ class FactorInertialKinematics_2KF : public testing::Test
FactorForceTorquePtr fac_ftAB = FactorBase::emplace<FactorForceTorque>(FFTAB_, FFTAB_, KFA_, CIKA_->getSensorIntrinsic(), nullptr, false);
}
-
- virtual void TearDown() override {}
};
@@ -566,9 +556,9 @@ TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
// 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); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 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);
@@ -596,9 +586,9 @@ 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_.head<3>(), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 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);
@@ -626,9 +616,9 @@ 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_.head<3>(), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 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);
@@ -657,9 +647,9 @@ 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_.head<3>(), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 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);
@@ -688,9 +678,9 @@ 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_.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_->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);
@@ -716,9 +706,9 @@ 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_.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_->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);
@@ -742,9 +732,9 @@ 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_.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_->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);
@@ -768,9 +758,9 @@ 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_.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_->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);
@@ -794,9 +784,9 @@ 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_.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_->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);
@@ -824,9 +814,9 @@ 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_.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_->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);
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 8902156fef0fee818dd1ff4c6e24ba7946e10c14..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
{
@@ -68,15 +70,16 @@ class FactorInertialKinematics_1KF : public testing::Test
CeresManagerPtr ceres_manager_;
// ProcessorBasePtr processor;
// ProcessorImuPtr processor_imu;
- VectorXd x_origin_;
- MatrixXd P_origin_;
+ VectorComposite x_origin_; // initial state
+ VectorComposite s_origin_; // initial sigmas for prior
FrameBasePtr KF0_;
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()
{
@@ -92,32 +95,20 @@ class FactorInertialKinematics_1KF : public testing::Test
ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
//===================================================== INITIALIZATION
- x_origin_.resize(10);
- x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
- P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+ x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+ s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
t_.set(0.0);
-
-
- // Set origin of the problem
- //KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
- // Specific factor for origin velocity (not covered by setPrior)
- //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
- //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
- //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
-
- KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
+ KF0_ = problem_->setPriorFactor(x_origin_, s_origin_, t_, 0.005);
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
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_);
@@ -131,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();
@@ -160,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);
@@ -185,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);
@@ -210,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);
@@ -234,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);
@@ -313,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_);
@@ -333,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();
@@ -383,7 +374,7 @@ TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt)
KF0_->perturb();
string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.head(3), 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);
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index f8c95c9b5ffe5f15cfa4111b6bdebacc38b7684b..b4b4cd15b5a7f245d07c3261152147d44b9d0d55 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -204,7 +204,7 @@ public:
// - call setOrigin on processors isMotion
setOriginState();
MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18,18);
- KF1_ = problem_->emplaceFrame(KEY, x_origin_, t0_);
+ KF1_ = problem_->emplaceFrame(KEY, t0_, x_origin_);
// Prior pose factor
CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1_, t0_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
pose_prior_capture->emplaceFeatureAndFactor();
@@ -560,8 +560,6 @@ TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
problem_->perturb();
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // 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);
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 510e2c235b695d3e3ff71861a58d2b39c5062f52..3a715897dfafab2acda28d1a991b5e2131d40636 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -102,7 +102,7 @@ class FactorInertialKinematics_2KF : public testing::Test
// Set origin of the problem
// KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
- KF0_ = problem_->emplaceFrame(KEY, x_origin_, t_);
+ KF0_ = problem_->emplaceFrame(KEY, t_, x_origin_);
///////////////////////////////////////////////////
// Prior pose factor