Rotate position in motion model constraint #365

jakemclaughlin6 · 2024-04-15T21:19:57Z

No description provided.

jakemclaughlin6 · 2024-04-17T15:52:26Z

@svwilliams @ayrton04 I'm not sure how to request a review directly so just commenting here

ayrton04 · 2024-05-15T12:15:25Z

I'm going to tag @efernandez, as blame tells me he wrote this originally.

Anyway, just thinking aloud here. We have three poses:

pose1           = (x1,    y1,    yaw1   ) = (parameters[0][0], parameters[0][1], parameters[1][0])
pose2           = (x2,    y2,    yaw2   ) = (parameters[5][0], parameters[5][1], parameters[6][0])
pose2_predicted = (xpred, ypred, yawpred) = (position_pred_x,  position_pred_y,  yaw_pred        )

The residual we want is effectively the pose2 - pose2_predicted. Given that both poses can be modeled as transform matrices, wouldn't we therefore want pose2_predicted^-1 * pose2? That would yield:

    double cos_yaw = std::cos(-yaw_pred);  // Note negative sign for inverse of pose2_predicted
    double sin_yaw = std::sin(-yaw_pred);  //
    ...
    <the rest as-is>

But I would also probably eschew the Eigen rotation matrix and just do it directly:

auto delta_x = parameters[5][0] - position_pred_x;
auto delta_y = parameters[5][1] - position_pred_y;
auto sin_pred_inv = std::sin(-yaw_pred);
auto cos_pred_inv= std::cos(-yaw_pred);
residuals[0] = cos_pred_inv * delta_x - sin_pred_inv * delta_y;
residuals[1] = sin_pred_inv * delta_x + cos_pred_inv * delta_y;

In any case, I'm not sure if it's correct for the residual to be in the frame of pose1? Again, I might be wrong about that.

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

fuse_models/test/test_unicycle_2d_state_cost_function.cpp

jakemclaughlin6 · 2024-05-15T13:56:23Z

@ayrton04 The reasoning behind putting the pose error into pose1's frame is mainly due to an issue found in this PR where the covariance is not in the same frame as the error. Since these constraints are relative, its normal to assume the covariance should be in the local frame. Currently the covariance and pose error are in the global frame, which can lead to someone accidentally defining their covariance in the local frame

svwilliams · 2024-05-15T14:38:39Z

I'm trying to get my head wrapped around this. "What frame is the covariance represented in?" is a perennial question in my experience.

In devel right now, the residuals are computed as:

  residuals_map(0) = position2[0] - position_pred[0];
  residuals_map(1) = position2[1] - position_pred[1];
  residuals_map(2) = yaw2[0] - yaw_pred[0];

position2 is represented in the map frame, as is position_pred. The residual computation is not doing any frame transformations, and so the residuals are represented in the map frame (or at least in the map frame orientation).

But that doesn't seem like how we would want a relative pose to work.
Instead, if we compute the predicted change/delta between Pose1 and Pose2 using the motion model (delta_predicted),
and we compute our "true" change between Pose1 and Pose2 (delta), then the residual would look something like:

  delta = Pose1^1 * Pose2;
  delta_predicted = Pose1^1 * Pose2Predicted;

  residuals_map(0) = delta[0] - delta_predicted[0];
  residuals_map(1) = delta[1] - delta_predicted[1];
  residuals_map(2) = delta[2] - delta_predicted[2];

Both delta and delta_predicted are represented in the Pose1 frame, and so the residual is also in the Pose1 frame.

And that matches my intuition about what frame the covariance should be in. The measurement is a transform from Pose1 to Pose2. I.e. The robot moved forward 1m. Thus, the measurement is represented in the Pose1 frame. It makes certain intuitive sense for the covariance to be represented in the same frame as the measurement.

svwilliams · 2024-05-15T14:53:59Z

And as an implementation note, we should be able to compute the predicted delta directly from the predict() function by providing the starting point as the origin instead of as Pose1. There should be no need to accumulate the position change starting at Pose1, only to then "subtract out" Pose1 afterwards.

  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];
  predict(
    [0.0, 0.0],
    [0.0],
    vel_linear1,
    vel_yaw1,
    acc_linear1,
    T(dt_),
    delta_position_pred,
    delta_yaw_pred,
    vel_linear_pred,
    vel_yaw_pred,
    acc_linear_pred);

ayrton04 · 2024-05-15T16:40:34Z

Makes sense to me!

efernandez · 2024-05-15T17:50:13Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

+    // rotate the position residual into the local frame
+    auto sin_pred_inv = std::sin(-yaw_pred);
+    auto cos_pred_inv= std::cos(-yaw_pred);
+
+    residuals[0] = cos_pred_inv * delta_position_pred_x - sin_pred_inv * delta_position_pred_y;
+    residuals[1] = sin_pred_inv * delta_position_pred_x + cos_pred_inv * delta_position_pred_y;
    residuals[2] = parameters[6][0] - yaw_pred;


I don't see why residuals[0] and residual[1] no longer depend on the previous pose (defined in parameters[0] and parameters[1], i.e. position1_x, position1_y and yaw1) and the next pose (defined in parameters[5] and parameters[6], i.e. position2_x, position2_y and yaw2).

Shouldn't we be computing the delta between those two and then compute the residuals as the delta between that and the predicted pose. That is:

Suggested change

// rotate the position residual into the local frame

auto sin_pred_inv = std::sin(-yaw_pred);

auto cos_pred_inv= std::cos(-yaw_pred);

residuals[0] = cos_pred_inv * delta_position_pred_x - sin_pred_inv * delta_position_pred_y;

residuals[1] = sin_pred_inv * delta_position_pred_x + cos_pred_inv * delta_position_pred_y;

residuals[2] = parameters[6][0] - yaw_pred;

const Eigen::Matrix<T, 2, 1> position1(parameters[0][0], parameters[0][1]);

const Eigen::Matrix<T, 2, 1> position2(parameters[5][0], parameters[5][1]);

const Eigen::Matrix<T, 2, 1> delta_position = fuse_core::rotationMatrix2D(parameters[1][0]).transpose() * (position2 - position1);

const T delta_yaw = parameters[6][0] - parameters[1][0]; // omitting fuse_core::wrapAngle2D because it is done later on

const Eigen::Matrix<T, 2, 1> delta_position_pred(delta_position_pred_x, delta_position_pred_y);

residuals.template head<2>() = fuse_core::rotationMatrix2D(delta_yaw).transpose() * (delta_position_pred - delta_position);

residuals[2] = delta_yaw - yaw_pred; // omitting fuse_core::wrapAngle2D because it is done later on

In a way, this is similar to

fuse/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.h

Lines 118 to 125 in 2c652c7

Eigen::Map<const Eigen::Matrix<T, 2, 1>> position1_vector(position1);

Eigen::Map<const Eigen::Matrix<T, 2, 1>> position2_vector(position2);

Eigen::Matrix<T, 3, 1> full_residuals_vector;

full_residuals_vector.template head<2>() =

fuse_core::rotationMatrix2D(orientation1[0]).transpose() * (position2_vector - position1_vector) -

b_.head<2>().template cast<T>();

full_residuals_vector(2) = fuse_core::wrapAngle2D(orientation2[0] - orientation1[0] - T(b_(2)));

In this case, I feel it would just be better to keep what I originally had, and just rotate the error back into the local frame

efernandez · 2024-05-15T17:52:21Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

@@ -147,27 +145,36 @@ bool Unicycle2DStateCostFunctor::operator()(
  const T* const acc_linear2,
  T* residual) const
 {
-  T position_pred[2];


I believe the comment I made above applies here as well. Indeed, I suspect with your change the compiler must be complaining of unused arguments, isn't it?

efernandez · 2024-05-15T17:56:43Z

fuse_models/test/test_unicycle_2d_state_cost_function.cpp

@@ -128,7 +128,7 @@ TEST(CostFunction, evaluateCostFunction)

  for (size_t i = 0; i < num_parameter_blocks; ++i)
  {
-    EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits<double>::epsilon())


This was probably too strict, but if we change the cost function or functor, so the residuals are computed differently, I'd expect the analytic jacobians are also computed differently. So I'm afraid that even a small error here is actually a consequence of an incorrect analytic jacobian. Maybe the test values are hiding that because they don't have a strong contribution to the jacobians that should've probably been updated to account for the residuals computation changes. 🤔

efernandez · 2024-05-16T15:19:38Z

fuse_models/test/test_unicycle_2d_state_cost_function.cpp

@@ -99,7 +99,17 @@ TEST(CostFunction, evaluateCostFunction)

  // Check jacobians are correct using a gradient checker
  ceres::NumericDiffOptions numeric_diff_options;
-  ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options);


