Colsolidate API of data2tangent()

include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h

@@ -133,12 +133,6 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
                              Eigen::MatrixXd&       _delta_cov,
                              Eigen::MatrixXd&       _jacobian_calib) const override;
 
-    void data2tangent(const Eigen::VectorXd&        _data,
-                       const Eigen::VectorXd&        _calib,
-                       Eigen::VectorXd& _tangent,
-                       Eigen::MatrixXd& _J_tangent_data,
-                       Eigen::MatrixXd& _J_tangent_bias) const;
-
     /** \brief composes a delta-state on top of another delta-state
      * \param _delta1 the first delta-state
      * \param _delta2 the second delta-state
@@ -249,6 +243,14 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
 
   public:
     virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+  private:
+    void data2tangent(const Eigen::VectorXd& _data,     ///< [a, w, f, t] raw measures
+                      const Eigen::VectorXd& _bias,     ///< [ab, wb]
+                      const Eigen::VectorXd& _model,    ///< [com, inertia, mass]
+                      Eigen::VectorXd&       _tangent,  ///< [a, w, f, t] calibrated
+                      Eigen::MatrixXd&       _J_tangent_data,
+                      Eigen::MatrixXd&       _J_tangent_bias,
+                      Eigen::MatrixXd&       _J_tangent_model) const;
 };
 
 inline Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::deltaZero() const

src/processor/processor_force_torque_inertial_preint_dynamics.cpp

@@ -111,36 +111,40 @@ void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor
     sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd&         _data,
-                                                               const Eigen::VectorXd&         _calib,
-                                                               Eigen::VectorXd&  _tangent,
-                                                               Eigen::MatrixXd& _J_tangent_data,
-                                                               Eigen::MatrixXd&  _J_tangent_bias) const
+void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,
+                                                              const Eigen::VectorXd& _bias,
+                                                              const Eigen::VectorXd& _model,
+                                                              Eigen::VectorXd&       _tangent,
+                                                              Eigen::MatrixXd&       _J_tangent_data,
+                                                              Eigen::MatrixXd&       _J_tangent_bias,
+                                                              Eigen::MatrixXd&       _J_tangent_model) const
 
 {
-    Vector6d        bias  = _calib.segment<6>(0);
-    Vector7d        model = _calib.segment<7>(6);
-    const Vector6d& awd   = _data.segment<6>(0);  // this is (a,w) of data in a 6-vector
-    const Vector3d& fd    = _data.segment<3>(6);
-    const Vector3d& td    = _data.segment<3>(9);
-    const Vector3d& c     = model.segment<3>(0);
-    // const Matrix3d& fd_cross = skew(fd);
+    const Vector6d& awd = _data.segment<6>(0);  // this is (a,w) of data in a 6-vector
+    const Vector3d& fd  = _data.segment<3>(6);
+    const Vector3d& td  = _data.segment<3>(9);
+    const Vector3d& c   = _model.segment<3>(0);
+    const Matrix3d& fd_cross = skew(fd);
     const Matrix3d& c_cross = skew(c);
 
     // tangent(a,w) = data(a,w) - bias(a,w)
     // tangent(f)   = data(f)
     // tangent(t)   = data(t) - model(c) x data(f)
-    _tangent.segment<6>(0) = awd - bias;
+    _tangent.segment<6>(0) = awd - _bias;
     _tangent.segment<3>(6) = fd;
     _tangent.segment<3>(9) = td - c.cross(fd);  // c x fd
 
     // J_tangent_data
-    _J_tangent_data                   = MatrixXd::Identity(12, 12);
+    _J_tangent_data.setIdentity(12, 12);
     _J_tangent_data.block<3, 3>(9, 6) = -c_cross;  // J_tt_fd = -c_cross
 
     // J_tangent_bias = [-Identity(6,6) ; Zero(6,6)]
     _J_tangent_bias.topRows<6>()    = -Matrix6d::Identity();
     _J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
+
+    // J_tangent_model
+    _J_tangent_model.setZero(12,7);
+    _J_tangent_model.block<3, 3>(9, 0) = fd_cross;  // J_tt_c = fd_cross
 }
 
 void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -274,23 +278,21 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
      *
      * ---------------------------------------------------------------
      *
-     * Notes:
-     *
-     * cross product and Jacobians
+     * Notes: Jacobians of cross product
      *
-     * a, b in R3
+     *  a, b in R^3
      *
-     * a_x, b_x, skew-symmetric matrices
+     *  a_x, b_x, skew-symmetric matrices, in R^3x3
      *
-     * a_x = [0 -az ay ; az 0 -ax ; -ay ax 0]
+     *  a_x = [0 -az ay ; az 0 -ax ; -ay ax 0]
      *
-     * a x b = a_x b
+     *  a x b = a_x b
      *
-     * a x b = - b x a = - b_x a
+     *  a x b = - b x a = - b_x a
      *
-     * J_(axb)_a = -b_x
+     *  J_(axb)_a = -b_x
      *
-     * J_(axb)_b = a_x
+     *  J_(axb)_b = a_x
      */
 
     // Matrix<double, 12, 1> tangent = _data;
@@ -322,14 +324,16 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
      * tangent  = [at,wt,ft,tt]
      */
 
-    VectorXd  tangent(12); 
+    Vector6d bias  = _calib.segment<6>(0);
+    Vector7d model = _calib.segment<7>(6);
+    VectorXd tangent(12);
     MatrixXd J_tangent_data(12, 12);
-    MatrixXd  J_tangent_bias(12, 6);
+    MatrixXd J_tangent_bias(12, 6);
+    MatrixXd J_tangent_model(12, 7);
 
-    data2tangent(_data, _calib, tangent, J_tangent_data, J_tangent_bias);
+    data2tangent(_data, bias, model, tangent, J_tangent_data, J_tangent_bias, J_tangent_model);
 
     // 2. go from tangent to delta. We need again the dynamic model for this
-    Vector7d               model = _calib.segment<7>(6);
     Matrix<double, 18, 12> J_delta_tangent;
     Matrix<double, 18, 7>  J_delta_model;
     fti::tangent2delta(
@@ -337,11 +341,11 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
 
     // 3. Compose jacobians
     Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias;
-    // !!!!!! Now J_delta_model seems to be calculated in function tangent2delta as J_delta_tangent * J_tangent_model !!!!!!!!!!!
-    // TODO: Check the calculation od the J_delta_model
-    // J_delta_model += J_delta_tangent * J_tangent_model; 
-    _jacobian_calib << J_delta_bias,
-        J_delta_model;  // J_delta_calib = _jacobian_calib = [J_delta_bias ; J_delta_model]
+    // !!!!!! Now J_delta_model seems to be calculated in function tangent2delta as J_delta_tangent * J_tangent_model
+    // !!!!!!!!!!!
+    // TODO: Check the calculation od the J_delta_model in fti::tangent2delta()
+    // J_delta_model += J_delta_tangent * J_tangent_model;
+    _jacobian_calib << J_delta_bias, J_delta_model;  // J_delta_calib = [J_delta_bias ; J_delta_model]
     const auto& J_delta_data = J_delta_tangent * J_tangent_data;
 
     // 4. compute covariance