diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
index f747d00029bd64925b8d7edb5fda01b6508ebb5e..8bcc1608659f2a373282c323099bdd38dc8d0666 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/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;
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 707cb405da41324703e49c8eaf834e3c1c288e8f..220ab7d8623c99c6fa97ecac998e3d0ea58493fb 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/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)
{