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

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

fuse_models/test/test_unicycle_2d_state_cost_function.cpp

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_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.

ayrton04 · 2024-06-13T08:06:51Z

@svwilliams @efernandez what did we conclude on this one?

efernandez · 2024-06-13T08:33:18Z

@svwilliams @efernandez what did we conclude on this one?

@ayrton04 - @jakemclaughlin6 still has to address @svwilliams comments, but this is a low priority task for us atm. Other than that, I believe we're all happy with the changes proposed in this PR.

jakemclaughlin6 · 2024-06-13T12:12:16Z

@svwilliams @efernandez what did we conclude on this one?

@ayrton04 - @jakemclaughlin6 still has to address @svwilliams comments, but this is a low priority task for us atm. Other than that, I believe we're all happy with the changes proposed in this PR.

I got a little stuck on computing the manual jacobians for the new cost functor so I put this on the backburner for a bit until I have time to get back to it

jakemclaughlin6 · 2024-09-13T17:26:21Z

@ayrton04 @svwilliams @efernandez I fixed the manual jacobians and the tests are passing. Let me know if we would want more tests and I can make some more, but the existing one should be fairly comprehensive as its running 100 randomized cases

efernandez

That Jacobians look way simpler. I haven't really look at the math, but if the test is passing now, that should be because they're good now.

I just made a few suggestion to simplify the code a bit more, since some operations in the Jacobians are also done for the residuals computation.

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

fuse_models/test/test_unicycle_2d_state_cost_function.cpp

efernandez

LGTM - just two minor comments

fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h

Co-authored-by: Enrique Fernandez Perdomo <efernandez@clearpath.ai>

jakemclaughlin6 · 2024-12-13T18:27:24Z

@svwilliams is this good to merge, or are you guys going to do some validations with this before choosing to merge it?

svwilliams · 2024-12-13T18:34:42Z

Sorry, I still want to do some validations on this PR. Or at least re-review it.

efernandez · 2025-11-17T11:47:13Z

Sorry, I still want to do some validations on this PR. Or at least re-review it.

@svwilliams Did you have a chance to look into this MR?

I'd be happy to help, if there's anything I can help with 😃

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 carlos-m159 and svwilliams April 17, 2024 16:31

ayrton04 reviewed May 15, 2024

View reviewed changes

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

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

fuse_models/test/test_unicycle_2d_state_cost_function.cpp Outdated 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

Temp changes

6b51158

jakemclaughlin6 added 2 commits September 13, 2024 13:23

Fix jacobians

fe8d05e

Fix tabs

fa46de3

efernandez approved these changes Sep 16, 2024

View reviewed changes

Address comments

8b8a633

efernandez approved these changes Sep 16, 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_functor.h Outdated Show resolved Hide resolved

jakemclaughlin6 and others added 9 commits September 16, 2024 10:50

Fix eigen maps

d0f9cb7

Fixed size block for function

043a2ba

Fixed size block for function

ebee7fe

Format

bbefa2f

format

71cb055

format

a692a76

Format

eacf435

Update fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h

df22203

Co-authored-by: Enrique Fernandez Perdomo <efernandez@clearpath.ai>

Use head over block

076af21

jakemclaughlin6 requested a review from ayrton04 November 19, 2024 21:40

ayrton04 approved these changes Nov 26, 2024

View reviewed changes

carlos-m159 approved these changes Nov 26, 2024

View reviewed changes

-    // 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

Uh oh!

Conversation

jakemclaughlin6 commented Apr 15, 2024

Uh oh!

jakemclaughlin6 commented Apr 17, 2024

Uh oh!

ayrton04 commented May 15, 2024

Uh oh!

Uh oh!

Uh oh!

Uh oh!

jakemclaughlin6 commented May 15, 2024

Uh oh!

svwilliams commented May 15, 2024

Uh oh!

svwilliams commented May 15, 2024

Uh oh!

ayrton04 commented May 15, 2024

Uh oh!

efernandez May 15, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

jakemclaughlin6 May 15, 2024

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

efernandez May 16, 2024

Choose a reason for hiding this comment

Uh oh!

efernandez May 16, 2024

Choose a reason for hiding this comment

Uh oh!

efernandez commented May 16, 2024

Uh oh!

svwilliams May 16, 2024

Choose a reason for hiding this comment

Uh oh!

svwilliams May 16, 2024

Choose a reason for hiding this comment

Uh oh!

svwilliams May 16, 2024

Choose a reason for hiding this comment

Uh oh!

svwilliams May 16, 2024

Choose a reason for hiding this comment

Uh oh!

ayrton04 commented Jun 13, 2024

Uh oh!

efernandez commented Jun 13, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

jakemclaughlin6 commented Jun 13, 2024

Uh oh!

jakemclaughlin6 commented Sep 13, 2024

Uh oh!

efernandez left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

efernandez left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

jakemclaughlin6 commented Dec 13, 2024

efernandez May 15, 2024 •

edited

Loading

efernandez commented Jun 13, 2024 •

edited

Loading