Rotate position in motion model constraint

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

@@ -147,24 +147,25 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
       acc_linear_pred_y,
       jacobians);
 
-    // rotate the position residual into the local frame
-    auto sin_pred_inv = std::sin(-yaw_pred);
-    auto cos_pred_inv= std::cos(-yaw_pred);
+    const Eigen::Vector2d position1(parameters[0][0], parameters[0][1]);
+    const Eigen::Vector2d position2(parameters[5][0], parameters[5][1]);
+    const Eigen::Vector2d delta_position = fuse_core::rotationMatrix2D(-parameters[1][0]) * (position2 - position1);
+    const double delta_yaw = parameters[6][0] - parameters[1][0]; 
+    const Eigen::Vector2d delta_position_pred(delta_position_pred_x, delta_position_pred_y);
 
-    residuals[0] = cos_pred_inv * delta_position_pred_x - sin_pred_inv * delta_position_pred_y;
-    residuals[1] = sin_pred_inv * delta_position_pred_x + cos_pred_inv * delta_position_pred_y;
-    residuals[2] = parameters[6][0] - yaw_pred;
-    residuals[3] = parameters[7][0] - vel_linear_pred_x;
-    residuals[4] = parameters[7][1] - vel_linear_pred_y;
-    residuals[5] = parameters[8][0] - vel_yaw_pred;
-    residuals[6] = parameters[9][0] - acc_linear_pred_x;
-    residuals[7] = parameters[9][1] - acc_linear_pred_y;
+    Eigen::Map<Eigen::Matrix<double, 8, 1>> residuals_map(residuals);
+    residuals_map.head<2>() = fuse_core::rotationMatrix2D(-delta_yaw) * (delta_position - delta_position_pred);
+    residuals_map(2) = delta_yaw - yaw_pred;  // omitting fuse_core::wrapAngle2D because it is done later on
+    residuals_map(3) = parameters[7][0] - vel_linear_pred_x;
+    residuals_map(4) = parameters[7][1] - vel_linear_pred_y;
+    residuals_map(5) = parameters[8][0] - vel_yaw_pred;
+    residuals_map(6) = parameters[9][0] - acc_linear_pred_x;
+    residuals_map(7) = parameters[9][1] - acc_linear_pred_y;
 
     fuse_core::wrapAngle2D(residuals[2]);
 
     // Scale the residuals by the square root information matrix to account for
     // the measurement uncertainty.
-    Eigen::Map<fuse_core::Vector8d> residuals_map(residuals);
     residuals_map.applyOnTheLeft(A_);
 
     if (jacobians)

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

@@ -146,7 +146,7 @@ bool Unicycle2DStateCostFunctor::operator()(
   T* residual) const
 {
   T delta_position_pred[2];
-  T yaw_pred[1];
+  T delta_yaw_pred[1];
   T vel_linear_pred[2];
   T vel_yaw_pred[1];
   T acc_linear_pred[2];
@@ -163,19 +163,21 @@ bool Unicycle2DStateCostFunctor::operator()(
     acc_linear1,
     T(dt_),
     delta_position_pred,
-    yaw_pred,
+    delta_yaw_pred,
     vel_linear_pred,
     vel_yaw_pred,
     acc_linear_pred);
 
   // rotate the position residual into the local frame
-  T sin_pred_inv = ceres::sin(-yaw_pred[0]);
-  T cos_pred_inv = ceres::cos(-yaw_pred[0]);
+  const Eigen::Matrix<T, 2, 1> position1_map(position1[0], position1[1]);
+  const Eigen::Matrix<T, 2, 1> position2_map(position2[0], position2[1]);
+  const Eigen::Matrix<T, 2, 1> delta_position = fuse_core::rotationMatrix2D(-yaw1[0]) * (position2_map - position1_map);
+  const T delta_yaw = yaw2[0] - yaw1[0];  // omitting fuse_core::wrapAngle2D because it is done later on
+  const Eigen::Matrix<T, 2, 1> delta_position_pred_map(delta_position_pred[0], delta_position_pred[1]);
 
   Eigen::Map<Eigen::Matrix<T, 8, 1>> residuals_map(residual);
-  residuals_map(0) = cos_pred_inv * delta_position_pred[0] - sin_pred_inv * delta_position_pred[1];
-  residuals_map(1) = sin_pred_inv * delta_position_pred[0] + cos_pred_inv * delta_position_pred[1];
-  residuals_map(2) = yaw2[0] - yaw_pred[0];
+  residuals_map.template head<2>() = fuse_core::rotationMatrix2D(-delta_yaw) * (delta_position - delta_position_pred_map);
+  residuals_map(2) = delta_yaw - delta_yaw_pred[0];  // omitting fuse_core::wrapAngle2D because it is done later on
   residuals_map(3) = vel_linear2[0] - vel_linear_pred[0];
   residuals_map(4) = vel_linear2[1] - vel_linear_pred[1];
   residuals_map(5) = vel_yaw2[0] - vel_yaw_pred[0];

fuse_models/test/test_unicycle_2d_state_cost_function.cpp

@@ -99,7 +99,17 @@ TEST(CostFunction, evaluateCostFunction)
 
   // Check jacobians are correct using a gradient checker
   ceres::NumericDiffOptions numeric_diff_options;
-  ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options);
+// #if !CERES_SUPPORTS_MANIFOLDS
+//   ceres::GradientChecker gradient_checker(
+//     &cost_function,
+//     static_cast<std::vector<const ceres::LocalParameterization*>*>(nullptr),
+//     numeric_diff_options);
+// #else
+  ceres::GradientChecker gradient_checker(
+    &cost_function,
+    static_cast<std::vector<const ceres::Manifold*>*>(nullptr),
+    numeric_diff_options);
+// #endif
 
   // We cannot use std::numeric_limits<double>::epsilon() tolerance because the worst relative error is 5.26356e-10
   ceres::GradientChecker::ProbeResults probe_results;