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();