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

Refactored gtests proc ftpreint to input changing acceleration, added loose... 

Refactored gtests proc ftpreint to input changing acceleration, added loose prior on bpc + some comments
parent 6983ead8
No related branches found
No related tags found
2 merge requests!18Release after RAL,!17After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
demos/processor_imu.yaml
+ 1
− 1
View file @ 28402ecb
......@@ -3,7 +3,7 @@ name: "Main imu" # This is ignored. The name provided to the SensorFactory pr
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00000001
keyframe_vote:
max_time_span: 1.99 # seconds
max_time_span: 2.99 # seconds
max_buff_length: 500 # motion deltas
dist_traveled: 20000.0 # meters
angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg)
......
include/bodydynamics/factor/factor_force_torque_preint.h
+ 12
− 7
View file @ 28402ecb
......@@ -179,17 +179,17 @@ inline FactorForceTorquePreint::FactorForceTorquePreint(
_ftr_ptr->getFrame()->getStateBlock("L"),
_ftr_ptr->getFrame()->getO()
),
dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, cq at preintegration time
dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
dq_preint_(_ftr_ptr->getMeasurement().data()+9),
pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()),
gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()),
J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)),
J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jacs of dp dv dq wrt biases
J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)),
J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)),
J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)),
J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias
J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias
dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
{
......@@ -266,7 +266,6 @@ inline bool FactorForceTorquePreint::residual(const Eigen::MatrixBase<D1> &
const Eigen::QuaternionBase<D3> & _q2,
Eigen::MatrixBase<D4> & _res) const
{
/* Help for the Imu residual function
*
* Notations:
......@@ -326,6 +325,12 @@ inline bool FactorForceTorquePreint::residual(const Eigen::MatrixBase<D1> &
Eigen::Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
Eigen::Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
// std::cout << "\n\nJ_dLc_pb_" << std::endl;
// std::cout << J_dLc_pb_ << std::endl;
// FullPivLU<Matrix3d> lu_decomp(J_dLc_pb_);
// std::cout << "J_dLc_pb_ rank: " << lu_decomp.rank() << std::endl;
// 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
Eigen::Matrix<T,3,1> dc_correct;
Eigen::Matrix<T,3,1> dcd_correct;
......
test/gtest_processor_force_torque_preint.cpp
+ 119
− 53
View file @ 28402ecb
......@@ -72,7 +72,7 @@ public:
ProcessorForceTorquePreintPtr proc_ftpreint_;
VectorXd x_origin_;
MatrixXd P_origin_;
FrameBasePtr KF0_;
FrameBasePtr KF1_;
virtual void SetUp()
{
......@@ -88,11 +88,6 @@ public:
ceres_manager_ = std::make_shared<CeresManager>(problem_, ceres_options);
//===================================================== INITIALIZATION
x_origin_.resize(19);
x_origin_ << ZERO3, 0, 0, 0, 1, ZERO3, ZERO3, ZERO3, ZERO3;
P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
t0_.set(0.0);
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
intr_ik->pbc_noise_std = 0.1;
......@@ -218,23 +213,29 @@ public:
virtual void SetUp()
{
ProcessorForceTorquePreintImuOdom3dIkin2KF::SetUp();
t0_.set(0.0);
TimeStamp t1; t1.set(1);
TimeStamp t2; t2.set(2);
TimeStamp t3; t3.set(3);
TimeStamp t4; t4.set(3);
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
// - call setOrigin on processors isMotion since the flag prior_is_set_ is not true when doing installProcessor (later)
KF0_ = problem_->emplaceFrame(KEY, x_origin_, t0_);
x_origin_.resize(19);
x_origin_ << ZERO3, 0, 0, 0, 1, ZERO3, ZERO3, ZERO3, ZERO3;
P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
KF1_ = problem_->emplaceFrame(KEY, x_origin_, t0_);
// Prior pose factor
CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF0_, t0_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1_, t0_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
pose_prior_capture->emplaceFeatureAndFactor();
///////////////////////////////////////////////////
// Prior velocity factor
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0_);
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1_, "Vel0", t0_);
FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.segment<3>(7), P_origin_.block<3, 3>(6, 6));
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF1_->getV());
initializeData1();
......@@ -243,11 +244,11 @@ public:
// 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
// 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
// 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
proc_inkin_->keyFrameCallback(KF0_, 0.05);
proc_imu_->setOrigin(KF0_);
proc_inkin_->keyFrameCallback(KF1_, 0.05);
proc_imu_->setOrigin(KF1_);
auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0_, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin0->process();
proc_ftpreint_->setOrigin(KF0_);
proc_ftpreint_->setOrigin(KF1_);
problem_->setPriorIsSet(true); // We handle the whole initialization manually here
// T1
......@@ -270,14 +271,24 @@ public:
auto CFTpreint2 = std::make_shared<CaptureForceTorquePreint>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_);
CFTpreint2->process();
changeForData3();
// T3
CaptureImuPtr CImu3 = std::make_shared<CaptureImu>(t3, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
CImu3->process();
CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin3->process();
auto CFTpreint3 = std::make_shared<CaptureForceTorquePreint>(t3, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_);
auto CFTpreint3 = std::make_shared<CaptureForceTorquePreint>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
CFTpreint3->process();
// T4, just here to make sure the KF at t3 is created
CaptureImuPtr CImu4 = std::make_shared<CaptureImu>(t4, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
CImu4->process();
CaptureInertialKinematicsPtr CIKin4 = std::make_shared<CaptureInertialKinematics>(t4, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin4->process();
auto CFTpreint4 = std::make_shared<CaptureForceTorquePreint>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
CFTpreint4->process();
/////////////////////////////////////////////
// Add an Odom3D to make the Imu biases observable
setOdomData();
......@@ -286,14 +297,23 @@ public:
KF2_ = problem_->getTrajectory()->getLastKeyFrame();
CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF2_, KF2_->getTimeStamp(), nullptr, prev_pose_curr_, rel_pose_cov);
FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_curr_, rel_pose_cov);
FactorBase::emplace<FactorOdom3d>(ftr_odom3d_base, ftr_odom3d_base, KF0_, nullptr, false);
FactorBase::emplace<FactorOdom3d>(ftr_odom3d_base, ftr_odom3d_base, KF1_, nullptr, false);
////////////////////////////////////////////
// Add absolute factor on Imu biases to simulate a fix()
Eigen::Matrix6d Q_bi = 1e-8 * Eigen::Matrix6d::Identity();
CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF0_, "bi0", t0_);
CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1_, "bi0", t0_);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic()); }
FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic());
// Add loose absolute factor on initial bp bias since dynamical trajectories
// able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
Eigen::Matrix3d Q_bp = 1e-1 * Eigen::Matrix3d::Identity();
CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_);
FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_, Q_bp);
FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic());
}
virtual void initializeData1()
{
......@@ -312,6 +332,11 @@ public:
// DO NOTHING IN BASE CLASS
}
virtual void changeForData3()
{
// DO NOTHING IN BASE CLASS
}
virtual void setOdomData()
{
prev_pose_curr_ << ZERO6, 1;
......@@ -444,12 +469,49 @@ public:
acc_gyr_meas_[0] = ACC;
}
void changeForData3() override
{
// Not really necessary, just as a reminder that at this time, we start from a zero motion
zeroMotionBiasedData(sen_ft_->getMass(), bias_pbc_,
acc_gyr_meas_,
acc_gyr_cov_,
meas_ikin_,
Iq_,
Lq_,
cap_ftp_data_,
Qftp_);
// put seemingly realist values for a small/heavy humanoid robot
Vector3d pbc = PBCX + PBCY + PBCZ;
double h = 1.0; // height of the base
double L = 0.2; // horizontal dist base to feet (the 3 being in the coronal plane)
Vector3d pbl1; pbl1 << 0, L, -h;
Vector3d pbl2; pbl2 << 0, -L, -h;
Vector3d f1 = -MASS*gravity()/2;
Vector3d f2 = -MASS*gravity()/2;
// taus according to static Euler eq
Vector3d M_f1 = (pbl1 - pbc).cross(f1); // torque at C due to f1
Vector3d M_f2 = (pbl2 - pbc).cross(f2); // torque at C due to f2
Vector3d tau_to_apply = -(M_f1 + M_f2); // 0
// fill the inertial kin data vectors:
meas_ikin_.head<3>() = pbc + bias_pbc_;
cap_ftp_data_.segment<3>(12) = pbc + bias_pbc_;
cap_ftp_data_.segment<3>(0) = f1;
cap_ftp_data_.segment<3>(3) = f2;
// arbitrary choice: put equal pure torque on both feet
cap_ftp_data_.segment<3>(6) = tau_to_apply/2;
cap_ftp_data_.segment<3>(9) = tau_to_apply/2;
cap_ftp_data_.segment<3>(18) = pbl1;
cap_ftp_data_.segment<3>(21) = pbl2;
}
void setOdomData() override
{
// double dt = KF2_->getTimeStamp() - KF0_->getTimeStamp();
double dt = 1.0; // no acceleration for 1 sec then ACC for 1 second
prev_pose_curr_ << ACC*dt*dt/2, 0, 0, 0, 0, 0, 1;
// double dt = KF2_->getTimeStamp() - KF1_->getTimeStamp();
double dt = 1.0; // no acceleration for 1 sec then ACC for 1 second then no ACC -> V = ACC*dt
// prev_pose_curr_ << ACC*dt*dt/2, 0, 0, 0, 0, 0, 1;
prev_pose_curr_ << ACC*dt*dt/2 + (ACC*dt)*dt, 0, 0, 0, 0, 0, 1;
}
};
......@@ -514,12 +576,12 @@ public:
// // problem_->print(4,true,true,true);
// // std::cout << report << std::endl;
// ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF0_->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(KF0_->getStateBlock("C")->getState().head<2>(), ZERO3.head<2>(), 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("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);
......@@ -528,7 +590,7 @@ public:
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF0_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), ZERO3.head<2>(), 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);
// }
......@@ -540,12 +602,12 @@ public:
// // problem_->print(4,true,true,true);
// // std::cout << report << std::endl;
// ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF0_->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(KF0_->getStateBlock("C")->getState().head<2>(), ZERO3.head<2>(), 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("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
......@@ -554,7 +616,7 @@ public:
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// // Bias value on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF0_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// }
......@@ -572,17 +634,17 @@ public:
// // std::cout << H.rows() << " " << H.cols() << std::endl;
// // std::cout << H << std::endl;
// double dt = KF2_->getTimeStamp() - KF0_->getTimeStamp();
// double dt = KF2_->getTimeStamp() - KF1_->getTimeStamp();
// Eigen::Vector3d posi = ZERO3;
// posi[0] = ACC * dt * dt / 2;
// Eigen::Vector3d vel = ZERO3;
// vel[0] = ACC * dt;
// ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF0_->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(KF0_->getStateBlock("C")->getState().head<2>(), ZERO3.head<2>(), 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("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, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel, 1e-6);
......@@ -592,7 +654,7 @@ public:
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// // Bias value on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF0_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// }
......@@ -605,15 +667,19 @@ TEST_F(Cst2KFZeroMotionBiasPbcPbl, ZeroMotionBias)
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;
std::cout << "bp of KF1 and KF2" << std::endl;
std::cout << KF1_->getCaptureOf(sen_ikin_)->getState().transpose() << std::endl;
std::cout << KF2_->getCaptureOf(sen_ikin_)->getState().transpose() << std::endl;
ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF0_->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(KF0_->getStateBlock("C")->getState().head<2>(), (ZERO3 + PBCY + PBCZ).head<2>(), 1e-6);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), (PBCX + PBCY + PBCZ), 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head<2>(), (ZERO3 + PBCY + PBCZ).head<2>(), 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), (PBCX + PBCY + PBCZ), 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("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
......@@ -622,7 +688,7 @@ TEST_F(Cst2KFZeroMotionBiasPbcPbl, ZeroMotionBias)
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF0_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
}
......@@ -642,17 +708,17 @@ TEST_F(Cst2KFZeroMotionBiasPbcPbl, ZeroMotionBias)
// // std::cout << H.rows() << " " << H.cols() << std::endl;
// // std::cout << H << std::endl;
// double dt = KF2_->getTimeStamp() - KF0_->getTimeStamp();
// double dt = KF2_->getTimeStamp() - KF1_->getTimeStamp();
// Eigen::Vector3d posi = ZERO3;
// posi[0] = ACC * dt * dt / 2;
// Eigen::Vector3d vel = ZERO3;
// vel[0] = ACC * dt;
// ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF0_->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(KF0_->getStateBlock("C")->getState().head<2>(), ZERO3.head<2>(), 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("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, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel, 1e-6);
......@@ -662,7 +728,7 @@ TEST_F(Cst2KFZeroMotionBiasPbcPbl, ZeroMotionBias)
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// // Bias value on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF0_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head<2>(), BIAS_PBC_SMAL.head<2>(), 1e-6);
// }
......
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