This looks like a change from another MR

If possible, get rid of this change here

efernandez · 2024-05-16T15:22:26Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

+  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> delta_position = fuse_core::rotationMatrix2D(-yaw1[0]) * (position2_map - position1_map);
+  const T delta_yaw = yaw2[0] - yaw1[0];  // omitting fuse_core::wrapAngle2D because it is done later on
+  const Eigen::Matrix<T, 2, 1> delta_position_pred_map(delta_position_pred[0], delta_position_pred[1]);


These aren't really Eigen maps, but I just realized you can use position1 because it's already taken.

Just highlighting that. I don't really have a better option, so it's ok for me. Maybe others have a better suggestion 🤔

efernandez · 2024-05-16T15:23:36Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

+  init_position[0] = (T)0.0;
+  init_position[1] = (T)0.0;
+  init_yaw[0] = (T)0.0;


I believe it's more common to see the following:

Suggested change

init_position[0] = (T)0.0;

init_position[1] = (T)0.0;

init_yaw[0] = (T)0.0;

init_position[0] = T(0.0);

init_position[1] = T(0.0);

init_yaw[0] = T(0.0);

efernandez · 2024-05-16T15:25:38Z

This now LGTM. I just made some minor comments.

Does this look good to you now @svwilliams @ayrton04 ? Does it align with what you mentioned before @svwilliams ?

svwilliams · 2024-05-16T17:33:11Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

-    double position_pred_x;
-    double position_pred_y;
+    double delta_position_pred_x;
+    double delta_position_pred_y;
    double yaw_pred;


This should be renamed to delta_yaw_pred to be consistent with the other variables

svwilliams · 2024-05-16T17:48:27Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

+    const Eigen::Vector2d delta_position_pred(delta_position_pred_x, delta_position_pred_y);
+
+    Eigen::Map<Eigen::Matrix<double, 8, 1>> residuals_map(residuals);
+    residuals_map.head<2>() = fuse_core::rotationMatrix2D(-delta_yaw) * (delta_position - delta_position_pred);


I'm not sure I understand why the difference of position changes are being transformed into....is this the Pose2 frame?
I feel like this should simply be:

residuals_map(0) = delta_position(0) - delta_position_pred(0); residuals_map(1) = delta_position(1) - delta_position_pred(1);

Both delta_position and delta_position_pred are represented in the Pose1 frame. If we just subtract the positions, that error will also be in the Pose1 frame/orientaiton. And that seems like the natural frame to compute the measurement covariance in as well.

svwilliams · 2024-05-16T18:07:22Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

-    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 = parameters[6][0] - parameters[1][0];


To be consistent with naming

Suggested change

const double delta_yaw = parameters[6][0] - parameters[1][0];

const double delta_yaw_est = parameters[6][0] - parameters[1][0];

svwilliams · 2024-05-16T23:32:14Z

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

+    const fuse_core::Matrix2d R_delta_yaw_inv = fuse_core::rotationMatrix2D(-delta_yaw);
+
+    Eigen::Map<Eigen::Matrix<double, 8, 1>> residuals_map(residuals);
+    residuals_map.head<2>() = R_delta_yaw_inv * (delta_position_pred - delta_position_est);


The delta_position_est is computed as Pose1^1 * Pose2. That effectively transforms Pose2 into the frame of Pose1.

Similarly, delta_position_pred is computed with Pose1 as the origin. So that quantity is also in the frame of Pose1.

If we did a simple subtraction, residual[0] = delta_position_est.x - delta_position_prod.x, then the residual would also be represented in the frame of Pose1. Which seems like the natural frame to compute the measurement covariance as well.

Here you are rotating the error into the ... Pose2 frame?
I feel like this should just be:

residuals_map(0) = delta_position_est.x - delta_position_pred.x; residuals_map(1) = delta_position_est.y - delta_position_pred.y; residuals_map(2) = delta_yaw_est - delta_yaw_pred; ...

But that is not a firm belief. I could be convinced otherwise.

jakemclaughlin6 added 3 commits April 15, 2024 17:19

Rotate position in motion model constraint

2432748

Don't use epsilon for test

c4c8109

Add rotation to functor

6cdf326

svwilliams requested review from svwilliams and carlos-m159 April 17, 2024 16:31

ayrton04 reviewed May 15, 2024

View reviewed changes

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h Outdated Show resolved Hide resolved

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h Outdated Show resolved Hide resolved

fuse_models/test/test_unicycle_2d_state_cost_function.cpp Show resolved Hide resolved

Jake McLaughlin added 4 commits May 15, 2024 10:03

Remove eigen transformations

de94366

Revert formatting

50a30ba

Revert formatting

8958141

Revert formatting

d178d86

Jake McLaughlin added 2 commits May 15, 2024 13:01

Start at identity in predict function

e5e4c15

Formatting

7512da2

efernandez suggested changes May 15, 2024

View reviewed changes

Compute measured delta

d24b42b

efernandez reviewed May 16, 2024

View reviewed changes

efernandez suggested changes May 16, 2024

View reviewed changes

Jake McLaughlin added 2 commits May 16, 2024 13:47

Update jacobian computation

805bca1

Update functor

629dfc5

svwilliams reviewed May 16, 2024

View reviewed changes

svwilliams self-requested a review May 16, 2024 23:32

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Rotate position in motion model constraint #365

Rotate position in motion model constraint #365

jakemclaughlin6 commented Apr 15, 2024

jakemclaughlin6 commented Apr 17, 2024

ayrton04 commented May 15, 2024

jakemclaughlin6 commented May 15, 2024

svwilliams commented May 15, 2024

svwilliams commented May 15, 2024

ayrton04 commented May 15, 2024

efernandez May 15, 2024 •

edited

jakemclaughlin6 May 15, 2024

efernandez May 15, 2024

efernandez May 15, 2024

efernandez May 16, 2024

efernandez May 16, 2024

efernandez May 16, 2024

efernandez commented May 16, 2024

svwilliams May 16, 2024

svwilliams May 16, 2024

svwilliams May 16, 2024

svwilliams May 16, 2024

-    // rotate the position residual into the local frame
-    auto sin_pred_inv = std::sin(-yaw_pred);
-    auto cos_pred_inv= std::cos(-yaw_pred);
-    residuals[0] = cos_pred_inv * delta_position_pred_x - sin_pred_inv * delta_position_pred_y;
-    residuals[1] = sin_pred_inv * delta_position_pred_x + cos_pred_inv * delta_position_pred_y;
-    residuals[2] = parameters[6][0] - yaw_pred;
+    const Eigen::Matrix<T, 2, 1> position1(parameters[0][0], parameters[0][1]);
+    const Eigen::Matrix<T, 2, 1> position2(parameters[5][0], parameters[5][1]);
+    const Eigen::Matrix<T, 2, 1> delta_position = fuse_core::rotationMatrix2D(parameters[1][0]).transpose() * (position2 - position1);
+    const T delta_yaw = parameters[6][0] - parameters[1][0];  // omitting fuse_core::wrapAngle2D because it is done later on
+    const Eigen::Matrix<T, 2, 1> delta_position_pred(delta_position_pred_x, delta_position_pred_y);
+    residuals.template head<2>() = fuse_core::rotationMatrix2D(delta_yaw).transpose() * (delta_position_pred - delta_position);
+    residuals[2] = delta_yaw - yaw_pred;  // omitting fuse_core::wrapAngle2D because it is done later on

	Eigen::Map<const Eigen::Matrix<T, 2, 1>> position1_vector(position1);
	Eigen::Map<const Eigen::Matrix<T, 2, 1>> position2_vector(position2);
	Eigen::Matrix<T, 3, 1> full_residuals_vector;

	full_residuals_vector.template head<2>() =
	fuse_core::rotationMatrix2D(orientation1[0]).transpose() * (position2_vector - position1_vector) -
	b_.head<2>().template cast<T>();
	full_residuals_vector(2) = fuse_core::wrapAngle2D(orientation2[0] - orientation1[0] - T(b_(2)));

	const double delta_yaw = parameters[6][0] - parameters[1][0];
	const double delta_yaw_est = parameters[6][0] - parameters[1][0];

Rotate position in motion model constraint #365

Are you sure you want to change the base?

Rotate position in motion model constraint #365

Conversation

jakemclaughlin6 commented Apr 15, 2024

jakemclaughlin6 commented Apr 17, 2024

ayrton04 commented May 15, 2024

jakemclaughlin6 commented May 15, 2024

svwilliams commented May 15, 2024

svwilliams commented May 15, 2024

ayrton04 commented May 15, 2024

efernandez May 15, 2024 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez commented May 16, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez May 15, 2024 •

edited