Skip to content
Snippets Groups Projects
Commit f5250cc2 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Fixed covariance calculation in gtest factor inertial kinematics

parent c444292a
No related branches found
No related tags found
3 merge requests!18Release after RAL,!17After 2nd RAL submission,!4Resolve "Migrate to state composites"
Hide whitespace changes
Inline Side-by-side
test/gtest_factor_inertial_kinematics.cpp
+ 42
− 41
View file @ f5250cc2
......@@ -58,6 +58,8 @@ using namespace wolf;
using namespace Eigen;
using namespace std;
const Vector3d ZERO3 = Vector3d::Zero();
const Vector6d ZERO6 = Vector6d::Zero();
class FactorInertialKinematics_1KF : public testing::Test
{
......@@ -74,9 +76,10 @@ class FactorInertialKinematics_1KF : public testing::Test
SensorInertialKinematicsPtr SIK_;
CaptureInertialKinematicsPtr CIK0_;
Eigen::Matrix3d Qp_, Qv_, Qw_;
Eigen::Vector3d bias_p_, bias_imu_;
FeatureInertialKinematicsPtr feat_in_;
StateBlockPtr sbbimu_;
Vector3d bias_p_;
Vector6d bias_imu_;
virtual void SetUp()
{
......@@ -101,13 +104,11 @@ class FactorInertialKinematics_1KF : public testing::Test
SIK_ = std::make_shared<SensorInertialKinematics>(Eigen::VectorXd(0), intr_ik);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
Vector3d zero3; zero3 << 0,0,0;
Vector6d zero6; zero6 << zero3, zero3;
Vector3d bias_p_ = zero3;
Vector6d bias_imu_ = zero6;
StateBlockPtr sbc = make_shared<StateBlock>(zero3);
StateBlockPtr sbd = make_shared<StateBlock>(zero3);
StateBlockPtr sbL = make_shared<StateBlock>(zero3);
bias_p_ = ZERO3;
bias_imu_ = ZERO6;
StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
sbbimu_ = make_shared<StateBlock>(bias_imu_);
......@@ -121,13 +122,13 @@ class FactorInertialKinematics_1KF : public testing::Test
// =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
// Measurements
Vector3d pBC_meas = zero3;
Vector3d vBC_meas = zero3;
Vector3d w_meas = zero3;
Vector3d pBC_meas = ZERO3;
Vector3d vBC_meas = ZERO3;
Vector3d w_meas = ZERO3;
// momentum parameters
Matrix3d Iq; Iq.setIdentity();
Vector3d Lq = zero3;
Vector3d Lq = ZERO3;
Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
......@@ -150,15 +151,15 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
// Fix the bp bias
CIK0_->getStateBlock("I")->fix();
KF0_->getStateBlock("C")->unfix();
Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas = zero3;
Eigen::Vector3d vBC_meas = zero3;
Eigen::Vector3d w_meas = zero3;
Eigen::Vector3d pBC_meas = ZERO3;
Eigen::Vector3d vBC_meas = ZERO3;
Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
Eigen::Vector3d Lq = zero3;
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
......@@ -175,15 +176,15 @@ class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1K
// Fix the bp bias
CIK0_->getStateBlock("I")->fix();
KF0_->getStateBlock("C")->unfix();
Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
Eigen::Vector3d vBC_meas = zero3;
Eigen::Vector3d w_meas = zero3;
Eigen::Vector3d vBC_meas = ZERO3;
Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
Eigen::Vector3d Lq = zero3;
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
......@@ -200,16 +201,16 @@ class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1K
// Fix the COM position
CIK0_->getStateBlock("I")->unfix();
KF0_->getStateBlock("C")->fix();
Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
Eigen::Vector3d vBC_meas = zero3;
Eigen::Vector3d w_meas = zero3;
Eigen::Vector3d vBC_meas = ZERO3;
Eigen::Vector3d w_meas = ZERO3;
bias_p_ << 1,0,0;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
Eigen::Vector3d Lq = zero3;
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
......@@ -224,19 +225,19 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
CIK0_->getStateBlock("I")->fix();
KF0_->getStateBlock("C")->unfix();
bias_p_ = zero3;
bias_p_ = ZERO3;
CIK0_->getStateBlock("I")->setState(bias_p_);
Eigen::Vector3d pBC_meas = zero3;
Eigen::Vector3d pBC_meas = ZERO3;
Eigen::Vector3d vBC_meas; vBC_meas << 1,0,0;
Eigen::Vector3d w_meas = zero3;
Eigen::Vector3d w_meas = ZERO3;
Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
Eigen::Vector3d Lq = zero3;
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
......@@ -303,13 +304,13 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
//
//
// // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
// Vector3d zero3; zero3 << 0,0,0;
// Vector6d zero6; zero6 << zero3, zero3;
// Vector3d bias_p_ = zero3;
// Vector6d bias_imu_ = zero6;
// StateBlockPtr sbc = make_shared<StateBlock>(zero3);
// StateBlockPtr sbd = make_shared<StateBlock>(zero3);
// StateBlockPtr sbL = make_shared<StateBlock>(zero3);
// Vector3d ZERO3; ZERO3 << 0,0,0;
// Vector6d ZERO6; ZERO6 << ZERO3, ZERO3;
// Vector3d bias_p_ = ZERO3;
// Vector6d bias_imu_ = ZERO6;
// StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
//
// KF0_->addStateBlock("C", sbcproblem_);
......@@ -323,13 +324,13 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
//
// // =============== SET DEFAULT" KIN MEASUREMENT AND COVARIANCE
// // Measurements
// Vector3d pBC_meas = zero3;
// Vector3d vBC_meas = zero3;
// Vector3d w_meas = zero3;
// Vector3d pBC_meas = ZERO3;
// Vector3d vBC_meas = ZERO3;
// Vector3d w_meas = ZERO3;
//
// // momentum parameters
// Matrix3d Iq; Iq.setIdentity();
// Vector3d Lq = zero3;
// Vector3d Lq = ZERO3;
//
// Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
// Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment