Add new files

.clang-format copy

+---
+Language: Cpp
+BasedOnStyle: Google
+IndentAccessModifiers: false
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: true
+AlignEscapedNewlines: Left
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: true
+BinPackParameters: false
+BreakBeforeBraces: Custom
+BraceWrapping:
+  AfterClass: true
+  AfterControlStatement: Always
+  AfterEnum: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterObjCDeclaration: true
+  AfterStruct: true
+  AfterUnion: true
+  AfterExternBlock: false
+  BeforeCatch: true
+  BeforeElse: true
+  IndentBraces: false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit: 119
+# CommentPragmas: "^ IWYU pragma: ^\\.+"
+CommentPragmas:  '^\\.+'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks: Preserve
+# IncludeCategories:
+#   - Regex: '^<pinocchio/fwd\.hpp>'
+#     Priority: 1
+#   - Regex: '^<ext/.*\.h>'
+#     Priority: 3
+#   - Regex: '^<.*\.h>'
+#     Priority: 2
+#   - Regex: "^<.*"
+#     Priority: 3
+#   - Regex: ".*"
+#     Priority: 4
+IncludeIsMainRegex: "([-_](test|unittest))?$"
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+TabWidth: 4
+UseTab: Never

include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H
+#define FACTOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/feature/feature_motion.h>
+#include <core/factor/factor_autodiff.h>
+
+namespace wolf
+{
+using namespace Eigen;
+using namespace bodydynamics;
+
+WOLF_PTR_TYPEDEFS(FactorForceTorqueInertialDynamics);
+
+/**
+ * @brief Pre-integrated factor taking IMU and force-torque measurements (FTI)
+ *
+ * This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum.
+ *
+ * State blocks considered:
+ *  - Frame 1 (origin)
+ *    'P' position
+ *    'V' velocity
+ *    'O' orientation
+ *    'L' angular momentum
+ *  - Capture 1 (origin)
+ *    'I' IMU biases
+ *  - Frame 2
+ *    'P' position
+ *    'V' velocity
+ *    'O' orientation
+ *    'L' angular momentum
+ *  - Sensor Force Torque Inertial
+ *    'C' position of the center of mass
+ *    'i' inertia matrix components (we are assuming that it is a diagonal matrix)
+ *    'm' mass
+ *
+ * The residual computed has the following components, in this order
+ *  - position delta error according to IMU preintegration
+ *  - velocity delta error according to IMU preintegration
+ *  - position delta error according to FT preintegration
+ *  - velocity delta error according to FT preintegration
+ *  - angular momentum delta error according to FT preintegration
+ *  - orientation delta error according to IMU preintegration
+ */
+class FactorForceTorqueInertialDynamics
+    : public FactorAutodiff<FactorForceTorqueInertialDynamics, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1>  // POVLB - POVL
+                                                                                                        // - Cim
+                                                                                                        // // TODO: add
+                                                                                                        // FT bias?
+{
+  public:
+    FactorForceTorqueInertialDynamics(const FeatureMotionPtr& _ftr,  // gets measurement and access to parents
+                                      const CaptureBasePtr&   _capture_origin,  // gets biases
+                                      const ProcessorBasePtr& _processor,       // gets access to processor and sensor
+                                      bool                    _apply_loss_function,
+                                      FactorStatus            _status = FAC_ACTIVE);
+
+    ~FactorForceTorqueInertialDynamics() override = default;
+
+    template <typename T>
+    bool operator()(const T* const _p1,  // position 1
+                    const T* const _q1,  // orientation
+                    const T* const _v1,  // velocity
+                    const T* const _L1,  // ang momentum
+                    const T* const _b1,  // IMU bias (acc and gyro)
+                    const T* const _p2,  // position 2
+                    const T* const _q2,  // orientation
+                    const T* const _v2,  // velocity
+                    const T* const _L2,  // ang momentum
+                    const T* const _C,   // center of mass
+                    const T* const _i,   // inertia matrix
+                    const T* const _m,   // mass
+                    T*             _res) const;      // residual
+
+    template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8>
+    bool residual(const MatrixBase<D1>&     _p1,
+                  const QuaternionBase<D4>& _q1,
+                  const MatrixBase<D1>&     _v1,
+                  const MatrixBase<D1>&     _L1,
+                  const MatrixBase<D2>&     _b1,
+                  const MatrixBase<D3>&     _p2,
+                  const QuaternionBase<D5>& _q2,
+                  const MatrixBase<D3>&     _v2,
+                  const MatrixBase<D3>&     _L2,
+                  const MatrixBase<D6>&     _C,
+                  const MatrixBase<D6>&     _i,
+                  const D7&                 _m,
+                  MatrixBase<D8>&           _res) const;
+
+    VectorXd residual() const;
+
+    VectorXd getCalibPreint() const
+    {
+        return calib_preint_;
+    }
+
+  private:
+    double                 dt_;
+    Matrix<double, 19, 1>  delta_preint_;
+    Matrix<double, 13, 1>  calib_preint_;
+    Matrix<double, 18, 13> J_delta_calib_;
+    Matrix<double, 18, 18> sqrt_info_upper_;
+};
+
+inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(const FeatureMotionPtr& _ftr,
+                                                                            const CaptureBasePtr&   _capture_origin,
+                                                                            const ProcessorBasePtr& _processor,
+                                                                            bool         _apply_loss_function,
+                                                                            FactorStatus _status)
+    : FactorAutodiff<FactorForceTorqueInertialDynamics, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1>(
+          "FactorForceTorqueInertialDynamics",
+          TOP_MOTION,
+          _ftr,                         // parent feature
+          _capture_origin->getFrame(),  // frame other
+          _capture_origin,              // capture other
+          nullptr,                      // feature other
+          nullptr,                      // landmark other
+          _processor,                   // processor
+          _apply_loss_function,
+          _status,
+          _capture_origin->getFrame()->getStateBlock('P'),    // previous frame P
+          _capture_origin->getFrame()->getStateBlock('O'),    // previous frame O
+          _capture_origin->getFrame()->getStateBlock('V'),    // previous frame V
+          _capture_origin->getFrame()->getStateBlock('L'),    // previous frame L
+          _capture_origin->getStateBlock('I'),                // previous frame IMU bias
+          _ftr->getFrame()->getStateBlock('P'),               // current frame P
+          _ftr->getFrame()->getStateBlock('O'),               // current frame O
+          _ftr->getFrame()->getStateBlock('V'),               // current frame V
+          _ftr->getFrame()->getStateBlock('L'),               // current frame L
+          _capture_origin->getSensor()->getStateBlock('C'),   // sensor C
+          _capture_origin->getSensor()->getStateBlock('i'),   // sensor i
+          _capture_origin->getSensor()->getStateBlock('m')),  // sensor m
+      dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
+      delta_preint_(_ftr->getDeltaPreint()),
+      calib_preint_(_ftr->getCalibrationPreint()),
+      J_delta_calib_(_ftr->getJacobianCalibration()),
+      sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper())
+{
+    //
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8>
+inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>&     _p1,
+                                                        const QuaternionBase<D4>& _q1,
+                                                        const MatrixBase<D1>&     _v1,
+                                                        const MatrixBase<D1>&     _L1,
+                                                        const MatrixBase<D2>&     _b1,
+                                                        const MatrixBase<D3>&     _p2,
+                                                        const QuaternionBase<D5>& _q2,
+                                                        const MatrixBase<D3>&     _v2,
+                                                        const MatrixBase<D3>&     _L2,
+                                                        const MatrixBase<D6>&     _C,
+                                                        const MatrixBase<D6>&     _i,
+                                                        const D7&                  _m,
+                                                        MatrixBase<D8>&           _res) const
+{
+    MatrixSizeCheck<18, 1>::check(_res);
+
+    typedef typename D4::Scalar  T;
+    typedef Map<Matrix<T, 3, 1>> Matrixt;
+
+    /* Will do the following:
+     *
+     * Compute estimated delta, using betweenStates()
+     *    d_est = x2 (-) x1
+     *
+     * Compute correction step according to bias change, this is a linear operation
+     *    d_step = J_delta_calib * (calib - bias_preint)
+     *
+     * Correct preintegrated delta with new bias, using plus()
+     *    d_corr = d_preint (+) d_step
+     *
+     * Compute error, using diff()
+     *    d_err = d_corr (-) d_est
+     *
+     * Compute residual
+     *    res = W * d_err , with W the sqrt of the info matrix.
+     */
+
+    /* WARNING -- order of state blocks might be confusing!!
+     * Frame blocks are ordered as follows:  "POVL"         -- pos, ori, vel, ang_momentum
+     * Factor blocks are ordered as follows: "POVL-B-POVL"  -- B is IMU bias
+     * Delta blocks are ordered as follows:  "PVpvLO"       -- where "P,V,O" are from IMU, and "p,v,L" from FT
+     */
+
+    Matrix<double, 3, 1> f;
+    Matrix<T, 19, 1>     delta_est;           // delta_est = x2 (-) x1, computed with betweenStates(x1,x2,dt)
+    Matrixt              dpi(&delta_est(0));  // 'P'
+    Matrixt              dvi(&delta_est(3));  // 'V'
+    Matrixt              dpd(&delta_est(6));  // 'p'
+    Matrixt              dvd(&delta_est(9));  // 'v'
+    Matrixt              dL(&delta_est(12));  // 'L'
+    Map<Quaternion<T>>   dq(&delta_est(15));  // 'O'
+    fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
+
+    Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
+    Matrix<T, 13, 1> calib;
+    calib << _b1, _C, _i, _m;
+    Matrix<T, 18, 1> delta_step = J_delta_calib_ * (calib - calib_preint_);
+    Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
+    Matrix<T, 18, 1> delta_err  = fti::diff(delta_est, delta_corr);
+    _res                        = sqrt_info_upper_ * delta_err;
+
+    return true;
+}
+
+inline VectorXd FactorForceTorqueInertialDynamics::residual() const
+{
+    VectorXd res(18);
+    Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P
+    Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
+    Vector3d v0 = getFrameOther()->getStateBlock('V')->getState();
+    Vector3d L0 = getFrameOther()->getStateBlock('L')->getState();
+    Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
+    Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P
+    Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
+    Vector3d v1 = getFrame()->getStateBlock('V')->getState();
+    Vector3d L1 = getFrame()->getStateBlock('L')->getState();
+    Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState();
+    Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState();
+    double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0);
+
+    residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res); 
+    return res;
+}
+
+template <typename T>
+inline bool FactorForceTorqueInertialDynamics::operator()(const T* const _p1,
+                                                          const T* const _q1,
+                                                          const T* const _v1,
+                                                          const T* const _L1,
+                                                          const T* const _b1,
+                                                          const T* const _p2,
+                                                          const T* const _q2,
+                                                          const T* const _v2,
+                                                          const T* const _L2,
+                                                          const T* const _C,
+                                                          const T* const _i,
+                                                          const T* const _m,
+                                                          T* _res) const
+{
+    Map<const Matrix<T, 3, 1>> p1(_p1);
+    Map<const Quaternion<T>>   q1(_q1);
+    Map<const Matrix<T, 3, 1>> v1(_v1);
+    Map<const Matrix<T, 3, 1>> L1(_L1);
+    Map<const Matrix<T, 6, 1>> b1(_b1);
+    Map<const Matrix<T, 3, 1>> p2(_p2);
+    Map<const Quaternion<T>>   q2(_q2);
+    Map<const Matrix<T, 3, 1>> v2(_v2);
+    Map<const Matrix<T, 3, 1>> L2(_L2);
+    Map<const Matrix<T, 3, 1>> C(_C);
+    Map<const Matrix<T, 3, 1>> i(_i);
+    const T&                   m = *_m;
+    Map<Matrix<T, 18, 1>>      res(_res);
+
+    residual(p1, q1, v1, L1, b1, p2, q2, v2, L2, C, i, m, res);
+
+    return true;
+}
+
+}  // namespace wolf
+
+#endif  // FACTOR_FORCE_TORQUE_INERTIAL_H
\ No newline at end of file

include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_preintegrator_force_torque_inertial.h
+ *
+ *  Created on: Aug 19, 2021
+ *      Author: jsola
+ */
+
+#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
+#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+
+#include <core/processor/processor_motion.h>
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialPreintDynamics);
+
+struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessorMotion
+{
+    ParamsProcessorForceTorqueInertialPreintDynamics() = default;
+    ParamsProcessorForceTorqueInertialPreintDynamics(std::string _unique_name, const ParamsServer& _server)
+        : ParamsProcessorMotion(_unique_name, _server)
+    {
+        // n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
+    }
+
+    std::string print() const override
+    {
+        return ParamsProcessorMotion::print() + "\n";               //
+              //  + "n_propellers: " + std::to_string(n_propellers);  //
+    }
+
+    // int n_propellers;
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreintDynamics);
+class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
+{
+  public:
+    ProcessorForceTorqueInertialPreintDynamics(
+        ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics_);
+    ~ProcessorForceTorqueInertialPreintDynamics() override;
+    void configure(SensorBasePtr _sensor) override;
+    WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics, ParamsProcessorForceTorqueInertialPreintDynamics);
+
+  protected:
+    ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_dynamics_;
+    SensorForceTorqueInertialPtr                sensor_fti_;
+
+  public:
+    /** \brief convert raw CaptureMotion data to the delta-state format.
+     *
+     * This function accepts raw data and time step dt,
+     * and computes the value of the delta-state and its covariance. Note that these values are
+     * held by the members delta_ and delta_cov_.
+     *
+     * @param _data measured motion data
+     * @param _data_cov covariance
+     * @param _dt time step
+     * @param _delta computed delta
+     * @param _delta_cov covariance
+     * @param _calib current state of the calibrated parameters
+     * @param _jacobian_calib Jacobian of the delta wrt calib
+     *
+     * Rationale:
+     *
+     * The delta-state format must be compatible for integration using
+     * the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
+     * See the class documentation for some Eigen::VectorXd suggestions.
+     *
+     * The data format is generally not the same as the delta format,
+     * because it is the format of the raw data provided by the Capture,
+     * which is unaware of the needs of this processor.
+     *
+     * Additionally, sometimes the data format is in the form of a
+     * velocity, or a higher derivative, while the delta is in the form of an increment.
+     * In such cases, converting from data to delta-state needs integrating
+     * the data over the period dt.
+     *
+     * Two trivial implementations would establish:
+     *  - If `_data` is an increment:
+     *
+     *         delta_     = _data;
+     *         delta_cov_ = _data_cov
+     *  - If `_data` is a velocity:
+     *
+     *         delta_     = _data * _dt
+     *         delta_cov_ = _data_cov * _dt.
+     *
+     *  However, other more complicated relations are possible.
+     *  In general, we'll have a nonlinear
+     *  function relating `delta_` to `_data` and `_dt`, as follows:
+     *
+     *      delta_ = f ( _data, _dt)
+     *
+     *  The delta covariance is obtained by classical uncertainty propagation of the data covariance,
+     *  that is, through the Jacobians of `f()`,
+     *
+     *      delta_cov_ = F_data * _data_cov * F_data.transpose
+     *
+     *  where `F_data = d_f/d_data` is the Jacobian of `f()`.
+     */
+    void computeCurrentDelta(const Eigen::VectorXd& _data,
+                             const Eigen::MatrixXd& _data_cov,
+                             const Eigen::VectorXd& _calib,
+                             const double           _dt,
+                             Eigen::VectorXd&       _delta,
+                             Eigen::MatrixXd&       _delta_cov,
+                             Eigen::MatrixXd&       _jacobian_calib) const override;
+
+    /** \brief composes a delta-state on top of another delta-state
+     * \param _delta1 the first delta-state
+     * \param _delta2 the second delta-state
+     * \param _dt2 the second delta-state's time delta
+     * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+     *
+     * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
+     */
+    void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                        const Eigen::VectorXd& _delta2,
+                        const double           _dt2,
+                        Eigen::VectorXd&       _delta1_plus_delta2) const override;
+
+    /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
+     * \param _delta1 the first delta-state
+     * \param _delta2 the second delta-state
+     * \param _dt2 the second delta-state's time delta
+     * \param _delta1_plus_delta2 the delta2 composed on top of delta1. It has the format of delta-state.
+     * \param _jacobian1 the jacobian of the composition w.r.t. _delta1.
+     * \param _jacobian2 the jacobian of the composition w.r.t. _delta2.
+     *
+     * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its
+     * jacobians.
+     */
+    void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                        const Eigen::VectorXd& _delta2,
+                        const double           _dt2,
+                        Eigen::VectorXd&       _delta1_plus_delta2,
+                        Eigen::MatrixXd&       _jacobian1,
+                        Eigen::MatrixXd&       _jacobian2) const override;
+
+    /** \brief composes a delta-state on top of a state
+     * \param _x the initial state
+     * \param _delta the delta-state
+     * \param _x_plus_delta the updated state. It has the same format as the initial state.
+     * \param _dt the time interval spanned by the Delta
+     *
+     * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
+     */
+    void statePlusDelta(const VectorComposite& _x,
+                        const Eigen::VectorXd& _delta,
+                        const double           _dt,
+                        VectorComposite&       _x_plus_delta) const override;
+
+    /** \brief Delta zero
+     * \return a delta state equivalent to the null motion.
+     *
+     * Examples (see documentation of the the class for info on Eigen::VectorXd):
+     *   - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
+     *   - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
+     *   - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
+     */
+    Eigen::VectorXd deltaZero() const override;
+
+    /** \brief correct the preintegrated delta
+     * This produces a delta correction according to the appropriate manifold.
+     * @param delta_preint : the preintegrated delta to correct
+     * @param delta_step : an increment in the tangent space of the manifold
+     *
+     * We want to do
+     *
+     *   delta_corr = delta_preint (+) d_step
+     *
+     * where the operator (+) is the right-plus operator on the delta's manifold.
+     *
+     * Note: usually in motion pre-integration we have
+     *
+     *   delta_step = Jac_delta_calib * (calib - calib_pre)
+     *
+     */
+    Eigen::VectorXd correctDelta(const Eigen::VectorXd& _delta_preint,
+                                 const Eigen::VectorXd& _delta_step) const override;
+
+  protected:
+    /** \brief emplace a CaptureMotion
+     * \param _frame_own frame owning the Capture to create
+     * \param _sensor Sensor that produced the Capture
+     * \param _ts time stamp
+     * \param _data a vector of motion data
+     * \param _data_cov covariances matrix of the motion data data
+     * \param _calib calibration vector
+     * \param _calib_preint calibration vector used during pre-integration
+     * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
+     */
+    CaptureMotionPtr emplaceCapture(const FrameBasePtr&   _frame_own,
+                                    const SensorBasePtr&  _sensor,
+                                    const TimeStamp&      _ts,
+                                    const VectorXd&       _data,
+                                    const MatrixXd&       _data_cov,
+                                    const VectorXd&       _calib,
+                                    const VectorXd&       _calib_preint,
+                                    const CaptureBasePtr& _capture_origin_ptr) override;
+
+    /** \brief emplace a feature corresponding to given capture and add the feature to this capture
+     * \param _capture_motion: the parent capture
+     */
+    FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
+
+    /** \brief emplace a factor and link it in the wolf tree
+     * \param _feature_motion: the parent feature
+     * \param _frame_origin: the frame constrained by this motion factor
+     */
+    FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
+
+    void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+  public:
+    virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+};
+
+inline Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::deltaZero() const
+{
+    return bodydynamics::fti::identity();
+}
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_ */

src/processor/processor_force_torque_inertial_preint_dynamics.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_preintegrator_force_torque_inertial_dynamics.cpp
+ *
+ *  Created on: Aug 19, 2021
+ *      Author: jsola
+ */
+
+// bodydynamics
+#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+
+// wolf
+#include <core/state_block/state_composite.h>
+#include <core/capture/capture_motion.h>
+
+namespace wolf
+{
+
+ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDynamics(
+    ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics)
+    : ProcessorMotion("ProcessorForceTorqueInertialPreintDynamics",
+                      "POVL",
+                      3,   // dim
+                      13,  // state size [p, q, v, L]
+                      19,  // delta size [pi, vi, pd, vd, L, q]
+                      18,  // delta cov size
+                      12,  // data size [a, w, f, t]
+                      13,  // calib size [ab, wb, C, i, m]
+                      _params_force_torque_inertial_preint_dynamics),
+      params_force_torque_inertial_preint_dynamics_(_params_force_torque_inertial_preint_dynamics)
+{
+    // TODO Auto-generated constructor stub
+}
+
+ProcessorForceTorqueInertialPreintDynamics::~ProcessorForceTorqueInertialPreintDynamics()
+{
+    // TODO Auto-generated destructor stub
+}
+
+CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(const FrameBasePtr&   _frame_own,
+                                                                            const SensorBasePtr&  _sensor,
+                                                                            const TimeStamp&      _ts,
+                                                                            const VectorXd&       _data,
+                                                                            const MatrixXd&       _data_cov,
+                                                                            const VectorXd&       _calib,
+                                                                            const VectorXd&       _calib_preint,
+                                                                            const CaptureBasePtr& _capture_origin_ptr)
+{
+    CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion> (_frame_own, 
+                                                                    "CaptureMotion", _ts,
+                                                                    _sensor, 
+                                                                    _data, 
+                                                                    _data_cov, 
+                                                                    _capture_origin_ptr);
+    setCalibration(capture, _calib);
+    capture->setCalibrationPreint(_calib_preint);
+    return capture;
+}
+
+FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
+{
+    auto motion = _capture_own->getBuffer().back();
+    FeatureBasePtr ftr = FeatureBase::emplace<FeatureMotion>(
+                        _capture_own,
+                        "FeatureMotion",
+                        motion.delta_integr_,
+                        motion.delta_integr_cov_,
+                        _capture_own->getCalibrationPreint(),
+                        motion.jacobian_calib_);
+    return ftr;
+}
+
+FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
+                                                                        CaptureBasePtr _capture_origin)
+{
+    
+    FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion);
+
+    auto fac_ftipd = FactorBase::emplace<FactorForceTorqueInertialDynamics>(_feature_motion, ftr_ftipd, _capture_origin, shared_from_this(), params_->apply_loss_function);
+    return fac_ftipd;
+
+    //FactorBasePtr returnValue;
+    //return returnValue;
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture,
+                                                                const VectorXd&      _calibration)
+{
+    _capture->getStateBlock('I')->setState(_calibration);  // Set IMU bias
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor)
+{
+    sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
+                                                                     const Eigen::MatrixXd& _data_cov,
+                                                                     const Eigen::VectorXd& _calib,
+                                                                     const double           _dt,
+                                                                     Eigen::VectorXd&       _delta,
+                                                                     Eigen::MatrixXd&       _delta_cov,
+                                                                     Eigen::MatrixXd&       _jacobian_calib) const
+{
+    // compute tangent by removing IMU bias
+    Matrix<double, 12, 1> tangent = _data;
+    tangent.head<6>() -= _calib.head<6>();  // J_tangent_data  = Identity(12,12)
+    Matrix<double, 12, 6> J_tangent_I;      // J_tangent_I = [-Identity(6,6) ; Zero(6,6)]
+    J_tangent_I.topRows<6>()    = -Matrix6d::Identity();
+    J_tangent_I.bottomRows<6>() = Matrix6d::Zero();
+
+    // go from tangent to delta. We need to dynamic model for this
+    const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+    Matrix<double, 18, 12>      J_delta_tangent;
+    Matrix<double, 18, 7>       J_delta_model;
+    fti::tangent2delta(tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);
+
+    // Compute cov and compose jacobians
+    Matrix<double, 18, 6> J_delta_I = J_delta_tangent * J_tangent_I;
+    _jacobian_calib << J_delta_I, J_delta_model;  // J_delta_calib = _jacobian_calib = [J_delta_I ; J_delta_model]
+    const auto& J_delta_data = J_delta_tangent;   //  * J_tangent_data, which is the Identity;
+    _delta_cov               = J_delta_data * _data_cov * J_delta_data.transpose();
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                                                const Eigen::VectorXd& _delta2,
+                                                                const double           _dt2,
+                                                                Eigen::VectorXd&       _delta1_plus_delta2) const
+{
+    bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                                                const Eigen::VectorXd& _delta2,
+                                                                const double           _dt2,
+                                                                Eigen::VectorXd&       _delta1_plus_delta2,
+                                                                Eigen::MatrixXd&       _jacobian1,
+                                                                Eigen::MatrixXd&       _jacobian2) const
+{
+    bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+}
+
+void ProcessorForceTorqueInertialPreintDynamics::statePlusDelta(const VectorComposite& _x,
+                                                                const Eigen::VectorXd& _delta,
+                                                                const double           _dt,
+                                                                VectorComposite&       _x_plus_delta) const
+{
+    auto     x            = _x.vector("PVLO");
+    VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
+    _x_plus_delta         = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
+}
+
+Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
+                                                                         const Eigen::VectorXd& _delta_step) const
+{
+    return fti::plus(_delta_preint, _delta_step);
+}
+
+VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
+{
+    if (_capture)
+        return _capture->getStateBlock('I')->getState();  // IMU bias
+    else
+        return getSensor()->getStateBlock('I')->getState();  // IMU bias
+}
+} /* namespace wolf */
+
+#include <core/processor/factory_processor.h>
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreintDynamics);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreintDynamics);
+}  // namespace wolf
\ No newline at end of file

