Update functor

clearpathrobotics · May 16, 2024 · 629dfc5 · 629dfc5
1 parent 805bca1
commit 629dfc5
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 9 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
@@ -146,7 +146,6 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
       jacobians);
 
     const double delta_yaw = parameters[6][0] - parameters[1][0];
-
     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_est = fuse_core::rotationMatrix2D(-parameters[1][0]) * (position2 - position1);

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
@@ -152,9 +152,9 @@ bool Unicycle2DStateCostFunctor::operator()(
   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;
+  init_position[0] = T(0.0);
+  init_position[1] = T(0.0);
+  init_yaw[0] = T(0.0);
   predict(
     init_position,
     init_yaw,
@@ -168,16 +168,15 @@ bool Unicycle2DStateCostFunctor::operator()(
     vel_yaw_pred,
     acc_linear_pred);
 
-  // rotate the position residual into the local frame
+  const T delta_yaw = yaw2[0] - yaw1[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_est = fuse_core::rotationMatrix2D(-yaw1[0]) * (position2_map - position1_map);
   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.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.template head<2>() = fuse_core::rotationMatrix2D(-delta_yaw) * (delta_position_est - delta_position_pred_map);
+  residuals_map(2) = delta_yaw - delta_yaw_pred[0];  
   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];