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..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 @@ -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,52 +113,52 @@ 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; - double yaw_pred; - double vel_linear_pred_x; - double vel_linear_pred_y; + fuse_core::Vector2d delta_position_pred; + double delta_yaw_pred; + fuse_core::Vector2d vel_linear_pred; double vel_yaw_pred; - double acc_linear_pred_x; - double acc_linear_pred_y; + fuse_core::Vector2d acc_linear_pred; 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, - yaw_pred, - vel_linear_pred_x, - vel_linear_pred_y, + 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, + acc_linear_pred.x(), + acc_linear_pred.y(), jacobians); - residuals[0] = parameters[5][0] - position_pred_x; - residuals[1] = parameters[5][1] - 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; + 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::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]); - fuse_core::wrapAngle2D(residuals[2]); + 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_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) @@ -182,6 +180,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, if (jacobians[0]) { Eigen::Map> jacobian(jacobians[0]); + jacobian.block<2, 2>(0, 0).applyOnTheLeft(R_yaw_inv); jacobian.applyOnTheLeft(-A_); } @@ -235,6 +234,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, { Eigen::Map> jacobian(jacobians[5]); jacobian = A_.block<8, 2>(0, 0); + jacobian.block<2, 2>(0, 0) *= R_yaw_inv; } // Jacobian wrt yaw2 @@ -274,9 +274,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 8c1b16e4a..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 @@ -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,28 +143,39 @@ bool Unicycle2DStateCostFunctor::operator()( const T* const acc_linear2, T* residual) const { - T position_pred[2]; - T yaw_pred[1]; + T delta_position_pred[2]; + T delta_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, - yaw_pred, + delta_position_pred, + delta_yaw_pred, vel_linear_pred, vel_yaw_pred, acc_linear_pred); + 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 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(0) = position2[0] - position_pred[0]; - residuals_map(1) = position2[1] - position_pred[1]; - residuals_map(2) = yaw2[0] - yaw_pred[0]; + 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]; 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 e7b1a9afd..1a97bb5ef 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -34,17 +34,17 @@ #include #include -#include #include +#include #include #include #include #include +#include #include - TEST(CostFunction, evaluateCostFunction) { // Create cost function @@ -56,85 +56,103 @@ TEST(CostFunction, evaluateCostFunction) const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information }; - // 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 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}; - - 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-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 - 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); - - 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())); - - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + 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); - for (size_t i = 0; i < num_parameter_blocks; ++i) + std::size_t N = 10; + for (std::size_t i = 0; i < N; i++) { - EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) - << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + // 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); + + 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); + + 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())); + + 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-13) + << "Autodiff Jacobian[" << i << "] =\n" + << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + << J[i].format(HeavyFmt); + } } } -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS();