diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h index 3df1c33698fc5bdf18d16e31a592f8df79913773..cc64c684f6d68e24d9bdffc3303b7f3fe329f17d 100644 --- a/include/bodydynamics/factor/factor_force_torque.h +++ b/include/bodydynamics/factor/factor_force_torque.h @@ -97,6 +97,7 @@ FactorForceTorque::FactorForceTorque( bool _apply_loss_function, FactorStatus _status) : FactorAutodiff("FactorForceTorque", + _feat, _frame_other, // _frame_other_ptr nullptr, // _capture_other_ptr nullptr, // _feature_other_ptr diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h index a26127e22973469d340930d9ce301843fcee7a12..c1c193692526129966c8d6eac3c586111e7b7603 100644 --- a/include/bodydynamics/factor/factor_force_torque_preint.h +++ b/include/bodydynamics/factor/factor_force_torque_preint.h @@ -161,6 +161,7 @@ inline FactorForceTorquePreint::FactorForceTorquePreint( FactorStatus _status) : FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>( "FactorForceTorquePreint", + _ftr_ptr, _cap_origin_ptr->getFrame(), _cap_origin_ptr, nullptr, diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h index 5fe91e4de7680669c2f24d005c5954f88b388e18..f39e0f7d160942e5df2f9c2c427fe0ece39789b8 100644 --- a/include/bodydynamics/factor/factor_inertial_kinematics.h +++ b/include/bodydynamics/factor/factor_inertial_kinematics.h @@ -68,6 +68,7 @@ FactorInertialKinematics::FactorInertialKinematics( bool _apply_loss_function, FactorStatus _status) : FactorAutodiff("FactorInertialKinematics", + _feat, nullptr, // _frame_other_ptr nullptr, // _capture_other_ptr nullptr, // _feature_other_ptr diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp index 20f54d5eb0249590f66532a8669fed8619e5ab98..192b34b03298b1e49070c0d267bb7cd54bcf4d31 100644 --- a/src/processor/processor_inertial_kinematics.cpp +++ b/src/processor/processor_inertial_kinematics.cpp @@ -133,7 +133,7 @@ inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureI StateBlockPtr sb_pbc1 = _cap_ikin_origin->getSensorIntrinsic(); StateBlockPtr sb_pbc2 = _cap_ikin->getSensorIntrinsic(); FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( - feat_diff, sb_pbc1, sb_pbc2, nullptr, _cap_ikin_origin + feat_diff, feat_diff, sb_pbc1, sb_pbc2, nullptr, _cap_ikin_origin ); } diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp index e9b93e8271ff31c482655b664f71f3941dce4106..8902156fef0fee818dd1ff4c6e24ba7946e10c14 100644 --- a/test/gtest_factor_inertial_kinematics.cpp +++ b/test/gtest_factor_inertial_kinematics.cpp @@ -76,6 +76,7 @@ class FactorInertialKinematics_1KF : public testing::Test Eigen::Matrix3d Qp_, Qv_, Qw_; Eigen::Vector3d bias_p_, bias_imu_; FeatureInertialKinematicsPtr feat_in_; + StateBlockPtr sbbimu_; virtual void SetUp() { @@ -118,12 +119,12 @@ class FactorInertialKinematics_1KF : public testing::Test StateBlockPtr sbd = make_shared<StateBlock>(zero3); StateBlockPtr sbL = make_shared<StateBlock>(zero3); StateBlockPtr sbb = make_shared<StateBlock>(bias_p_); - StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu_); + sbbimu_ = make_shared<StateBlock>(bias_imu_); KF0_->addStateBlock("C", sbc, problem_); KF0_->addStateBlock("D", sbd, problem_); KF0_->addStateBlock("L", sbL, problem_); - KF0_->addStateBlock("I", sbbimu, problem_); // IMU bias + KF0_->addStateBlock("I", sbbimu_, problem_); // IMU bias // Fix the one we cannot estimate KF0_->getStateBlock("I")->fix(); // Imu @@ -142,12 +143,9 @@ class FactorInertialKinematics_1KF : public testing::Test Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity(); Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity(); - // =============== CREATE CAPURE/FEATURE/FACTOR + // =============== CREATE CAPURE Vector9d meas_ikin0; meas_ikin0 << pBC_meas, vBC_meas, w_meas; CIK0_ = CaptureBase::emplace<CaptureInertialKinematics>(KF0_, t_, SIK_, meas_ikin0, Iq, Lq, bias_p_); - Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas_ikin0.head<3>(), meas_ikin0.tail<3>(), Iq); - feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas_ikin0, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); - auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu, nullptr, false); } virtual void TearDown(){} @@ -172,11 +170,10 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics Eigen::Matrix3d Iq; Iq.setIdentity(); Eigen::Vector3d Lq = zero3; - feat_in_->setMeasurement(meas); + // =============== 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); - feat_in_->setMeasurementCovariance(Q_ikin_err); - feat_in_->setBIq(Iq); - feat_in_->setBLcm(Lq); + feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); + auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false); } }; @@ -198,11 +195,10 @@ class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1K Eigen::Matrix3d Iq; Iq.setIdentity(); Eigen::Vector3d Lq = zero3; - feat_in_->setMeasurement(meas); + // =============== 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); - feat_in_->setMeasurementCovariance(Q_ikin_err); - feat_in_->setBIq(Iq); - feat_in_->setBLcm(Lq); + feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); + auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false); } }; @@ -225,11 +221,10 @@ class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1K Eigen::Matrix3d Iq; Iq.setIdentity(); Eigen::Vector3d Lq = zero3; - feat_in_->setMeasurement(meas); + // =============== 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); - feat_in_->setMeasurementCovariance(Q_ikin_err); - feat_in_->setBIq(Iq); - feat_in_->setBLcm(Lq); + feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); + auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false); } }; @@ -253,11 +248,10 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF Eigen::Matrix3d Iq; Iq.setIdentity(); Eigen::Vector3d Lq = zero3; - feat_in_->setMeasurement(meas); + // =============== 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); - feat_in_->setMeasurementCovariance(Q_ikin_err); - feat_in_->setBIq(Iq); - feat_in_->setBLcm(Lq); + feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); + auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false); } }; @@ -366,7 +360,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF // CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(KF0_, "CaptureInertialKinematics", t_, nullptr); // auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); // feat_in_ = static_pointer_cast<FeatureInertialKinematics>(feat); // !! auto feat_in_ creates no compilation error but a segfault -// // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu, nullptr, false); +// // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false); // } // // virtual void TearDown(){} diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp index 50f96e712de13d3d923461eed39bbbd558a0643a..937b8d3fec151435ce68702a9ef88beece035ba4 100644 --- a/test/gtest_processor_force_torque_preint.cpp +++ b/test/gtest_processor_force_torque_preint.cpp @@ -204,7 +204,7 @@ public: // Prior velocity factor 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, KF1_->getV(), nullptr, false); + FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1_->getV(), nullptr, false); initializeData1(); @@ -272,7 +272,7 @@ public: Matrix6d Q_bi = 1e-8 * Matrix6d::Identity(); CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1_, "bi0", t0_); FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, 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 // able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest @@ -280,7 +280,7 @@ public: CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_); FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_, Q_bp); // FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ + (Vector3d()<<0.1,0.1,0.1).finished(), 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); } diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp index 89e2c81bd873a3c8edc2e7a10200e3b7894537ab..f43d047d04c7806ff186b0d849f9e89342d01ce4 100644 --- a/test/gtest_processor_inertial_kinematics.cpp +++ b/test/gtest_processor_inertial_kinematics.cpp @@ -113,7 +113,7 @@ class FactorInertialKinematics_2KF : public testing::Test // Prior velocity factor CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_); 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(), nullptr, false); + FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF0_->getV(), nullptr, false); // Set origin of processor Imu std::static_pointer_cast<ProcessorImu>(proc)->setOrigin(KF0_); @@ -122,7 +122,7 @@ class FactorInertialKinematics_2KF : public testing::Test Matrix6d Q_bi = 1e-8 * Matrix6d::Identity(); CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF0_, "bi0", t_); FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi); - FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false); + FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false); }