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