From 8b8a63348894ea2eeec5bf5245d995ef8b71d2e0 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 09:44:14 -0400 Subject: [PATCH] Address comments --- .../unicycle_2d_state_cost_function.h | 64 +++++++------------ .../unicycle_2d_state_cost_functor.h | 23 ++++--- .../test_unicycle_2d_state_cost_function.cpp | 10 +-- 3 files changed, 39 insertions(+), 58 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 index ff32dc75b..b55f3b453 100644 --- 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,55 +115,39 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, */ bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override { - double delta_position_pred_x; - double delta_position_pred_y; double delta_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( - 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); + fuse_core::Vector2d delta_position_pred; + fuse_core::Vector2d vel_linear_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); const double delta_yaw_est = parameters[6][0] - parameters[1][0]; const fuse_core::Vector2d position1(parameters[0][0], parameters[0][1]); const fuse_core::Vector2d position2(parameters[5][0], parameters[5][1]); const fuse_core::Vector2d position_diff = (position2 - position1); - const double sin_yaw_inv = std::sin(-parameters[1][0]); - const double cos_yaw_inv = std::cos(-parameters[1][0]); + const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]); - residuals[0] = (cos_yaw_inv * position_diff.x() - sin_yaw_inv * position_diff.y()) - delta_position_pred_x; - residuals[1] = (sin_yaw_inv * position_diff.x() + cos_yaw_inv * position_diff.y()) - delta_position_pred_y; - residuals[2] = delta_yaw_est - delta_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; + Eigen::Map residuals_map(residuals); + 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(); + 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]); + fuse_core::wrapAngle2D(residuals_map(2)); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residuals_map(residuals); residuals_map.applyOnTheLeft(A_); if (jacobians) @@ -185,8 +169,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, if (jacobians[0]) { Eigen::Map> jacobian(jacobians[0]); - const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]); - jacobian.block<2, 2>(0, 0) = R_yaw_inv * jacobian.block<2, 2>(0, 0); + jacobian.block<2, 2>(0, 0).applyOnTheLeft(R_yaw_inv); jacobian.applyOnTheLeft(-A_); } @@ -240,8 +223,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, { Eigen::Map> jacobian(jacobians[5]); jacobian = A_.block<8, 2>(0, 0); - const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]); - jacobian.block<2, 2>(0, 0) = jacobian.block<2, 2>(0, 0) * R_yaw_inv; + jacobian.block<2, 2>(0, 0) *= R_yaw_inv; } // Jacobian wrt yaw2 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 index 33523a888..7964f6859 100644 --- 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,23 +170,22 @@ bool Unicycle2DStateCostFunctor::operator()( const Eigen::Matrix position1_map(position1[0], position1[1]); const Eigen::Matrix position2_map(position2[0], position2[1]); const Eigen::Matrix position_diff = (position2_map - position1_map); - const T sin_yaw_inv = ceres::sin(-yaw1[0]); - const T cos_yaw_inv = ceres::cos(-yaw1[0]); + const fuse_core::Matrix R_yaw_inv = fuse_core::rotationMatrix2D(yaw1[0]); + Eigen::Map> delta_position_pred_map(delta_position_pred); - residual[0] = (cos_yaw_inv * position_diff.x() - sin_yaw_inv * position_diff.y()) - delta_position_pred[0]; - residual[1] = (sin_yaw_inv * position_diff.x() + cos_yaw_inv * position_diff.y()) - delta_position_pred[1]; - residual[2] = delta_yaw_est - delta_yaw_pred[0]; - residual[3] = vel_linear2[0] - vel_linear_pred[0]; - residual[4] = vel_linear2[1] - vel_linear_pred[1]; - residual[5] = vel_yaw2[0] - vel_yaw_pred[0]; - residual[6] = acc_linear2[0] - acc_linear_pred[0]; - residual[7] = acc_linear2[1] - acc_linear_pred[1]; + Eigen::Map> 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]; + 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]; + residuals_map(6) = acc_linear2[0] - acc_linear_pred[0]; + residuals_map(7) = acc_linear2[1] - acc_linear_pred[1]; - // fuse_core::wrapAngle2D(residual[2]); + fuse_core::wrapAngle2D(residuals_map(2)); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residuals_map(residual); residuals_map.applyOnTheLeft(A_.template cast()); return true; diff --git a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp index 282fc0fee..358889fa0 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -56,8 +56,8 @@ TEST(CostFunction, evaluateCostFunction) const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information }; - std::random_device dev; - std::mt19937 gen(dev()); + const auto seed = 123456789; + std::mt19937 gen(seed); std::uniform_real_distribution<> position_dist(0.0, 0.5); std::uniform_real_distribution<> yaw_dist(-M_PI / 10.0, M_PI / 10.0); @@ -110,15 +110,15 @@ TEST(CostFunction, evaluateCostFunction) // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; - #if !CERES_SUPPORTS_MANIFOLDS +#if !CERES_SUPPORTS_MANIFOLDS ceres::GradientChecker gradient_checker( &cost_function, static_cast*>(nullptr), numeric_diff_options); - #else +#else ceres::GradientChecker gradient_checker(&cost_function, static_cast*>(nullptr), numeric_diff_options); - #endif +#endif // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 ceres::GradientChecker::ProbeResults probe_results;