From 24327487d4e5076a995a03f679af61cd9c3e0dbf Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 15 Apr 2024 17:19:10 -0400 Subject: [PATCH 01/25] Rotate position in motion model constraint --- .../unicycle_2d_state_cost_functor.h | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) 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 8c1b16e4a..770e7bc66 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 @@ -40,13 +40,11 @@ #include #include - namespace fuse_models { - /** * @brief Create a cost function for a 2D state vector - * + * * The state vector includes the following quantities, given in this order: * x position * y position @@ -70,7 +68,7 @@ namespace fuse_models * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost * function of the form @@ -127,9 +125,7 @@ class Unicycle2DStateCostFunctor fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : - dt_(dt), - A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } @@ -165,9 +161,18 @@ bool Unicycle2DStateCostFunctor::operator()( vel_yaw_pred, acc_linear_pred); + // rotate the position residual into the local frame + T cos_yaw = ceres::cos(yaw1[0]); + T sin_yaw = ceres::sin(yaw1[0]); + Eigen::Matrix rotation_matrix; + rotation_matrix << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + Eigen::Matrix position_residual; + position_residual << position2[0] - position_pred[0], position2[1] - position_pred[1]; + Eigen::Matrix rotated_position_residual = rotation_matrix * position_residual; + Eigen::Map> residuals_map(residual); - residuals_map(0) = position2[0] - position_pred[0]; - residuals_map(1) = position2[1] - position_pred[1]; + residuals_map(0) = rotated_position_residual[0]; + residuals_map(1) = rotated_position_residual[1]; residuals_map(2) = yaw2[0] - yaw_pred[0]; residuals_map(3) = vel_linear2[0] - vel_linear_pred[0]; residuals_map(4) = vel_linear2[1] - vel_linear_pred[1]; From c4c810966d1ebe39af6fefb281e71c386ae15758 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Tue, 16 Apr 2024 09:50:30 -0400 Subject: [PATCH 02/25] Don't use epsilon for test --- .../src/unicycle_2d_state_kinematic_constraint.cpp | 8 +++++--- fuse_models/test/test_unicycle_2d_state_cost_function.cpp | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 1f9505a46..511ab1964 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -33,6 +33,7 @@ */ #include #include +#include #include #include @@ -43,6 +44,7 @@ #include #include +#include #include #include @@ -105,13 +107,13 @@ ceres::CostFunction* Unicycle2DStateKinematicConstraint::costFunction() const // Here we return a cost function that computes the analytic derivatives/jacobians, but we could use automatic // differentiation as follows: // - // return new ceres::AutoDiffCostFunction( - // new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); + return new ceres::AutoDiffCostFunction( + new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); // // which requires: // // #include - return new Unicycle2DStateCostFunction(dt_, sqrt_information_); + // return new Unicycle2DStateCostFunction(dt_, sqrt_information_); } } // namespace fuse_models 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 e7b1a9afd..14e899d85 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -128,7 +128,7 @@ TEST(CostFunction, evaluateCostFunction) for (size_t i = 0; i < num_parameter_blocks; ++i) { - EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) + EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-15) << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); } From 6cdf3261682baacda35b727dca483c3b1e1dc8b9 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Tue, 16 Apr 2024 11:03:14 -0400 Subject: [PATCH 03/25] Add rotation to functor --- .../unicycle_2d_state_cost_function.h | 27 ++++++++++--------- ...unicycle_2d_state_kinematic_constraint.cpp | 8 +++--- 2 files changed, 18 insertions(+), 17 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 66808102d..c7d7c94e7 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 @@ -42,13 +42,11 @@ #include - namespace fuse_models { - /** * @brief Create a cost function for a 2D state vector - * + * * The state vector includes the following quantities, given in this order: * x position * y position @@ -72,7 +70,7 @@ namespace fuse_models * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost * function of the form @@ -115,9 +113,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * computed for the parameters where jacobians[i] is not NULL. * @return The return value indicates whether the computation of the residuals and/or jacobians was successful or not. */ - bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const override + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override { double position_pred_x; double position_pred_y; @@ -147,8 +143,17 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, acc_linear_pred_y, jacobians); - residuals[0] = parameters[5][0] - position_pred_x; - residuals[1] = parameters[5][1] - position_pred_y; + // rotate the position residual into the local frame + double cos_yaw = std::cos(parameters[1][0]); + double sin_yaw = std::sin(parameters[1][0]); + Eigen::Matrix2d rotation_matrix; + rotation_matrix << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + Eigen::Vector2d position_residual; + position_residual << parameters[5][0] - position_pred_x, parameters[5][1] - position_pred_y; + Eigen::Vector2d rotated_position_residual = rotation_matrix * position_residual; + + residuals[0] = rotated_position_residual[0]; + residuals[1] = rotated_position_residual[1]; residuals[2] = parameters[6][0] - yaw_pred; residuals[3] = parameters[7][0] - vel_linear_pred_x; residuals[4] = parameters[7][1] - vel_linear_pred_y; @@ -274,9 +279,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : - dt_(dt), - A_(A) +Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 511ab1964..1f9505a46 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -33,7 +33,6 @@ */ #include #include -#include #include #include @@ -44,7 +43,6 @@ #include #include -#include #include #include @@ -107,13 +105,13 @@ ceres::CostFunction* Unicycle2DStateKinematicConstraint::costFunction() const // Here we return a cost function that computes the analytic derivatives/jacobians, but we could use automatic // differentiation as follows: // - return new ceres::AutoDiffCostFunction( - new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); + // return new ceres::AutoDiffCostFunction( + // new Unicycle2DStateCostFunctor(dt_, sqrt_information_)); // // which requires: // // #include - // return new Unicycle2DStateCostFunction(dt_, sqrt_information_); + return new Unicycle2DStateCostFunction(dt_, sqrt_information_); } } // namespace fuse_models From de94366f4a3f77d846c72b5d44e6038b5091346b Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 15 May 2024 10:03:07 -0400 Subject: [PATCH 04/25] Remove eigen transformations --- .../unicycle_2d_state_cost_function.h | 17 +++++++---------- .../unicycle_2d_state_cost_functor.h | 15 ++++++--------- 2 files changed, 13 insertions(+), 19 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 c7d7c94e7..0f641a842 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 @@ -144,16 +144,13 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, jacobians); // rotate the position residual into the local frame - double cos_yaw = std::cos(parameters[1][0]); - double sin_yaw = std::sin(parameters[1][0]); - Eigen::Matrix2d rotation_matrix; - rotation_matrix << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; - Eigen::Vector2d position_residual; - position_residual << parameters[5][0] - position_pred_x, parameters[5][1] - position_pred_y; - Eigen::Vector2d rotated_position_residual = rotation_matrix * position_residual; - - residuals[0] = rotated_position_residual[0]; - residuals[1] = rotated_position_residual[1]; + auto delta_x = parameters[5][0] - position_pred_x; + auto delta_y = parameters[5][1] - position_pred_y; + auto sin_pred_inv = std::sin(-yaw_pred); + auto cos_pred_inv= std::cos(-yaw_pred); + residuals[0] = cos_pred_inv * delta_x - sin_pred_inv * delta_y; + residuals[1] = sin_pred_inv * delta_x + cos_pred_inv * delta_y; + residuals[2] = parameters[6][0] - yaw_pred; residuals[3] = parameters[7][0] - vel_linear_pred_x; residuals[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 index 770e7bc66..d3f4f0503 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 @@ -162,17 +162,14 @@ bool Unicycle2DStateCostFunctor::operator()( acc_linear_pred); // rotate the position residual into the local frame - T cos_yaw = ceres::cos(yaw1[0]); - T sin_yaw = ceres::sin(yaw1[0]); - Eigen::Matrix rotation_matrix; - rotation_matrix << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; - Eigen::Matrix position_residual; - position_residual << position2[0] - position_pred[0], position2[1] - position_pred[1]; - Eigen::Matrix rotated_position_residual = rotation_matrix * position_residual; + T delta_x = parameters[5][0] - position_pred_x; + T delta_y = parameters[5][1] - position_pred_y; + T sin_pred_inv = ceres::sin(-yaw_pred); + T cos_pred_inv = ceres::cos(-yaw_pred); Eigen::Map> residuals_map(residual); - residuals_map(0) = rotated_position_residual[0]; - residuals_map(1) = rotated_position_residual[1]; + residuals_map(0) = cos_pred_inv * delta_x - sin_pred_inv * delta_y; + residuals_map(1) = sin_pred_inv * delta_x + cos_pred_inv * delta_y; residuals_map(2) = yaw2[0] - yaw_pred[0]; residuals_map(3) = vel_linear2[0] - vel_linear_pred[0]; residuals_map(4) = vel_linear2[1] - vel_linear_pred[1]; From 50a30ba96d6a23784e2a8c5391c0dbbc538af39e Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 15 May 2024 10:05:33 -0400 Subject: [PATCH 05/25] Revert formatting --- .../fuse_models/unicycle_2d_state_cost_function.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 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 0f641a842..70b500d40 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 @@ -113,7 +113,9 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * computed for the parameters where jacobians[i] is not NULL. * @return The return value indicates whether the computation of the residuals and/or jacobians was successful or not. */ - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { double position_pred_x; double position_pred_y; @@ -148,9 +150,9 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, auto delta_y = parameters[5][1] - position_pred_y; auto sin_pred_inv = std::sin(-yaw_pred); auto cos_pred_inv= std::cos(-yaw_pred); + residuals[0] = cos_pred_inv * delta_x - sin_pred_inv * delta_y; residuals[1] = sin_pred_inv * delta_x + cos_pred_inv * delta_y; - residuals[2] = parameters[6][0] - yaw_pred; residuals[3] = parameters[7][0] - vel_linear_pred_x; residuals[4] = parameters[7][1] - vel_linear_pred_y; @@ -276,7 +278,9 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) +Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : + dt_(dt), + A_(A) { } From 895814191a84aef2235034a713b651ac26d2d122 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 15 May 2024 10:07:33 -0400 Subject: [PATCH 06/25] Revert formatting --- .../fuse_models/unicycle_2d_state_cost_function.h | 6 ++++-- .../fuse_models/unicycle_2d_state_cost_functor.h | 12 ++++++++---- 2 files changed, 12 insertions(+), 6 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 70b500d40..69625f354 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 @@ -42,11 +42,13 @@ #include + namespace fuse_models { + /** * @brief Create a cost function for a 2D state vector - * + * * The state vector includes the following quantities, given in this order: * x position * y position @@ -56,7 +58,7 @@ namespace fuse_models * yaw velocity * x acceleration * y acceleration - * + * * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that * applies a prior constraint on both the entire state vector. * 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 d3f4f0503..f10b551df 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 @@ -40,11 +40,13 @@ #include #include + namespace fuse_models { + /** * @brief Create a cost function for a 2D state vector - * + * * The state vector includes the following quantities, given in this order: * x position * y position @@ -54,7 +56,7 @@ namespace fuse_models * yaw velocity * x acceleration * y acceleration - * + * * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that * applies a prior constraint on both the entire state vector. * @@ -68,7 +70,7 @@ namespace fuse_models * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost * function of the form @@ -125,7 +127,9 @@ class Unicycle2DStateCostFunctor fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : + dt_(dt), + A_(A) { } From d178d86d97ecd6d0a6de69c8c2a2d3376e101da7 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 15 May 2024 10:08:26 -0400 Subject: [PATCH 07/25] Revert formatting --- .../include/fuse_models/unicycle_2d_state_cost_function.h | 4 ++-- .../include/fuse_models/unicycle_2d_state_cost_functor.h | 2 +- 2 files changed, 3 insertions(+), 3 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 69625f354..f04d0e28a 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 @@ -58,7 +58,7 @@ namespace fuse_models * yaw velocity * x acceleration * y acceleration - * + * * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that * applies a prior constraint on both the entire state vector. * @@ -72,7 +72,7 @@ namespace fuse_models * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost * function of the form 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 f10b551df..25e3be776 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 @@ -56,7 +56,7 @@ namespace fuse_models * yaw velocity * x acceleration * y acceleration - * + * * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that * applies a prior constraint on both the entire state vector. * From e5e4c152f37a8e76df5cf4d86cfbc38bd88900ae Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 15 May 2024 13:01:21 -0400 Subject: [PATCH 08/25] Start at identity in predict function --- .../unicycle_2d_state_cost_function.h | 20 +++++------ .../unicycle_2d_state_cost_functor.h | 33 +++++++++---------- 2 files changed, 25 insertions(+), 28 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 f04d0e28a..c17fecb12 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 @@ -119,8 +119,8 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, double* residuals, double** jacobians) const override { - double position_pred_x; - double position_pred_y; + double delta_position_pred_x; + double delta_position_pred_y; double yaw_pred; double vel_linear_pred_x; double vel_linear_pred_y; @@ -128,17 +128,17 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, double acc_linear_pred_x; double acc_linear_pred_y; predict( - parameters[0][0], // position1_x - parameters[0][1], // position1_y - parameters[1][0], // yaw1 + 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_, - position_pred_x, - position_pred_y, + delta_position_pred_x, + delta_position_pred_y, yaw_pred, vel_linear_pred_x, vel_linear_pred_y, @@ -148,13 +148,11 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, jacobians); // rotate the position residual into the local frame - auto delta_x = parameters[5][0] - position_pred_x; - auto delta_y = parameters[5][1] - position_pred_y; auto sin_pred_inv = std::sin(-yaw_pred); auto cos_pred_inv= std::cos(-yaw_pred); - residuals[0] = cos_pred_inv * delta_x - sin_pred_inv * delta_y; - residuals[1] = sin_pred_inv * delta_x + cos_pred_inv * delta_y; + residuals[0] = cos_pred_inv * delta_position_pred_x - sin_pred_inv * delta_position_pred_y; + residuals[1] = sin_pred_inv * delta_position_pred_x + cos_pred_inv * delta_position_pred_y; residuals[2] = parameters[6][0] - yaw_pred; residuals[3] = parameters[7][0] - vel_linear_pred_x; residuals[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 index 25e3be776..3fe0a38b0 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 @@ -40,13 +40,11 @@ #include #include - namespace fuse_models { - /** * @brief Create a cost function for a 2D state vector - * + * * The state vector includes the following quantities, given in this order: * x position * y position @@ -70,7 +68,7 @@ namespace fuse_models * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost * function of the form @@ -127,9 +125,7 @@ class Unicycle2DStateCostFunctor fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : - dt_(dt), - A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } @@ -147,33 +143,36 @@ bool Unicycle2DStateCostFunctor::operator()( const T* const acc_linear2, T* residual) const { - T position_pred[2]; + T delta_position_pred[2]; T yaw_pred[1]; T vel_linear_pred[2]; T vel_yaw_pred[1]; 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; predict( - position1, - yaw1, + init_position, + init_yaw, vel_linear1, vel_yaw1, acc_linear1, T(dt_), - position_pred, + delta_position_pred, yaw_pred, vel_linear_pred, vel_yaw_pred, acc_linear_pred); // rotate the position residual into the local frame - T delta_x = parameters[5][0] - position_pred_x; - T delta_y = parameters[5][1] - position_pred_y; - T sin_pred_inv = ceres::sin(-yaw_pred); - T cos_pred_inv = ceres::cos(-yaw_pred); + T sin_pred_inv = ceres::sin(-yaw_pred[0]); + T cos_pred_inv = ceres::cos(-yaw_pred[0]); Eigen::Map> residuals_map(residual); - residuals_map(0) = cos_pred_inv * delta_x - sin_pred_inv * delta_y; - residuals_map(1) = sin_pred_inv * delta_x + cos_pred_inv * delta_y; + residuals_map(0) = cos_pred_inv * delta_position_pred[0] - sin_pred_inv * delta_position_pred[1]; + residuals_map(1) = sin_pred_inv * delta_position_pred[0] + cos_pred_inv * delta_position_pred[1]; residuals_map(2) = yaw2[0] - yaw_pred[0]; residuals_map(3) = vel_linear2[0] - vel_linear_pred[0]; residuals_map(4) = vel_linear2[1] - vel_linear_pred[1]; From 7512da275485f30d2da8c6346ce30c9eacb38fb0 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 15 May 2024 13:08:02 -0400 Subject: [PATCH 09/25] Formatting --- .../include/fuse_models/unicycle_2d_state_cost_functor.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 3fe0a38b0..18afbc87d 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 @@ -125,7 +125,9 @@ class Unicycle2DStateCostFunctor fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : + dt_(dt), + A_(A) { } From d24b42b56c882d3eb23beaf09494b6f656ec6087 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 15 May 2024 17:27:55 -0400 Subject: [PATCH 10/25] Compute measured delta --- .../unicycle_2d_state_cost_function.h | 25 ++++++++++--------- .../unicycle_2d_state_cost_functor.h | 16 ++++++------ .../test_unicycle_2d_state_cost_function.cpp | 12 ++++++++- 3 files changed, 33 insertions(+), 20 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 c17fecb12..b0dfb7c8b 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 @@ -147,24 +147,25 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, acc_linear_pred_y, jacobians); - // rotate the position residual into the local frame - auto sin_pred_inv = std::sin(-yaw_pred); - auto cos_pred_inv= std::cos(-yaw_pred); + 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 = fuse_core::rotationMatrix2D(-parameters[1][0]) * (position2 - position1); + const double delta_yaw = parameters[6][0] - parameters[1][0]; + const Eigen::Vector2d delta_position_pred(delta_position_pred_x, delta_position_pred_y); - residuals[0] = cos_pred_inv * delta_position_pred_x - sin_pred_inv * delta_position_pred_y; - residuals[1] = sin_pred_inv * delta_position_pred_x + cos_pred_inv * delta_position_pred_y; - residuals[2] = parameters[6][0] - 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>() = fuse_core::rotationMatrix2D(-delta_yaw) * (delta_position - delta_position_pred); + residuals_map(2) = delta_yaw - yaw_pred; // omitting fuse_core::wrapAngle2D because it is done later on + 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]); // 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) 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 18afbc87d..e887330de 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 @@ -146,7 +146,7 @@ bool Unicycle2DStateCostFunctor::operator()( T* residual) const { T delta_position_pred[2]; - T yaw_pred[1]; + T delta_yaw_pred[1]; T vel_linear_pred[2]; T vel_yaw_pred[1]; T acc_linear_pred[2]; @@ -163,19 +163,21 @@ bool Unicycle2DStateCostFunctor::operator()( acc_linear1, T(dt_), delta_position_pred, - yaw_pred, + delta_yaw_pred, vel_linear_pred, vel_yaw_pred, acc_linear_pred); // rotate the position residual into the local frame - T sin_pred_inv = ceres::sin(-yaw_pred[0]); - T cos_pred_inv = ceres::cos(-yaw_pred[0]); + const Eigen::Matrix position1_map(position1[0], position1[1]); + const Eigen::Matrix position2_map(position2[0], position2[1]); + const Eigen::Matrix 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 delta_position_pred_map(delta_position_pred[0], delta_position_pred[1]); Eigen::Map> residuals_map(residual); - residuals_map(0) = cos_pred_inv * delta_position_pred[0] - sin_pred_inv * delta_position_pred[1]; - residuals_map(1) = sin_pred_inv * delta_position_pred[0] + cos_pred_inv * delta_position_pred[1]; - residuals_map(2) = yaw2[0] - yaw_pred[0]; + 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(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]; 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 14e899d85..6fe8b9c69 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -99,7 +99,17 @@ TEST(CostFunction, evaluateCostFunction) // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; - ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); +// #if !CERES_SUPPORTS_MANIFOLDS +// ceres::GradientChecker gradient_checker( +// &cost_function, +// static_cast*>(nullptr), +// numeric_diff_options); +// #else + ceres::GradientChecker gradient_checker( + &cost_function, + static_cast*>(nullptr), + numeric_diff_options); +// #endif // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 ceres::GradientChecker::ProbeResults probe_results; From 805bca181a74cbf1af3a1714e24b5dde8ae83333 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Thu, 16 May 2024 13:47:10 -0400 Subject: [PATCH 11/25] Update jacobian computation --- .../unicycle_2d_state_cost_function.h | 62 +++++++++++++------ .../test_unicycle_2d_state_cost_function.cpp | 12 +--- 2 files changed, 45 insertions(+), 29 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 b0dfb7c8b..1cb0a6a82 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 @@ -42,13 +42,11 @@ #include - namespace fuse_models { - /** * @brief Create a cost function for a 2D state vector - * + * * The state vector includes the following quantities, given in this order: * x position * y position @@ -72,7 +70,7 @@ namespace fuse_models * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function * that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost * function of the form @@ -147,15 +145,17 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, acc_linear_pred_y, 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 = fuse_core::rotationMatrix2D(-parameters[1][0]) * (position2 - position1); - const double delta_yaw = parameters[6][0] - parameters[1][0]; + const Eigen::Vector2d delta_position_est = fuse_core::rotationMatrix2D(-parameters[1][0]) * (position2 - position1); const Eigen::Vector2d delta_position_pred(delta_position_pred_x, delta_position_pred_y); + const fuse_core::Matrix2d R_delta_yaw_inv = fuse_core::rotationMatrix2D(-delta_yaw); Eigen::Map> residuals_map(residuals); - residuals_map.head<2>() = fuse_core::rotationMatrix2D(-delta_yaw) * (delta_position - delta_position_pred); - residuals_map(2) = delta_yaw - yaw_pred; // omitting fuse_core::wrapAngle2D because it is done later on + residuals_map.head<2>() = R_delta_yaw_inv * (delta_position_pred - delta_position_est); + residuals_map(2) = delta_yaw - 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; @@ -183,25 +183,47 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // } // } + // For the following jacobian computations we consider E to be the full residual, and Ep to be the top 2 rows of + // the residual (wrt the first position) + // We then consider L to be the full "prediction" vector from the predict function, and Lp is the top 2 rows of + // the prediction vector where the prediction vector is defined as: + // [ + // delta_position_pred_x + // delta_position_pred_y + // yaw_pred, + // vel_linear_pred_x, + // vel_linear_pred_y, + // vel_yaw_pred, + // acc_linear_pred_x, + // acc_linear_pred_y, + // ] + fuse_core::Matrix d_Ep_d_Lp = R_delta_yaw_inv; + // Update jacobian wrt position1 if (jacobians[0]) { - Eigen::Map> jacobian(jacobians[0]); - jacobian.applyOnTheLeft(-A_); + Eigen::Map> d_L_d_position1(jacobians[0]); + fuse_core::Matrix d_Lp_d_position1 = d_L_d_position1.block<2, 2>(0, 0); + d_L_d_position1.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_position1; + d_L_d_position1.applyOnTheLeft(-A_); } // Update jacobian wrt yaw1 if (jacobians[1]) { - Eigen::Map jacobian(jacobians[1]); - jacobian.applyOnTheLeft(-A_); + Eigen::Map d_L_d_yaw1(jacobians[1]); + fuse_core::Vector2d d_Lp_d_yaw1 = d_L_d_yaw1.head<2>(); + d_L_d_yaw1.head<2>() = d_Ep_d_Lp * d_Lp_d_yaw1; + d_L_d_yaw1.applyOnTheLeft(-A_); } // Update jacobian wrt vel_linear1 if (jacobians[2]) { - Eigen::Map> jacobian(jacobians[2]); - jacobian.applyOnTheLeft(-A_); + Eigen::Map> d_L_d_vel_linear1(jacobians[2]); + fuse_core::Matrix d_Lp_d_vel_linear1 = d_L_d_vel_linear1.block<2, 2>(0, 0); + d_L_d_vel_linear1.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_vel_linear1; + d_L_d_vel_linear1.applyOnTheLeft(-A_); } // Update jacobian wrt vel_yaw1 @@ -214,8 +236,10 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // Update jacobian wrt acc_linear1 if (jacobians[4]) { - Eigen::Map> jacobian(jacobians[4]); - jacobian.applyOnTheLeft(-A_); + Eigen::Map> d_L_d_acc_linear1(jacobians[4]); + fuse_core::Matrix d_Lp_d_acc_linear1 = d_L_d_acc_linear1.block<2, 2>(0, 0); + d_L_d_acc_linear1.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_acc_linear1; + d_L_d_acc_linear1.applyOnTheLeft(-A_); } // It might be possible to simplify the code below implementing something like this but using compile-time @@ -238,8 +262,10 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // Jacobian wrt position2 if (jacobians[5]) { - Eigen::Map> jacobian(jacobians[5]); - jacobian = A_.block<8, 2>(0, 0); + Eigen::Map> d_L_d_position2(jacobians[5]); + d_L_d_position2 = A_.block<8, 2>(0, 0); + fuse_core::Matrix d_Lp_d_position2 = d_L_d_position2.block<2, 2>(0, 0); + d_L_d_position2.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_position2; } // Jacobian wrt yaw2 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 6fe8b9c69..14e899d85 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -99,17 +99,7 @@ TEST(CostFunction, evaluateCostFunction) // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; -// #if !CERES_SUPPORTS_MANIFOLDS -// ceres::GradientChecker gradient_checker( -// &cost_function, -// static_cast*>(nullptr), -// numeric_diff_options); -// #else - ceres::GradientChecker gradient_checker( - &cost_function, - static_cast*>(nullptr), - numeric_diff_options); -// #endif + ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 ceres::GradientChecker::ProbeResults probe_results; From 629dfc507c62dea26b2657098f952ad4315d9b5c Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Thu, 16 May 2024 13:49:25 -0400 Subject: [PATCH 12/25] Update functor --- .../fuse_models/unicycle_2d_state_cost_function.h | 1 - .../fuse_models/unicycle_2d_state_cost_functor.h | 15 +++++++-------- 2 files changed, 7 insertions(+), 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 index 1cb0a6a82..0ba61c681 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 @@ -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 index e887330de..bc67ba487 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 @@ -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 position1_map(position1[0], position1[1]); const Eigen::Matrix position2_map(position2[0], position2[1]); - const Eigen::Matrix 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 delta_position_est = fuse_core::rotationMatrix2D(-yaw1[0]) * (position2_map - position1_map); const Eigen::Matrix delta_position_pred_map(delta_position_pred[0], delta_position_pred[1]); Eigen::Map> 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]; From 6b51158bfa82b901f5b166be5e0d43369c72e27f Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 3 Jun 2024 09:30:54 -0400 Subject: [PATCH 13/25] Temp changes --- .../unicycle_2d_state_cost_function.h | 103 ++++----- .../unicycle_2d_state_cost_functor.h | 30 +-- .../test_unicycle_2d_state_cost_function.cpp | 203 +++++++++++------- 3 files changed, 189 insertions(+), 147 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 0ba61c681..b7bcb209b 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 @@ -113,13 +113,11 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * computed for the parameters where jacobians[i] is not NULL. * @return The return value indicates whether the computation of the residuals and/or jacobians was successful or not. */ - bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const override + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override { double delta_position_pred_x; double delta_position_pred_y; - double yaw_pred; + double delta_yaw_pred; double vel_linear_pred_x; double vel_linear_pred_y; double vel_yaw_pred; @@ -137,7 +135,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, dt_, delta_position_pred_x, delta_position_pred_y, - yaw_pred, + delta_yaw_pred, vel_linear_pred_x, vel_linear_pred_y, vel_yaw_pred, @@ -145,26 +143,27 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, acc_linear_pred_y, 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); - const Eigen::Vector2d delta_position_pred(delta_position_pred_x, delta_position_pred_y); - const fuse_core::Matrix2d R_delta_yaw_inv = fuse_core::rotationMatrix2D(-delta_yaw); + 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]); - Eigen::Map> residuals_map(residuals); - residuals_map.head<2>() = R_delta_yaw_inv * (delta_position_pred - delta_position_est); - residuals_map(2) = delta_yaw - 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; + 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; fuse_core::wrapAngle2D(residuals[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) @@ -182,47 +181,39 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // } // } - // For the following jacobian computations we consider E to be the full residual, and Ep to be the top 2 rows of - // the residual (wrt the first position) - // We then consider L to be the full "prediction" vector from the predict function, and Lp is the top 2 rows of - // the prediction vector where the prediction vector is defined as: - // [ - // delta_position_pred_x - // delta_position_pred_y - // yaw_pred, - // vel_linear_pred_x, - // vel_linear_pred_y, - // vel_yaw_pred, - // acc_linear_pred_x, - // acc_linear_pred_y, - // ] - fuse_core::Matrix d_Ep_d_Lp = R_delta_yaw_inv; - // Update jacobian wrt position1 if (jacobians[0]) { - Eigen::Map> d_L_d_position1(jacobians[0]); - fuse_core::Matrix d_Lp_d_position1 = d_L_d_position1.block<2, 2>(0, 0); - d_L_d_position1.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_position1; - d_L_d_position1.applyOnTheLeft(-A_); + Eigen::Map> djacobian(jacobians[0]); + fuse_core::Matrix d_DPP_d_position1 = jacobian.block<2, 2>(0, 0); + jacobian.block<2, 2>(0, 0) = R_yaw_inv * d_DPP_d_position1; + jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt yaw1 if (jacobians[1]) { - Eigen::Map d_L_d_yaw1(jacobians[1]); - fuse_core::Vector2d d_Lp_d_yaw1 = d_L_d_yaw1.head<2>(); - d_L_d_yaw1.head<2>() = d_Ep_d_Lp * d_Lp_d_yaw1; - d_L_d_yaw1.applyOnTheLeft(-A_); + const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]); + const double cos_yaw = std::cos(parameters[1][0]); + const double sin_yaw = std::sin(parameters[1][0]); + + const fuse_core::Vector2d d_DPE_d_yaw1( + -position_diff.x() * sin_yaw + position_diff.y() * cos_yaw, + -position_diff.x() * cos_yaw - position_diff.y() * sin_yaw); + + Eigen::Map d_pred_d_yaw1(jacobians[1]); + fuse_core::Vector2d d_DPP_d_yaw1 = d_pred_d_yaw1.head<2>(); + // d_L_d_yaw1(0) = d_DPE_d_yaw1[0] * d_Lp_d_yaw1[0]; + // d_L_d_yaw1(1) = d_DPE_d_yaw1[1] * d_Lp_d_yaw1[1]; + d_pred_d_yaw1.head<2>() = R_yaw_inv.transpose() * (d_DPE_d_yaw1 - d_DPP_d_yaw1); + d_pred_d_yaw1.applyOnTheLeft(-A_); } // Update jacobian wrt vel_linear1 if (jacobians[2]) { - Eigen::Map> d_L_d_vel_linear1(jacobians[2]); - fuse_core::Matrix d_Lp_d_vel_linear1 = d_L_d_vel_linear1.block<2, 2>(0, 0); - d_L_d_vel_linear1.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_vel_linear1; - d_L_d_vel_linear1.applyOnTheLeft(-A_); + Eigen::Map> jacobian(jacobians[2]); + jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_yaw1 @@ -235,10 +226,8 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // Update jacobian wrt acc_linear1 if (jacobians[4]) { - Eigen::Map> d_L_d_acc_linear1(jacobians[4]); - fuse_core::Matrix d_Lp_d_acc_linear1 = d_L_d_acc_linear1.block<2, 2>(0, 0); - d_L_d_acc_linear1.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_acc_linear1; - d_L_d_acc_linear1.applyOnTheLeft(-A_); + Eigen::Map> jacobian(jacobians[4]); + jacobian.applyOnTheLeft(-A_); } // It might be possible to simplify the code below implementing something like this but using compile-time @@ -261,10 +250,10 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // Jacobian wrt position2 if (jacobians[5]) { - Eigen::Map> d_L_d_position2(jacobians[5]); - d_L_d_position2 = A_.block<8, 2>(0, 0); - fuse_core::Matrix d_Lp_d_position2 = d_L_d_position2.block<2, 2>(0, 0); - d_L_d_position2.block<2, 2>(0, 0) = d_Ep_d_Lp * d_Lp_d_position2; + Eigen::Map> jacobian(jacobians[5]); + jacobian = A_.block<8, 2>(0, 0); + fuse_core::Matrix d_DPP_d_position2 = jacobian.block<2, 2>(0, 0); + jacobian.block<2, 2>(0, 0) = R_yaw_inv * d_DPP_d_position2; } // Jacobian wrt yaw2 @@ -304,9 +293,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : - dt_(dt), - A_(A) +Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } 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 bc67ba487..e46b19c53 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 @@ -125,9 +125,7 @@ class Unicycle2DStateCostFunctor fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : - dt_(dt), - A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } @@ -168,25 +166,27 @@ bool Unicycle2DStateCostFunctor::operator()( vel_yaw_pred, acc_linear_pred); - const T delta_yaw = yaw2[0] - yaw1[0]; + const T delta_yaw_est = yaw2[0] - yaw1[0]; const Eigen::Matrix position1_map(position1[0], position1[1]); const Eigen::Matrix position2_map(position2[0], position2[1]); - const Eigen::Matrix delta_position_est = fuse_core::rotationMatrix2D(-yaw1[0]) * (position2_map - position1_map); - const Eigen::Matrix delta_position_pred_map(delta_position_pred[0], delta_position_pred[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]); - Eigen::Map> residuals_map(residual); - 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]; - residuals_map(6) = acc_linear2[0] - acc_linear_pred[0]; - residuals_map(7) = acc_linear2[1] - acc_linear_pred[1]; + 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]; - fuse_core::wrapAngle2D(residuals_map(2)); + fuse_core::wrapAngle2D(residual[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 14e899d85..b78750892 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -34,107 +34,162 @@ #include #include -#include #include +#include +#include #include #include -#include #include +#include #include - TEST(CostFunction, evaluateCostFunction) { // Create cost function const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d covariance = fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); - const double dt{ 0.1 }; - const fuse_core::Matrix8d sqrt_information{ covariance.inverse().llt().matrixU() }; - - const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information }; + const double dt { 0.1 }; + const fuse_core::Matrix8d sqrt_information { covariance.inverse().llt().matrixU() }; - // Evaluate cost function - const double position1[] = {0.0, 0.0}; - const double yaw1[] = {0.0}; - const double vel_linear1[] = {1.0, 0.0}; - const double vel_yaw1[] = {1.570796327}; - const double acc_linear1[] = {1.0, 0.0}; + const fuse_models::Unicycle2DStateCostFunction cost_function { dt, sqrt_information }; - const double position2[] = {0.105, 0.0}; - const double yaw2[] = {0.1570796327}; - const double vel_linear2[] = {1.1, 0.0}; - const double vel_yaw2[] = {1.570796327}; - const double acc_linear2[] = {1.0, 0.0}; + std::random_device dev; + std::mt19937 gen(dev()); + std::uniform_real_distribution<> position_dist(0.0, 0.5); + std::uniform_real_distribution<> yaw_dist(-M_PI / 10.0, M_PI / 10.0); - const double* parameters[] = + std::size_t N = 100; + for (std::size_t i = 0; i < N; i++) { - position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 - }; - - fuse_core::Vector8d residuals; - - const auto& block_sizes = cost_function.parameter_block_sizes(); - const auto num_parameter_blocks = block_sizes.size(); - - const auto num_residuals = cost_function.num_residuals(); - - std::vector J(num_parameter_blocks); - std::vector jacobians(num_parameter_blocks); - - for (size_t i = 0; i < num_parameter_blocks; ++i) - { - J[i].resize(num_residuals, block_sizes[i]); - jacobians[i] = J[i].data(); - } - - EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); - - // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 above the residuals - // are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 - EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15); - - // Check jacobians are correct using a gradient checker - ceres::NumericDiffOptions numeric_diff_options; - ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); - - // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 - ceres::GradientChecker::ProbeResults probe_results; - // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, but Probe actually - // returns true and the jacobians are correct according to the gradient checker numeric differentiation - // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << probe_results.error_log; - - // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunction + // Randomly generate first state + double position1[] = { position_dist(gen), position_dist(gen) }; + double yaw1[] = { yaw_dist(gen) }; + double vel_linear1[] = { position_dist(gen), position_dist(gen) }; + double vel_yaw1[] = { yaw_dist(gen) / 10.0 }; + double acc_linear1[] = { vel_linear1[0] / dt, vel_linear1[1] / dt }; + + // Compute second state from first + double vel_linear2[2]; + double position2[2]; + double yaw2[1]; + double vel_yaw2[1]; + double acc_linear2[2]; + fuse_models::predict( + position1, + yaw1, + vel_linear1, + vel_yaw1, + acc_linear1, + dt, + position2, + yaw2, + vel_linear2, + vel_yaw2, + acc_linear2); + + position1[0] -= 0.2; + position1[1] -= 0.21; + yaw1[0] -= 0.01; + vel_linear1[0] -= 0.01; + vel_linear1[1] -= 0.01; + vel_yaw1[0] -= 0.01; + acc_linear1[0] -= 0.01; + acc_linear1[1] -= 0.01; + + position2[0] += 0.2; + position2[1] += 0.21; + yaw2[0] += 0.01; + vel_linear2[0] += 0.01; + vel_linear2[1] += 0.01; + vel_yaw2[0] += 0.01; + acc_linear2[0] += 0.01; + acc_linear2[1] += 0.01; + // // Compute second state from first + // const double vel_linear2[] = { vel_linear1[0] + acc_linear1[0] * dt, vel_linear1[1] + acc_linear1[1] * dt }; + // const double position2[] = { position1[0] + (vel_linear1[0] + vel_linear2[0]) * 0.5 * dt, + // position1[1] + (vel_linear1[1] + vel_linear2[1]) * 0.5 * dt }; + // const double yaw2[] = { yaw1[0] + vel_yaw1[0] * dt }; + // const double vel_yaw2[] = { vel_yaw1[0] }; + // const double acc_linear2[] = { acc_linear1[0], acc_linear1[1] }; + + const double* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; + + fuse_core::Vector8d residuals; + + const auto& block_sizes = cost_function.parameter_block_sizes(); + const auto num_parameter_blocks = block_sizes.size(); + + const auto num_residuals = cost_function.num_residuals(); + + std::vector J(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); + + for (size_t i = 0; i < num_parameter_blocks; ++i) + { + J[i].resize(num_residuals, block_sizes[i]); + jacobians[i] = J[i].data(); + } + + EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); + + // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 above the + // residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 + EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-13); + + // Check jacobians are correct using a gradient checker + ceres::NumericDiffOptions numeric_diff_options; + // #if !CERES_SUPPORTS_MANIFOLDS + // ceres::GradientChecker gradient_checker( + // &cost_function, + // static_cast*>(nullptr), + // numeric_diff_options); + // #else + ceres::GradientChecker gradient_checker( + &cost_function, + static_cast*>(nullptr), + numeric_diff_options); + // #endif + + // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 + ceres::GradientChecker::ProbeResults probe_results; + // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, but Probe actually + // returns true and the jacobians are correct according to the gradient checker numeric differentiation + // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << probe_results.error_log; + + // Create cost function using automatic differentiation on the cost functor + ceres::AutoDiffCostFunction cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); - // Evaluate cost function that uses automatic differentiation - std::vector J_autodiff(num_parameter_blocks); - std::vector jacobians_autodiff(num_parameter_blocks); + // Evaluate cost function that uses automatic differentiation + std::vector J_autodiff(num_parameter_blocks); + std::vector jacobians_autodiff(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) - { - J_autodiff[i].resize(num_residuals, block_sizes[i]); - jacobians_autodiff[i] = J_autodiff[i].data(); - } + for (size_t i = 0; i < num_parameter_blocks; ++i) + { + J_autodiff[i].resize(num_residuals, block_sizes[i]); + jacobians_autodiff[i] = J_autodiff[i].data(); + } - EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians_autodiff.data())); + EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians_autodiff.data())); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - for (size_t i = 0; i < num_parameter_blocks; ++i) - { - EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-15) - << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + for (size_t i = 0; i < num_parameter_blocks; ++i) + { + // EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-15) + // << "Autodiff Jacobian[" << i << "] =\n" + // << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + // << J[i].format(HeavyFmt); + EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-13) << "[" << i << "] \n"; + } } } -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From fe8d05ee61fc447887666b0347d9a3342f382820 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Fri, 13 Sep 2024 13:23:58 -0400 Subject: [PATCH 14/25] Fix jacobians --- .../unicycle_2d_state_cost_function.h | 26 ++---- .../unicycle_2d_state_cost_functor.h | 2 +- .../test_unicycle_2d_state_cost_function.cpp | 89 ++++++------------- 3 files changed, 35 insertions(+), 82 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 b7bcb209b..ff32dc75b 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 @@ -184,29 +184,17 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // Update jacobian wrt position1 if (jacobians[0]) { - Eigen::Map> djacobian(jacobians[0]); - fuse_core::Matrix d_DPP_d_position1 = jacobian.block<2, 2>(0, 0); - jacobian.block<2, 2>(0, 0) = R_yaw_inv * d_DPP_d_position1; + 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.applyOnTheLeft(-A_); } // Update jacobian wrt yaw1 if (jacobians[1]) { - const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]); - const double cos_yaw = std::cos(parameters[1][0]); - const double sin_yaw = std::sin(parameters[1][0]); - - const fuse_core::Vector2d d_DPE_d_yaw1( - -position_diff.x() * sin_yaw + position_diff.y() * cos_yaw, - -position_diff.x() * cos_yaw - position_diff.y() * sin_yaw); - - Eigen::Map d_pred_d_yaw1(jacobians[1]); - fuse_core::Vector2d d_DPP_d_yaw1 = d_pred_d_yaw1.head<2>(); - // d_L_d_yaw1(0) = d_DPE_d_yaw1[0] * d_Lp_d_yaw1[0]; - // d_L_d_yaw1(1) = d_DPE_d_yaw1[1] * d_Lp_d_yaw1[1]; - d_pred_d_yaw1.head<2>() = R_yaw_inv.transpose() * (d_DPE_d_yaw1 - d_DPP_d_yaw1); - d_pred_d_yaw1.applyOnTheLeft(-A_); + Eigen::Map jacobian(jacobians[1]); + jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_linear1 @@ -252,8 +240,8 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, { Eigen::Map> jacobian(jacobians[5]); jacobian = A_.block<8, 2>(0, 0); - fuse_core::Matrix d_DPP_d_position2 = jacobian.block<2, 2>(0, 0); - jacobian.block<2, 2>(0, 0) = R_yaw_inv * d_DPP_d_position2; + 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 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 e46b19c53..33523a888 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 @@ -182,7 +182,7 @@ bool Unicycle2DStateCostFunctor::operator()( residual[6] = acc_linear2[0] - acc_linear_pred[0]; residual[7] = acc_linear2[1] - acc_linear_pred[1]; - fuse_core::wrapAngle2D(residual[2]); + // fuse_core::wrapAngle2D(residual[2]); // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. 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 b78750892..170a55a5f 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -37,9 +37,9 @@ #include #include -#include #include #include +#include #include #include @@ -51,17 +51,17 @@ TEST(CostFunction, evaluateCostFunction) const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d covariance = fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); - const double dt { 0.1 }; - const fuse_core::Matrix8d sqrt_information { covariance.inverse().llt().matrixU() }; + const double dt{ 0.1 }; + const fuse_core::Matrix8d sqrt_information{ covariance.inverse().llt().matrixU() }; - const fuse_models::Unicycle2DStateCostFunction cost_function { dt, sqrt_information }; + const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information }; std::random_device dev; std::mt19937 gen(dev()); std::uniform_real_distribution<> position_dist(0.0, 0.5); std::uniform_real_distribution<> yaw_dist(-M_PI / 10.0, M_PI / 10.0); - std::size_t N = 100; + std::size_t N = 10; for (std::size_t i = 0; i < N; i++) { // Randomly generate first state @@ -77,46 +77,14 @@ TEST(CostFunction, evaluateCostFunction) double yaw2[1]; double vel_yaw2[1]; double acc_linear2[2]; - fuse_models::predict( - position1, - yaw1, - vel_linear1, - vel_yaw1, - acc_linear1, - dt, - position2, - yaw2, - vel_linear2, - vel_yaw2, - acc_linear2); - - position1[0] -= 0.2; - position1[1] -= 0.21; - yaw1[0] -= 0.01; - vel_linear1[0] -= 0.01; - vel_linear1[1] -= 0.01; - vel_yaw1[0] -= 0.01; - acc_linear1[0] -= 0.01; - acc_linear1[1] -= 0.01; - - position2[0] += 0.2; - position2[1] += 0.21; - yaw2[0] += 0.01; - vel_linear2[0] += 0.01; - vel_linear2[1] += 0.01; - vel_yaw2[0] += 0.01; - acc_linear2[0] += 0.01; - acc_linear2[1] += 0.01; - // // Compute second state from first - // const double vel_linear2[] = { vel_linear1[0] + acc_linear1[0] * dt, vel_linear1[1] + acc_linear1[1] * dt }; - // const double position2[] = { position1[0] + (vel_linear1[0] + vel_linear2[0]) * 0.5 * dt, - // position1[1] + (vel_linear1[1] + vel_linear2[1]) * 0.5 * dt }; - // const double yaw2[] = { yaw1[0] + vel_yaw1[0] * dt }; - // const double vel_yaw2[] = { vel_yaw1[0] }; - // const double acc_linear2[] = { acc_linear1[0], acc_linear1[1] }; - - const double* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; + fuse_models::predict(position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, dt, position2, yaw2, vel_linear2, + vel_yaw2, acc_linear2); + + const double* parameters[] = + { + position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 + }; fuse_core::Vector8d residuals; @@ -142,17 +110,15 @@ TEST(CostFunction, evaluateCostFunction) // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; - // #if !CERES_SUPPORTS_MANIFOLDS - // ceres::GradientChecker gradient_checker( - // &cost_function, - // static_cast*>(nullptr), - // numeric_diff_options); - // #else - ceres::GradientChecker gradient_checker( - &cost_function, - static_cast*>(nullptr), - numeric_diff_options); - // #endif + #if !CERES_SUPPORTS_MANIFOLDS + ceres::GradientChecker gradient_checker( + &cost_function, + static_cast*>(nullptr), + numeric_diff_options); + #else + ceres::GradientChecker gradient_checker(&cost_function, static_cast*>(nullptr), + numeric_diff_options); + #endif // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 ceres::GradientChecker::ProbeResults probe_results; @@ -162,7 +128,7 @@ TEST(CostFunction, evaluateCostFunction) // Create cost function using automatic differentiation on the cost functor ceres::AutoDiffCostFunction - cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); + cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); @@ -180,11 +146,10 @@ TEST(CostFunction, evaluateCostFunction) for (size_t i = 0; i < num_parameter_blocks; ++i) { - // EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-15) - // << "Autodiff Jacobian[" << i << "] =\n" - // << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" - // << J[i].format(HeavyFmt); - EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-13) << "[" << i << "] \n"; + EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-13) + << "Autodiff Jacobian[" << i << "] =\n" + << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + << J[i].format(HeavyFmt); } } } From fa46de361a365f90b454c7558ad308f9f76a19cc Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Fri, 13 Sep 2024 13:24:51 -0400 Subject: [PATCH 15/25] Fix tabs --- fuse_models/test/test_unicycle_2d_state_cost_function.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 170a55a5f..282fc0fee 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -111,10 +111,10 @@ TEST(CostFunction, evaluateCostFunction) // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; #if !CERES_SUPPORTS_MANIFOLDS - ceres::GradientChecker gradient_checker( - &cost_function, - static_cast*>(nullptr), - numeric_diff_options); + ceres::GradientChecker gradient_checker( + &cost_function, + static_cast*>(nullptr), + numeric_diff_options); #else ceres::GradientChecker gradient_checker(&cost_function, static_cast*>(nullptr), numeric_diff_options); From 8b8a63348894ea2eeec5bf5245d995ef8b71d2e0 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 09:44:14 -0400 Subject: [PATCH 16/25] 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; From d0f9cb70263dfb9509e4372ffc75e141271e6233 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 10:50:55 -0400 Subject: [PATCH 17/25] Fix eigen maps --- .../unicycle_2d_state_cost_function.h | 35 ++++++++++++------- .../unicycle_2d_state_cost_functor.h | 6 ++-- 2 files changed, 26 insertions(+), 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 index b55f3b453..f3caa7880 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,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,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 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(); @@ -144,7 +155,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, 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 index 7964f6859..f95732a45 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,9 +170,9 @@ 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 fuse_core::Matrix R_yaw_inv = fuse_core::rotationMatrix2D(yaw1[0]); - Eigen::Map> delta_position_pred_map(delta_position_pred); - + const fuse_core::Matrix R_yaw_inv = fuse_core::rotationMatrix2D(-yaw1[0]); + const Eigen::Map> delta_position_pred_map(delta_position_pred); + 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]; From 043a2ba2774fe85a86e20a57cc09a7616b711d61 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 11:00:55 -0400 Subject: [PATCH 18/25] Fixed size block for function --- .../include/fuse_models/unicycle_2d_state_cost_function.h | 2 +- 1 file changed, 1 insertion(+), 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 index f3caa7880..4f2d42f88 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 @@ -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 residuals_map(residuals); - residuals_map.block(0, 0, 2, 1) = R_yaw_inv * position_diff - delta_position_pred; + residuals_map<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(); From ebee7fe2439eb4b440f8344e2da3f8b4f70cc067 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 11:01:02 -0400 Subject: [PATCH 19/25] Fixed size block for function --- .../include/fuse_models/unicycle_2d_state_cost_function.h | 2 +- 1 file changed, 1 insertion(+), 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 index 4f2d42f88..5975eb32c 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 @@ -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 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(); From bbefa2ff56a0e78354202911c8f01ed6f35bd754 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 12:42:24 -0400 Subject: [PATCH 20/25] Format --- .../test_unicycle_2d_state_cost_function.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) 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 358889fa0..383c4eca7 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -80,11 +80,8 @@ TEST(CostFunction, evaluateCostFunction) fuse_models::predict(position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, dt, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2); - const double* parameters[] = - { - position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 - }; + const double* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; fuse_core::Vector8d residuals; @@ -112,9 +109,7 @@ TEST(CostFunction, evaluateCostFunction) ceres::NumericDiffOptions numeric_diff_options; #if !CERES_SUPPORTS_MANIFOLDS ceres::GradientChecker gradient_checker( - &cost_function, - static_cast*>(nullptr), - numeric_diff_options); + &cost_function, static_cast*>(nullptr), numeric_diff_options); #else ceres::GradientChecker gradient_checker(&cost_function, static_cast*>(nullptr), numeric_diff_options); @@ -147,9 +142,9 @@ TEST(CostFunction, evaluateCostFunction) for (size_t i = 0; i < num_parameter_blocks; ++i) { EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-13) - << "Autodiff Jacobian[" << i << "] =\n" - << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" - << J[i].format(HeavyFmt); + << "Autodiff Jacobian[" << i << "] =\n" + << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + << J[i].format(HeavyFmt); } } } From 71cb05531135e84f8992869c2880e8ebe71a4c38 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 15:19:11 -0400 Subject: [PATCH 21/25] format --- fuse_models/test/test_unicycle_2d_state_cost_function.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 383c4eca7..127d65a89 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -80,8 +80,11 @@ TEST(CostFunction, evaluateCostFunction) fuse_models::predict(position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, dt, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2); - const double* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; + const double* parameters[] = + { + position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 + }; fuse_core::Vector8d residuals; From a692a76b32c74fa89e4ca7fa264a68bdadc90cee Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 16:15:19 -0400 Subject: [PATCH 22/25] format --- fuse_models/test/test_unicycle_2d_state_cost_function.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 127d65a89..1a97bb5ef 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -80,10 +80,10 @@ TEST(CostFunction, evaluateCostFunction) fuse_models::predict(position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, dt, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2); - const double* parameters[] = - { + const double* parameters[] = + { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; fuse_core::Vector8d residuals; From eacf435dba3d180463eb75ea6033e45465169383 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 16 Sep 2024 17:09:32 -0400 Subject: [PATCH 23/25] Format --- .../include/fuse_models/unicycle_2d_state_cost_functor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f95732a45..c5a019b1c 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 @@ -172,7 +172,7 @@ bool Unicycle2DStateCostFunctor::operator()( const Eigen::Matrix position_diff = (position2_map - position1_map); const fuse_core::Matrix R_yaw_inv = fuse_core::rotationMatrix2D(-yaw1[0]); const Eigen::Map> delta_position_pred_map(delta_position_pred); - + 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]; From df22203d5353accd598a78208f0d88c891f48666 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Tue, 17 Sep 2024 09:30:28 -0400 Subject: [PATCH 24/25] Update fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h Co-authored-by: Enrique Fernandez Perdomo --- .../include/fuse_models/unicycle_2d_state_cost_function.h | 2 +- 1 file changed, 1 insertion(+), 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 index 5975eb32c..8823aa5c8 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 @@ -143,7 +143,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, 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 fuse_core::Vector2d position_diff = position2 - position1; const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]); Eigen::Map residuals_map(residuals); From 076af21fec5513c0ccee842d8a5c471038abc55c Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Tue, 17 Sep 2024 09:36:48 -0400 Subject: [PATCH 25/25] Use head over block --- .../include/fuse_models/unicycle_2d_state_cost_function.h | 2 +- .../include/fuse_models/unicycle_2d_state_cost_functor.h | 2 +- 2 files changed, 2 insertions(+), 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 index 8823aa5c8..171a3e586 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 @@ -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 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 index c5a019b1c..736a595ca 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 @@ -174,7 +174,7 @@ bool Unicycle2DStateCostFunctor::operator()( const Eigen::Map> delta_position_pred_map(delta_position_pred); Eigen::Map> 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];