Skip to content

Commit

Permalink
Address comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@jakemclaughlin6
jakemclaughlin6 committed Sep 16, 2024
1 parent fa46de3 commit 8b8a633
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 58 deletions.
64 changes: 23 additions & 41 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 @@ -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<fuse_core::Vector8d> 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<Eigen::Matrix<double, 8, 1>> residuals_map(residuals);
residuals_map.applyOnTheLeft(A_);

if (jacobians)
Expand All @@ -185,8 +169,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
if (jacobians[0])
{
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.block<2, 2>(0, 0).applyOnTheLeft(R_yaw_inv);
jacobian.applyOnTheLeft(-A_);
}

Expand Down Expand Up @@ -240,8 +223,7 @@ 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);
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
Expand Down
23 changes: 11 additions & 12 deletions 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 @@ -170,23 +170,22 @@ bool Unicycle2DStateCostFunctor::operator()(
const Eigen::Matrix<T, 2, 1> position1_map(position1[0], position1[1]);
const Eigen::Matrix<T, 2, 1> position2_map(position2[0], position2[1]);
const Eigen::Matrix<T, 2, 1> 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<T, 2, 2> R_yaw_inv = fuse_core::rotationMatrix2D(yaw1[0]);
Eigen::Map<Eigen::Matrix<T, 2, 1>> 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<Eigen::Matrix<T, 8, 1>> 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<Eigen::Matrix<T, 8, 1>> residuals_map(residual);
residuals_map.applyOnTheLeft(A_.template cast<T>());

return true;
Expand Down
10 changes: 5 additions & 5 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 @@ -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);

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

0 comments on commit 8b8a633

Please sign in to comment.