New factor angular momentum and new emplace at the processor

include/bodydynamics/factor/factor_angular_momentum.h

@@ -48,12 +48,12 @@ WOLF_PTR_TYPEDEFS(FactorAngularMomentum);
  *    'i' inertia matrix components (we are assuming that it is a diagonal matrix)
  *
  * The residual computed has the following components, in this order
- *  - angular velocity error according to FT preintegration
+ *  - angular velocity error
  */
-class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>  // State Blocks: L, I, i
+class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3>  // State Blocks: L, I, i
 {
   public:
-    FactorAngularMomentum(const FeatureMotionPtr& _ftr,        // gets measurement and access to parents
+    FactorAngularMomentum(const FeatureBasePtr&   _ftr,        // gets measurement and access to parents
                           const ProcessorBasePtr& _processor,  // gets access to processor and sensor
                           bool                    _apply_loss_function,
                           FactorStatus            _status = FAC_ACTIVE);
@@ -75,39 +75,39 @@ class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3,
     Vector3d residual() const;
 
   private:
-    Matrix<double, 12, 1> data;
-    Matrix<double, 3, 3> sqrt_info_upper_;
+    Vector3d measurement_ang_vel_;  // gyroscope measurement data
+    Matrix3d sqrt_info_upper_;
 };
 
-inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
+inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr&   _ftr,
                                                     const ProcessorBasePtr& _processor,
-                                                    bool         _apply_loss_function,
-                                                    FactorStatus _status)
-    : FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>(
+                                                    bool                    _apply_loss_function,
+                                                    FactorStatus            _status)
+    : FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3>(
           "FactorAngularMomentum",
           TOP_MOTION,
-          _ftr,                         // parent feature
-          nullptr,                 // frame other
-          nullptr,              // capture other
-          nullptr,                      // feature other
-          nullptr,                      // landmark other
-          _processor,                   // processor
+          _ftr,        // parent feature
+          nullptr,     // frame other
+          nullptr,     // capture other
+          nullptr,     // feature other
+          nullptr,     // landmark other
+          _processor,  // processor
           _apply_loss_function,
           _status,
-          _ftr->getFrame()->getStateBlock('L'),    // previous frame L
-          _capture_origin->getStateBlock('I'),                // previous frame IMU bias
-          _processor->getSensor()->getStateBlock('i'),   // sensor i
-          data(ftr->getMeasurement()),
-          sqrt_info_upper_(_processor->getSensor()->getNoiseCov().block<3,3>(3,3)))
+          _ftr->getFrame()->getStateBlock('L'),          // current linear momentum
+          _ftr->getCapture()->getStateBlock('I'),        // IMU bias
+          _processor->getSensor()->getStateBlock('i')),  // moment of inertia
+      measurement_ang_vel_(_ftr->getMeasurement()),
+      sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper())  // Buscar funcio correcte
 {
     //
 }
 
 template <typename D1, typename D2, typename D3, typename D4>
-inline bool FactorAngularMomentum::residual(const MatrixBase<D1>&     _L,
-                                            const MatrixBase<D2>&     _I,
-                                            const MatrixBase<D3>&     _i,
-                                            MatrixBase<D4>&           _res) const
+inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
+                                            const MatrixBase<D2>& _I,
+                                            const MatrixBase<D3>& _i,
+                                            MatrixBase<D4>&       _res) const
 {
     MatrixSizeCheck<3, 1>::check(_res);
 
@@ -126,16 +126,18 @@ inline bool FactorAngularMomentum::residual(const MatrixBase<D1>&     _L,
      *    res = W * err , with W the sqrt of the info matrix.
      */
 
-    Matrix<T, 3, 1> w_real = data.segment<3>(3) - _I.segment<3>(3);
-    double Lx = _L(0);
-    double Ly = _L(1);
-    double Lz = _L(2);
-    double ixx = _i(0);
-    double iyy = _i(1);
-    double izz = _i(2);
-    Matrix<T, 3, 1> w_est = [Lx/ixx, Ly/iyy, Lz/izz];
-    Matrix<T, 3, 1> err   = w_real - w_est;
-    _res                  = sqrt_info_upper_ * err;
+    typedef typename D4::Scalar T;
+
+    auto            w_real = measurement_ang_vel_ - _I.segment<3>(3);
+    const auto&     Lx     = _L(0);
+    const auto&     Ly     = _L(1);
+    const auto&     Lz     = _L(2);
+    const auto&     ixx    = _i(0);
+    const auto&     iyy    = _i(1);
+    const auto&     izz    = _i(2);
+    Matrix<T, 3, 1> w_est(Lx / ixx, Ly / iyy, Lz / izz);
+    Matrix<T, 3, 1> err = w_real - w_est;
+    _res                = sqrt_info_upper_ * err;
 
     return true;
 }
@@ -144,23 +146,20 @@ inline Vector3d FactorAngularMomentum::residual() const
 {
     Vector3d res(3);
     Vector3d L = getFrame()->getStateBlock('L')->getState();
-    Vector6d I = getFeature()->getCapture()->getSensor()->getState();
-    Vector3d i = getFeature()->getSensor()->getStateBlock('i')->getState();
+    Vector6d I = getCapture()->getStateBlock('I')->getState();
+    Vector3d i = getSensor()->getStateBlock('i')->getState();
 
     residual(L, I, i, res);
     return res;
 }
 
 template <typename T>
-inline bool FactorAngularMomentum::operator()(const T* const _L,
-                                              const T* const _I,
-                                              const T* const _i,
-                                              T*             _res) const
+inline bool FactorAngularMomentum::operator()(const T* const _L, const T* const _I, const T* const _i, T* _res) const
 {
     Map<const Matrix<T, 3, 1>> L(_L);
     Map<const Matrix<T, 6, 1>> I(_I);
     Map<const Matrix<T, 3, 1>> i(_i);
-    Map<Matrix<T, 3, 1>>      res(_res);
+    Map<Matrix<T, 3, 1>>       res(_res);
 
     residual(L, I, i, res);
 

src/processor/processor_force_torque_inertial_dynamics.cpp

@@ -28,6 +28,7 @@
 
 // bodydynamics
 #include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/factor/factor_angular_momentum.h"
 #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
 #include "bodydynamics/math/force_torque_inertial_delta_tools.h"
 #include "bodydynamics/capture/capture_force_torque_inertial.h"
@@ -98,8 +99,13 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
     // - feature has the current gyro measurement
     // - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
 
-    // auto measurement_gyro = motion.data_.segment<3>(3);
-    // auto meas_cov = sensor_fti_->getNoiseCov().block<3,3>(3,3); // ??
+    auto measurement_gyro = motion.data_.segment<3>(3);
+    auto gyro_cov         = sensor_fti_->getNoiseCov().block<3, 3>(3, 3);
+
+    auto ftr_base = FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase", measurement_gyro, gyro_cov);
+
+    FactorBase::emplace<FactorAngularMomentum>(
+        ftr_base, ftr_base, _capture_own->getSensor()->getProcessorList().back(), params_->apply_loss_function);
 
     // 3. Feature and factor bias -- IMU bias drift for acc and gyro
     //---------------------------------------------------------------