Fix eigen maps

clearpathrobotics · Sep 16, 2024 · d0f9cb7 · d0f9cb7
1 parent 8b8a633
commit d0f9cb7
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 15 deletions.
diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h
@@ -115,19 +115,30 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
    */
   bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
   {
-    double delta_yaw_pred;
-    double vel_yaw_pred;
     fuse_core::Vector2d delta_position_pred;
+    double delta_yaw_pred;
     fuse_core::Vector2d vel_linear_pred;
+    double vel_yaw_pred;
     fuse_core::Vector2d acc_linear_pred;
-    predict(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_, delta_position_pred.x(), delta_position_pred.y(), delta_yaw_pred, vel_linear_pred.x(),
-            vel_linear_pred.y(), vel_yaw_pred, acc_linear_pred.x(), acc_linear_pred.y(), jacobians);
+    predict(
+      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_,
+      delta_position_pred.x(),
+      delta_position_pred.y(),
+      delta_yaw_pred,
+      vel_linear_pred.x(),
+      vel_linear_pred.y(),
+      vel_yaw_pred,
+      acc_linear_pred.x(),
+      acc_linear_pred.y(),
+      jacobians);
 
     const double delta_yaw_est = parameters[6][0] - parameters[1][0];
     const fuse_core::Vector2d position1(parameters[0][0], parameters[0][1]);
@@ -136,15 +147,15 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
     const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]);
 
     Eigen::Map<fuse_core::Vector8d> residuals_map(residuals);
-    residuals_map.head<2>() = R_yaw_inv * position_diff - delta_position_pred;
+    residuals_map.block(0, 0, 2, 1) = R_yaw_inv * position_diff - delta_position_pred;
     residuals_map(2) = delta_yaw_est - delta_yaw_pred;
     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_map(2));
+  fuse_core::wrapAngle2D(residuals_map(2));
 
     // Scale the residuals by the square root information matrix to account for
     // the measurement uncertainty.

diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h
@@ -170,9 +170,9 @@ bool Unicycle2DStateCostFunctor::operator()(
   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> position_diff = (position2_map - position1_map);
-  const fuse_core::Matrix<T, 2, 2> R_yaw_inv = fuse_core::rotationMatrix2D(yaw1[0]);
-  Eigen::Map<Eigen::Matrix<T, 2, 1>> delta_position_pred_map(delta_position_pred);
-
+  const fuse_core::Matrix<T, 2, 2> R_yaw_inv = fuse_core::rotationMatrix2D(-yaw1[0]);
+  const Eigen::Map<Eigen::Matrix<T, 2, 1>> delta_position_pred_map(delta_position_pred);
+  
   Eigen::Map<Eigen::Matrix<T, 8, 1>> residuals_map(residual);
   residuals_map.block(0, 0, 2, 1) = R_yaw_inv * position_diff - delta_position_pred_map;
   residuals_map(2) = delta_yaw_est - delta_yaw_pred[0];