Add method PrcFTI::emplaceFeaturesAndFactors()

include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h

@@ -242,6 +242,18 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
      */
     FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
 
+    /** \brief Emplace features and factors
+     * 
+     * This processor emplaces tree possible factors (with its features):
+     * - A Force-torque-inertial pre-integrated factor -- the motion factor
+     * - An Angular-momentum factor -- links angular momentum with angular velocity and inertia
+     * - An IMU bias drift factor -- this one only if the IMU biases are dynamic
+     * 
+     * \param _capture_origin: origin of the integrated delta
+     * \param _capture_own: final of the integrated delta
+     */
+    void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
     void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
 
     bool voteForKeyFrame() const override;

src/processor/processor_force_torque_inertial_preint_dynamics.cpp

@@ -34,6 +34,7 @@
 // wolf
 #include <core/state_block/state_composite.h>
 #include <core/capture/capture_motion.h>
+#include <core/factor/factor_block_difference.h>
 
 namespace wolf
 {
@@ -99,6 +100,62 @@ FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureB
     return fac_ftipd;
 }
 
+void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(CaptureBasePtr   _capture_origin,
+                                                                           CaptureMotionPtr _capture_own)
+{
+    // 1. Feature and factor FTI -- force-torque-inertial
+    //----------------------------------------------------
+    auto motion  = _capture_own->getBuffer().back();
+    auto ftr_fti = FeatureBase::emplace<FeatureMotion>(_capture_own,
+                                                   "FeatureMotion",
+                                                   motion.delta_integr_,
+                                                   motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
+                                                   _capture_own->getCalibrationPreint(),
+                                                   motion.jacobian_calib_);
+
+    FactorBase::emplace<FactorForceTorqueInertialDynamics>(
+        ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
+
+    // 2. Feature and factor L -- angular moment
+    //-------------------------------------------
+    // - feature has the current gyro measurement
+    // - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
+
+    // 3. Feature and factor bias -- IMU bias drift for acc and gyro
+    //---------------------------------------------------------------
+    // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
+    // - feature has zero measurement size 6, with the cov of the bias drift size 6x6
+    // - factor relates bias(last capture) and bias(origin capture)
+    if (getSensor()->isStateBlockDynamic('I'))
+    {
+        const auto& sb_imubias_own    = _capture_own->getStateBlock('I');
+        const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+        if (sb_imubias_own != sb_imubias_origin)  // make sure it's two different state blocks! -- just in case
+        {
+            auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+            auto ftr_bias =
+                FeatureBase::emplace<FeatureBase>(_capture_own,
+                                                  "FeatureBase",
+                                                  Vector6d::Zero(),      // mean IMU drift is zero
+                                                  imu_drift_cov_ * dt);  // IMU drift cov specified in continuous time
+            FactorBase::emplace<FactorBlockDifference>(ftr_bias,
+                                                       ftr_bias,
+                                                       sb_imubias_own,      // IMU bias block at t=own
+                                                       sb_imubias_origin,   // IMU bias block at t=origin
+                                                       nullptr,             // frame other
+                                                       _capture_origin,     // origin capture
+                                                       nullptr,             // feature other
+                                                       nullptr,             // landmark other
+                                                       0,                   // take all of first state block
+                                                       -1,                  // take all of first state block
+                                                       0,                   // take all of first second block
+                                                       -1,                  // take all of first second block
+                                                       shared_from_this(),  // this processor
+                                                       false);              // loss function
+        }
+    }
+}
+
 void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture,
                                                                 const VectorXd&      _calibration)
 {