Use head over block

clearpathrobotics · Sep 17, 2024 · 076af21 · 076af21
1 parent df22203
commit 076af21
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 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
@@ -147,7 +147,7 @@ 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.block<2, 1>(0, 0) = R_yaw_inv * position_diff - delta_position_pred;
+    residuals_map.head<2>() = 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();

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
@@ -174,7 +174,7 @@ bool Unicycle2DStateCostFunctor::operator()(
   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.template head<2>() = R_yaw_inv * position_diff - delta_position_pred_map;
   residuals_map(2) = delta_yaw_est - delta_yaw_pred[0];
   residuals_map(3) = vel_linear2[0] - vel_linear_pred[0];
   residuals_map(4) = vel_linear2[1] - vel_linear_pred[1];