Fixed size block for function

clearpathrobotics · Sep 16, 2024 · ebee7fe · ebee7fe
1 parent 043a2ba
commit ebee7fe
Showing 1 changed file with 1 addition and 1 deletion.
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<2, 1>(0, 0) = R_yaw_inv * position_diff - delta_position_pred;
+    residuals_map.block<2, 1>(0, 0) = 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();