Improve doc and remove old commented code

src/processor/processor_force_torque_inertial_dynamics.cpp

@@ -177,12 +177,9 @@ void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _
     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<3>(6) = fd;
-    _tangent.segment<3>(9) = td - c.cross(fd);  // c x fd
+    _tangent.segment<6>(0) = awd - _bias;       // awt = awd - awb
+    _tangent.segment<3>(6) = fd;                // ft = fd
+    _tangent.segment<3>(9) = td - c.cross(fd);  // tt = td - c x fd
 
     // J_tangent_data
     _J_tangent_data.setIdentity(12, 12);
@@ -295,15 +292,52 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
      *  J_delta_tangent
      *  J_delta_model
      *
-     * these are obtained directly by the function g() = tangent2delta().
-     *
-     * Note 1: It is unclear to me (JS, 4-aug-2022) if this function tangent2delta() is completely accurate
-     * with regard to the two different reference frames: BASE and COM. It is possible that
-     * we have to revise the math.
-     *
-     * Note 2: It is even possible that the function tangent2delta() is OK, but then that the function
-     * betweenStates() does not account for the difference in reference frames. So we also need to revise
-     * the math inside betweenStates() with regard to the two reference frames BASE and COM.
+     * these are obtained directly by the function g() = tangent2delta(). This function does:
+     *
+     *  angacc      = I.inv * ( tt -  wt x ( I * wt ) )    
+     *      --> J_aa_i, J_aa_tt, J_aa_wt
+     *  acc_dyn     = ft / _mass - angacc x com - wt x ( wt x com )  
+     *      --> J_ad_ft, J_ad_m, J_ad_aa, J_ad_c, J_ad_wt
+     *      ==> J_ad_wt, J_ad_ft, J_ad_tt, J_ad_c, J_ad_i, J_ad_m
+     *
+     *  delta_p_imu = 0.5 * at * dt^2       --> J_dpi_at
+     *  delta_v_imu = at * _dt              --> J_dvi_at
+     *  delta_p_dyn = 0.5 * acc_dyn * dt^2  --> J_dpd_ad
+     *  delta_v_dyn = acc_dyn * _dt         --> J_dvd_ad
+     *  delta_L     = tt * _dt              --> J_dL_tt
+     *  delta_q     = exp_q(wt * _dt)       --> J_dq_wt
+     * 
+     * Assembling Jacobians gives:
+     * 
+     *                      at          wt          ft          tt
+     *  J_delta_tangent = [ J_dpi_at    0           0           0
+     *                      J_dvi_at    0           0           0
+     *                      0           J_dpd_wt    J_dpd_ft    J_dpd_tt
+     *                      0           J_dvd_wt    J_dvd_ft    J_dvd_tt
+     *                      0           0           0           J_dL_tt
+     *                      0           J_dq_wt     0           0       ]
+     * with
+     *  J_dpd_wt = J_dpd_ad * ( J_ad_wt + J_ad_aa * J_aa_wt )
+     *  J_dpd_ft = J_dpd_ad * J_ad_ft
+     *  J_dpd_tt = J_dpd_ad * J_ad_aa * J_aa_tt
+     *  J_dvd_wt = J_dvd_ad * ( J_ad*wt + J_ad_aa * J_aa_wt )
+     *  J_dvd_ft = J_dvd_ad * J_ad_ft
+     *  J_dvd_tt = J_dvd_ad * J_ad_aa * J_aa_tt
+     *
+     *                      c       i       m
+     *  J_delta_model = [   0       0       0
+     *                      0       0       0
+     *                      J_dpd_c J_dpd_i J_dpd_m
+     *                      J_dvd_c J_dvd_i J_dvd_m
+     *                      0       0       0
+     *                      0       0       0   ]
+     * with
+     *  J_dpd_c = J_dpd_ad * J_ad_c
+     *  J_dpd_i = J_dpd_ad * J_ad_aa * J_aa_i
+     *  J_dpd_m = J_dpd_ad * J_ad_m
+     *  J_dvd_c = J_dvd_ad * J_ad_c
+     *  J_dvd_i = J_dvd_ad * J_ad_aa * J_aa_i
+     *  J_dvd_m = J_dvd_ad * J_ad_m
      *
      * 3) On a third stage, we need to apply the chain rule for the functions f() and g(),
      * that is delta = g( f( data, bias, model), model)  -- yes, it's a mess!!
@@ -317,10 +351,12 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
      *
      *  J_delta_calib = [ J_delta_bias | J_delta_model ]
      *
+     *
      * 4) On a fourth stage, we compute the covariance of the delta:
      *
      *  Q_delta = J_delta_data * Q_data * J_delta_data.transpose()
      *
+     *
      * 5) The function returns the following quantities, which relate to the variables used above:
      *
      *  - _delta            = delta
@@ -346,26 +382,6 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
      *  J_(axb)_b = a_x
      */
 
-    // 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();
-
-    ////////////////////// NOU CODI QUE HAURE DE REVISAR I CANVIAR PEL QUE HI HA ADALT //////////////////////////
-
     /*
      * 1. tangent = f(data, bias, model) --> J_tangent_data, J_tangent_bias, J_tangent_model
      *
@@ -465,13 +481,13 @@ bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
     // time span
     if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_force_torque_inertial_dynamics_->max_time_span)
     {
-        WOLF_DEBUG("PM: vote: time span");
+        WOLF_DEBUG("PM ", getName(), " vote: time span");
         return true;
     }
     // buffer length
     if (getBuffer().size() > params_force_torque_inertial_dynamics_->max_buff_length)
     {
-        WOLF_DEBUG("PM: vote: buffer length");
+        WOLF_DEBUG("PM ", getName(), " vote: buffer length");
         return true;
     }
     // dist_traveled
@@ -482,14 +498,14 @@ bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
     double dist = (X1.at('P') - X0.at('P')).norm();
     if (dist > params_force_torque_inertial_dynamics_->dist_traveled)
     {
-        WOLF_DEBUG("PM: vote: distance traveled");
+        WOLF_DEBUG("PM ", getName(), " vote: distance traveled");
         return true;
     }
     // angle turned
     double angle = 2.0 * acos(getMotion().delta_integr_(15));
     if (angle > params_force_torque_inertial_dynamics_->angle_turned)
     {
-        WOLF_DEBUG("PM: vote: angle turned");
+        WOLF_DEBUG("PM ", getName(), " vote: angle turned");
         return true;
     }