Skip to content

Commit

Permalink
Fix jacobians
Browse files Browse the repository at this point in the history
  • Loading branch information
@jakemclaughlin6
jakemclaughlin6 committed Sep 13, 2024
1 parent 6b51158 commit fe8d05e
Show file tree
Hide file tree
Showing 3 changed files with 35 additions and 82 deletions.
26 changes: 7 additions & 19 deletions fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -184,29 +184,17 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
// Update jacobian wrt position1
if (jacobians[0])
{
Eigen::Map<fuse_core::Matrix<double, 8, 2>> djacobian(jacobians[0]);
fuse_core::Matrix<double, 2, 2> 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<fuse_core::Matrix<double, 8, 2>> 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<fuse_core::Vector8d> 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<fuse_core::Vector8d> jacobian(jacobians[1]);
jacobian.applyOnTheLeft(-A_);
}

// Update jacobian wrt vel_linear1
Expand Down Expand Up @@ -252,8 +240,8 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
{
Eigen::Map<fuse_core::Matrix<double, 8, 2>> jacobian(jacobians[5]);
jacobian = A_.block<8, 2>(0, 0);
fuse_core::Matrix<double, 2, 2> 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
Expand Down
2 changes: 1 addition & 1 deletion fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
89 changes: 27 additions & 62 deletions fuse_models/test/test_unicycle_2d_state_cost_function.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,9 @@
#include <fuse_core/eigen_gtest.h>
#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <ceres/autodiff_cost_function.h>
#include <ceres/gradient_checker.h>
#include <Eigen/Dense>

#include <limits>
#include <random>
Expand All @@ -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
Expand All @@ -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;

Expand All @@ -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<std::vector<const ceres::LocalParameterization*>*>(nullptr),
// numeric_diff_options);
// #else
ceres::GradientChecker gradient_checker(
&cost_function,
static_cast<std::vector<const ceres::Manifold*>*>(nullptr),
numeric_diff_options);
// #endif
#if !CERES_SUPPORTS_MANIFOLDS
ceres::GradientChecker gradient_checker(
&cost_function,
static_cast<std::vector<const ceres::LocalParameterization*>*>(nullptr),
numeric_diff_options);
#else
ceres::GradientChecker gradient_checker(&cost_function, static_cast<std::vector<const ceres::Manifold*>*>(nullptr),
numeric_diff_options);
#endif

// We cannot use std::numeric_limits<double>::epsilon() tolerance because the worst relative error is 5.26356e-10
ceres::GradientChecker::ProbeResults probe_results;
Expand All @@ -162,7 +128,7 @@ TEST(CostFunction, evaluateCostFunction)

// Create cost function using automatic differentiation on the cost functor
ceres::AutoDiffCostFunction<fuse_models::Unicycle2DStateCostFunctor, 8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2>
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<fuse_core::MatrixXd> J_autodiff(num_parameter_blocks);
Expand All @@ -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);
}
}
}
Expand Down

0 comments on commit fe8d05e

Please sign in to comment.