working again (except gtest_processor_inertial_kinematics)

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

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,

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

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
         );
     }
 

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(){}

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);
  
     }
 

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);
 
     }