Fix: simplify processor_odom3d computeCurrentDelta

src/processor/processor_odom_3d.cpp

@@ -41,22 +41,17 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
                                           Eigen::MatrixXd& _jacobian_calib) const
 {
     assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3d.");
-    double disp, rot; // displacement and rotation of this motion step
     if (_data.size() == (long int)6)
     {
         // rotation in vector form
         _delta.head<3>()    = _data.head<3>();
         Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3);
         q                   = v2q(_data.tail<3>());
-        disp                = _data.head<3>().norm();
-        rot                 = _data.tail<3>().norm();
     }
     else
     {
         // rotation in quaternion form
         _delta  = _data;
-        disp    = _data.head<3>().norm();
-        rot     = 2.0 * acos(_data(6)); // '6' is the real part of the quaternion
     }
     /* Jacobians of d = data2delta(data, dt)
      * with: d =    [Dp Dq]
@@ -72,13 +67,7 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
      *
      * so, J = I, and delta_cov = _data_cov
      */
-    // We discard _data_cov and create a new one from the measured motion
-    double disp_var = min_disp_var_ + k_disp_to_disp_ * disp;
-    double rot_var = min_rot_var_ + k_disp_to_rot_ * disp + k_rot_to_rot_ * rot;
-    Eigen::Matrix6d data_cov(Eigen::Matrix6d::Identity());
-    data_cov(0, 0) = data_cov(1, 1) = data_cov(2, 2) = disp_var;
-    data_cov(3, 3) = data_cov(4, 4) = data_cov(5, 5) = rot_var;
-    _delta_cov = data_cov;
+    _delta_cov = _data_cov;
 }
 
 void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,