test/gtest_factor_force_torque_inertial_dynamics.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
+{
+  public:
+    ProblemPtr                                    P;
+    SensorForceTorqueInertialPtr                  S;
+    ProcessorForceTorqueInertialPreintDynamicsPtr p;
+    FrameBasePtr                                  F0, F1, F;
+    CaptureMotionPtr                              C0, C1, C;
+    FeatureMotionPtr                              f1;
+    FactorForceTorqueInertialDynamicsPtr          c1;
+
+    VectorXd              data;
+    MatrixXd              data_cov;
+    VectorXd              delta;
+    MatrixXd              delta_cov;
+    Matrix<double, 13, 1> calib;
+    MatrixXd              J_delta_calib;
+
+  protected:
+    void SetUp() override
+    {
+        data     = VectorXd::Zero(12);  // [ a, w, f, t ]
+        data_cov = MatrixXd::Identity(12, 12);
+
+        std::string  wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+        ParserYaml   parser    = ParserYaml("problem_force_torque_inertial_dynamics.yaml", wolf_root + "/test/yaml/");
+        ParamsServer server    = ParamsServer(parser.getParams());
+        P                      = Problem::autoSetup(server);
+
+        S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+        p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+
+        //P->print(4, 1, 1, 1);
+
+        C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+        C->process();
+
+        P->print(4, 1, 1, 1);
+
+        C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+        F0 = C0->getFrame();
+
+        ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
+        ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
+
+        C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+        C->process();
+
+        C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
+        F1 = C1->getFrame();
+    }
+
+};
+
+
+class Test_FactorForceTorqueInertialDynamics : public testing::Test
+{
+  public:
+    ProblemPtr                                    P;
+    SensorForceTorqueInertialPtr                  S;
+    ProcessorForceTorqueInertialPreintDynamicsPtr p;
+    FrameBasePtr                                  F0, F1;
+    CaptureMotionPtr                              C0, C1;
+    FeatureMotionPtr                              f1;
+    FactorForceTorqueInertialDynamicsPtr          c1;
+
+    VectorXd              data;
+    MatrixXd              data_cov;
+    VectorXd              delta;
+    MatrixXd              delta_cov;
+    Matrix<double, 13, 1> calib;
+    MatrixXd              J_delta_calib;
+
+  protected:
+    void SetUp() override
+    {
+        data     = VectorXd::Zero(12);  // [ a, w, f, t ]
+        data_cov = MatrixXd::Identity(12, 12);
+
+        // crear un problem
+        P = Problem::create("POVL", 3);
+
+        // crear un sensor
+        auto     params_sensor = std::make_shared<ParamsSensorForceTorqueInertial>();
+        Vector7d extrinsics;
+        extrinsics << 0, 0, 0, 0, 0, 0, 1;
+        S = SensorBase::emplace<SensorForceTorqueInertial>(P->getHardware(), extrinsics, params_sensor);
+        S->setStateBlockDynamic('I', true);
+
+        // crear processador
+        auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialPreintDynamics>();
+        p = ProcessorBase::emplace<ProcessorForceTorqueInertialPreintDynamics>(S, params_processor);
+
+        // crear dos frame
+        VectorXd state(13);
+        state << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
+        VectorComposite state_c(state, "POVL", {3, 4, 3, 3});
+        F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 0.0, "POVL", state_c);
+        F1 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 1.0, "POVL", state_c);
+
+        // crear dues capture
+        VectorXd data(VectorXd::Zero(12));
+        MatrixXd data_cov(MatrixXd::Identity(12, 12));
+        Vector6d bias(Vector6d::Zero());
+        auto     sb_bias = std::make_shared<StateParams6>(bias);
+        C0 = CaptureBase::emplace<CaptureMotion>(F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr,
+                                                 nullptr, sb_bias);
+        C1 = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr,
+                                                 sb_bias);
+
+        // crear un feature
+        VectorXd delta_preint(VectorXd::Zero(19));
+        delta_preint.head<3>() = 0.5*gravity();
+        delta_preint.segment<3>(3) = gravity();
+        delta_preint(15) = 1;
+        delta_preint.segment<3>(6) = 0.5*gravity();
+        delta_preint.segment<3>(9) = gravity();
+        MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+        VectorXd calib_preint(VectorXd::Zero(13));
+        MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+        f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
+                                                 jacobian_calib);
+
+        // crear un factor
+        c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
+    }
+};
+
+TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
+{
+    ASSERT_EQ(c1->getCapture(), C1);
+    ASSERT_EQ(c1->getCalibPreint().size(), 13);
+}
+
+//Test en el que no hi ha moviment
+// TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
+// {
+//     VectorXd              res_exp = VectorXd::Zero(18);
+//     Matrix<double, 18, 1> res;
+//     VectorXd              bias = VectorXd::Zero(6);
+
+//     c1->residual(F0->getStateBlock('P')->getState(),                      // p0
+//                  Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+//                  F0->getStateBlock('V')->getState(),                      // v0
+//                  F0->getStateBlock('L')->getState(),                      // L0
+//                  bias,                                                    // I
+//                  F1->getStateBlock('P')->getState(),                      // p1
+//                  Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+//                  F1->getStateBlock('V')->getState(),                      // v1
+//                  F1->getStateBlock('L')->getState(),                      // L1
+//                  S->getCom(),                                             // C
+//                  S->getInertia(),                                         // i
+//                  S->getMass(),                                            // m
+//                  res);
+
+//     ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
+
+//Test en el que s'avança 1m en direcció x
+
+// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
+// {
+//     VectorXd              res_exp = VectorXd::Zero(18);
+//     Matrix<double, 18, 1> res;
+//     VectorXd              bias = VectorXd::Zero(6);
+
+//     //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+//     F1->getStateBlock('P')->setState(Vector3d (1,0,0));
+
+//     //Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+//     VectorXd delta_preint(VectorXd::Zero(19));
+//     delta_preint.head<3>() = 0.5*gravity();
+//     delta_preint(0) = 1;
+//     delta_preint.segment<3>(3) = gravity();
+//     delta_preint(15) = 1;
+//     delta_preint.segment<3>(6) = 0.5*gravity();
+//     delta_preint(6) = 1;
+//     delta_preint.segment<3>(9) = gravity();
+//     MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+//     VectorXd calib_preint(VectorXd::Zero(13));
+//     MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+//     FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib);
+
+//     FactorForceTorqueInertialDynamicsPtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+
+
+//     c2->residual(F0->getStateBlock('P')->getState(),                      // p0
+//                  Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+//                  F0->getStateBlock('V')->getState(),                      // v0
+//                  F0->getStateBlock('L')->getState(),                      // L0
+//                  bias,                                                    // I
+//                  F1->getStateBlock('P')->getState(),                      // p1
+//                  Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+//                  F1->getStateBlock('V')->getState(),                      // v1
+//                  F1->getStateBlock('L')->getState(),                      // L1
+//                  S->getCom(),                                             // C
+//                  S->getInertia(),                                         // i
+//                  S->getMass(),                                            // m
+//                  res);
+
+//     ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
+
+
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+
+    return RUN_ALL_TESTS();
+}
\ No newline at end of file

test/yaml/problem_force_torque_inertial_dynamics.yaml

+config:
+  problem:
+    frame_structure: "POVL"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+        V: [0,0,0]
+        L: [0,0,0]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+        V: [1,1,1]
+        L: [1,1,1]
+      time_tolerance: 0.1
+    tree_manager: 
+      type: "None"
+  map:
+    type: "MapBase"
+    plugin: "core"
+  sensors:
+   -
+    name: "sensor FTI"
+    type: "SensorForceTorqueInertial"
+    plugin: "bodydynamics"
+    extrinsic:
+      pose: [0,0,0, 0,0,0,1]
+    follow: "./sensor_force_torque_inertial.yaml"
+
+  processors:
+   -
+    name: "proc FTID"
+    type: "ProcessorForceTorqueInertialPreintDynamics"
+    sensor_name: "sensor FTI"
+    plugin: "bodydynamics"
+    follow: "processor_force_torque_inertial_preint_dynamics.yaml"
+    
\ No newline at end of file

test/yaml/processor_force_torque_inertial_preint_dynamics.yaml

+    time_tolerance: 0.1
+    apply_loss_function: false
+    unmeasured_perturbation_std: 0.00000001
+    state_getter: true
+    state_priority: 1
+    n_propellers: 3
+    keyframe_vote:
+      voting_active:    false
+      max_time_span:    1
+      max_buff_length:  5   # motion deltas
+      dist_traveled:    1   # meters
+      angle_turned:     1   # radians (1 rad approx 57 deg, approx 60 deg)
+