processor_odom_2d exact integration if theta != 0

processor_odom_2d has two integration functions :

• exact integration if theta != 0 (impl : theta > 10e-3)
• Runge-Kutta integration otherwise

Added a test with data :

test_processor_odom_2d ~/path/to/wolf/src/example/data/odom_2d_loop.dat .

Data are basically ROS geometry_msgs/TwistStamped recorded from TIAGo. The robot in moving around for 90sec coming back to its starting point with but facing back ( ~ x:0 y:0 theta:pi ).

See e.g. slide 5

src/processor_odom_2D.h

@@ -58,6 +58,9 @@ class ProcessorOdom2D : public ProcessorMotion
Scalar cov_det_th_;
Scalar elapsed_time_th_;
+        void integrateRungeKutta(const Eigen::VectorXs& _data, const Eigen::MatrixXs &_data_cov);
+        void integrateExact(const Eigen::VectorXs& _data, const Eigen::MatrixXs &_data_cov);
+
// Factory method
public:
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
@@ -83,28 +86,92 @@ inline void ProcessorOdom2D::data2delta(const Eigen::VectorXs& _data, const Eige
assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
-    // data  is [dtheta, dr]
-    // delta is [dx, dy, dtheta]
-    // motion model is 1/2 turn + straight + 1/2 turn
-    delta_(0) = cos(_data(1)/2) * _data(0);
-    delta_(1) = sin(_data(1)/2) * _data(0);
-    delta_(2) = _data(1);
-
-    // Fill delta covariance
-    Eigen::MatrixXs J(delta_cov_size_,data_size_);
-    J(0,0) =   cos(_data(1) / 2);
-    J(1,0) =   sin(_data(1) / 2);
-    J(2,0) =   0;
-    J(0,1) = - _data(0) * sin(_data(1) / 2) / 2;
-    J(1,1) =   _data(0) * cos(_data(1) / 2) / 2;
-    J(2,1) =   1;
-
-    delta_cov_ = J * _data_cov * J.transpose();
+    Eigen::abs(_data(1)) < 10e-3 ? integrateRungeKutta(_data, _data_cov) :
+                                   integrateExact(_data, _data_cov);
//std::cout << "data cov:" << std::endl << _data_cov << std::endl;
//std::cout << "delta cov:" << std::endl << _delta_cov << std::endl;
}
+inline void ProcessorOdom2D::integrateRungeKutta(const Eigen::VectorXs& _data,
+                                                 const Eigen::MatrixXs& _data_cov)
+{
+  assert(_data.size()     == data_size_ && "Wrong _data vector size");
+  assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
+  assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
+
+  const Scalar direction     = _data(1) * Scalar(.5);
+  const Scalar cos_direction = Eigen::cos(direction);
+  const Scalar sin_direction = Eigen::sin(direction);
+
+  // data  is [dtheta, dr]
+  // delta is [dx, dy, dtheta]
+  delta_(0) = cos_direction * _data(0);
+  delta_(1) = sin_direction * _data(0);
+  delta_(2) = _data(1);
+
+  // Fill delta covariance
+  Eigen::MatrixXs J(delta_cov_size_, data_size_);
+  J(0,0) =  cos_direction;
+  J(1,0) =  sin_direction;
+  J(2,0) =  0;
+
+  J(0,1) = -_data(0) * sin_direction * Scalar(.5);
+  J(1,1) =  _data(0) * cos_direction * Scalar(.5);
+  J(2,1) =  1;
+
+  delta_cov_ = J * _data_cov * J.transpose();
+
+  //std::cout << "data cov:" << std::endl << _data_cov << std::endl;
+  //std::cout << "delta cov:" << std::endl << _delta_cov << std::endl;
+}
+
+inline void ProcessorOdom2D::integrateExact(const Eigen::VectorXs& _data,
+                                            const Eigen::MatrixXs& _data_cov)
+{
+  assert(_data.size()     == data_size_ && "Wrong _data vector size");
+  assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
+  assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
+
+  const Scalar r = _data(0) / _data(1);
+
+  const Scalar heading_prev = delta_integrated_(2);
+  const Scalar heading      = heading_prev + _data(1);
+
+  const Scalar sin_h_p = Eigen::sin(heading_prev);
+  const Scalar cos_h_p = Eigen::cos(heading_prev);
+
+  const Scalar sin_h = Eigen::sin(heading);
+  const Scalar cos_h = Eigen::cos(heading);
+
+  // data  is [dtheta, dr]
+  // delta is [dx, dy, dtheta]
+  delta_(0) = r * (sin_h - sin_h_p);
+  delta_(1) = r * (cos_h - cos_h_p);
+  delta_(2) = _data(1);
+
+  Scalar inv_w    = Scalar(1) / _data(1);
+  Scalar inv_w2   = inv_w * inv_w;
+
+  Scalar l_inv_w  = _data(0) * inv_w;
+  Scalar l_inv_w2 = _data(0) * inv_w2;
+
+  // Fill delta covariance
+  Eigen::MatrixXs J(delta_cov_size_, data_size_);
+  J(0,0) =  (sin_h - sin_h_p) * inv_w;
+  J(1,0) =  (cos_h - cos_h_p) * inv_w;
+  J(2,0) =  0;
+
+  J(0,1) = -(l_inv_w * cos_h) - l_inv_w2 * (sin_h + sin_h_p);
+  J(1,1) =  (l_inv_w * sin_h) - l_inv_w2 * (cos_h + cos_h_p);
+  J(2,1) =  1;
+
+  delta_cov_ = J * _data_cov * J.transpose();
+
+  //std::cout << "data cov:" << std::endl << _data_cov << std::endl;
+  //std::cout << "delta cov:" << std::endl << _delta_cov << std::endl;
+}
+
inline void ProcessorOdom2D::xPlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt, Eigen::VectorXs& _x_plus_delta)
{