Skip to content
Snippets Groups Projects
Commit c444292a authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge remote-tracking branch 'origin/devel' into 4-migrate-to-state-composites

parents d5019fab 7022b8c8
No related branches found
No related tags found
3 merge requests!18Release after RAL,!17After 2nd RAL submission,!4Resolve "Migrate to state composites"
...@@ -441,7 +441,7 @@ int main (int argc, char **argv) { ...@@ -441,7 +441,7 @@ int main (int argc, char **argv) {
// Prior velocity factor // Prior velocity factor
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1, "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)); 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, KF1->getV(), nullptr, false); FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1->getV(), nullptr, false);
// Special trick to make things work in the IMU + IKin + FTPreint case // Special trick to make things work in the IMU + IKin + FTPreint case
// 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0 // 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0
...@@ -467,14 +467,14 @@ int main (int argc, char **argv) { ...@@ -467,14 +467,14 @@ int main (int argc, char **argv) {
Matrix6d Q_bi = 1e-8 * Matrix6d::Identity(); Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0); CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", Vector6d::Zero(), Q_bi); FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", Vector6d::Zero(), Q_bi);
FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false); FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
// Add loose absolute factor on initial bp bias since dynamical trajectories // 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 // able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
Matrix3d Q_bp = pow(std_abs_biasp, 2)* Matrix3d::Identity(); Matrix3d Q_bp = pow(std_abs_biasp, 2)* Matrix3d::Identity();
CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0); CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp); FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp);
FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false); FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
/////////////////////////////////////////// ///////////////////////////////////////////
......
...@@ -541,20 +541,20 @@ public: ...@@ -541,20 +541,20 @@ public:
TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias) TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
{ {
// TEST FIRST PURE INTEGRATION // // TEST FIRST PURE INTEGRATION
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 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("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->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("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->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("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->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("L")->getState(), ZERO3, 1e-6);
// THEN SOLVE // THEN SOLVE
problem_->perturb(); problem_->perturb();
...@@ -612,19 +612,19 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias) ...@@ -612,19 +612,19 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2); posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3; Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT; vel_diff[0] = ACC * 3*DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 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("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->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("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_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); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("C")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_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("L")->getState(), ZERO3, 1e-6);
problem_->perturb(); problem_->perturb();
...@@ -657,19 +657,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias) ...@@ -657,19 +657,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2); posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3; Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT; vel_diff[0] = ACC * 3*DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 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("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("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("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->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("P")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_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 // // 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("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("D")->getState(), vel_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
problem_->perturb(); problem_->perturb();
...@@ -758,20 +758,20 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias) ...@@ -758,20 +758,20 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
} }
TEST_F(Cst2KFXaxisRotationPureTorque, RotationOnlyNoSolve) // TEST_F(Cst2KFXaxisRotationPureTorque, RotationOnlyNoSolve)
{ // {
Vector3d Lc_diff; Lc_diff << M_PI, 0, 0; // Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
Vector4d q_diff; q_diff << 1,0,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("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->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("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("O")->getState(), q_diff, 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(KF2_->getStateBlock("L")->getState(), Lc_diff, 1e-6);
} // }
int main(int argc, char **argv) int main(int argc, char **argv)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment