Rotate position in motion model constraint

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

@@ -119,26 +119,26 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
                 double* residuals,
                 double** jacobians) const override
   {
-    double position_pred_x;
-    double position_pred_y;
+    double delta_position_pred_x;
+    double delta_position_pred_y;
     double yaw_pred;
     double vel_linear_pred_x;
     double vel_linear_pred_y;
     double vel_yaw_pred;
     double acc_linear_pred_x;
     double acc_linear_pred_y;
     predict(
-      parameters[0][0],  // position1_x
-      parameters[0][1],  // position1_y
-      parameters[1][0],  // yaw1
+      0.0,
+      0.0,
+      0.0,
       parameters[2][0],  // vel_linear1_x
       parameters[2][1],  // vel_linear1_y
       parameters[3][0],  // vel_yaw1
       parameters[4][0],  // acc_linear1_x
       parameters[4][1],  // acc_linear1_y
       dt_,
-      position_pred_x,
-      position_pred_y,
+      delta_position_pred_x,
+      delta_position_pred_y,
       yaw_pred,
       vel_linear_pred_x,
       vel_linear_pred_y,
@@ -147,20 +147,25 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
       acc_linear_pred_y,
       jacobians);
 
-    residuals[0] = parameters[5][0] - position_pred_x;
-    residuals[1] = parameters[5][1] - 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;
+    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);
+
+    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

@@ -40,13 +40,11 @@
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/util.h>
 
-
 namespace fuse_models
 {
-
 /**
  * @brief Create a cost function for a 2D state vector
- * 
+ *
  * The state vector includes the following quantities, given in this order:
  *   x position
  *   y position
@@ -70,7 +68,7 @@ namespace fuse_models
  *             ||    [  yaw_vel_t2 - proj(yaw_vel_t1) ] ||
  *             ||    [    x_acc_t2 - proj(x_acc_t1)   ] ||
  *             ||    [    y_acc_t2 - proj(y_acc_t1)   ] ||
- * 
+ *
  * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function
  * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost
  * function of the form
@@ -147,28 +145,39 @@ bool Unicycle2DStateCostFunctor::operator()(
   const T* const acc_linear2,
   T* residual) const
 {
-  T position_pred[2];
-  T yaw_pred[1];
+  T delta_position_pred[2];
+  T delta_yaw_pred[1];
   T vel_linear_pred[2];
   T vel_yaw_pred[1];
   T acc_linear_pred[2];
+  T init_position[2];
+  T init_yaw[1];
+  init_position[0] = (T)0.0;
+  init_position[1] = (T)0.0;
+  init_yaw[0] = (T)0.0;
   predict(
-    position1,
-    yaw1,
+    init_position,
+    init_yaw,
     vel_linear1,
     vel_yaw1,
     acc_linear1,
     T(dt_),
-    position_pred,
-    yaw_pred,
+    delta_position_pred,
+    delta_yaw_pred,
     vel_linear_pred,
     vel_yaw_pred,
     acc_linear_pred);
 
+  // rotate the position residual into the local frame
+  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) = position2[0] - position_pred[0];
-  residuals_map(1) = position2[1] - 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;
@@ -128,7 +138,7 @@ TEST(CostFunction, evaluateCostFunction)
 
   for (size_t i = 0; i < num_parameter_blocks; ++i)
   {
-    EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits<double>::epsilon())
+    EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-15)
       << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt)
       << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt);
   }