Skip to content

Rotate position in motion model constraint #365

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jakemclaughlin6 wants to merge 25 commits into
base: devel
Choose a base branch
from
Open

Rotate position in motion model constraint #365

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
25 commits
Select commit Hold shift + click to select a range
2432748
Rotate position in motion model constraint
jakemclaughlin6 Apr 15, 2024
c4c8109
Don't use epsilon for test
jakemclaughlin6 Apr 16, 2024
6cdf326
Add rotation to functor
jakemclaughlin6 Apr 16, 2024
de94366
Remove eigen transformations
May 15, 2024
50a30ba
Revert formatting
May 15, 2024
8958141
Revert formatting
May 15, 2024
d178d86
Revert formatting
May 15, 2024
e5e4c15
Start at identity in predict function
May 15, 2024
7512da2
Formatting
May 15, 2024
d24b42b
Compute measured delta
May 15, 2024
805bca1
Update jacobian computation
May 16, 2024
629dfc5
Update functor
May 16, 2024
6b51158
Temp changes
Jun 3, 2024
fe8d05e
Fix jacobians
jakemclaughlin6 Sep 13, 2024
fa46de3
Fix tabs
jakemclaughlin6 Sep 13, 2024
8b8a633
Address comments
jakemclaughlin6 Sep 16, 2024
d0f9cb7
Fix eigen maps
jakemclaughlin6 Sep 16, 2024
043a2ba
Fixed size block for function
jakemclaughlin6 Sep 16, 2024
ebee7fe
Fixed size block for function
jakemclaughlin6 Sep 16, 2024
bbefa2f
Format
jakemclaughlin6 Sep 16, 2024
71cb055
format
jakemclaughlin6 Sep 16, 2024
a692a76
format
jakemclaughlin6 Sep 16, 2024
eacf435
Format
jakemclaughlin6 Sep 16, 2024
df22203
Update fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h
jakemclaughlin6 Sep 17, 2024
076af21
Use head over block
jakemclaughlin6 Sep 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 13 additions & 9 deletions fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,26 +119,26 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
double* residuals,
double** jacobians) const override
{
double position_pred_x;
double position_pred_y;
double delta_position_pred_x;
double delta_position_pred_y;
double yaw_pred;

@svwilliams svwilliams May 16, 2024

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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(
parameters[0][0], // position1_x
parameters[0][1], // position1_y
parameters[1][0], // yaw1
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_,
position_pred_x,
position_pred_y,
delta_position_pred_x,
delta_position_pred_y,
yaw_pred,
vel_linear_pred_x,
vel_linear_pred_y,
Expand All @@ -147,8 +147,12 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
acc_linear_pred_y,
jacobians);

residuals[0] = parameters[5][0] - position_pred_x;
residuals[1] = parameters[5][1] - position_pred_y;
// 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;

@efernandez efernandez May 15, 2024 •
edited
Loading

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@jakemclaughlin6 jakemclaughlin6 May 15, 2024

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

residuals[3] = parameters[7][0] - vel_linear_pred_x;
residuals[4] = parameters[7][1] - vel_linear_pred_y;
Expand Down
27 changes: 17 additions & 10 deletions fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,11 @@
#include <fuse_core/fuse_macros.h>
#include <fuse_core/util.h>


namespace fuse_models
{

/**
* @brief Create a cost function for a 2D state vector
*
*
* The state vector includes the following quantities, given in this order:
* x position
* y position
Expand All @@ -70,7 +68,7 @@ namespace fuse_models
* || [ yaw_vel_t2 - proj(yaw_vel_t1) ] ||
* || [ x_acc_t2 - proj(x_acc_t1) ] ||
* || [ y_acc_t2 - proj(y_acc_t1) ] ||
*
*
* where, the matrix A is fixed, the state variables are provided at two discrete time steps, and proj is a function
* that projects the state variables from time t1 to time t2. In case the user is interested in implementing a cost
* function of the form
Expand Down Expand Up @@ -147,27 +145,36 @@ bool Unicycle2DStateCostFunctor::operator()(
const T* const acc_linear2,
T* residual) const
{
T position_pred[2];
Comment thread
jakemclaughlin6 marked this conversation as resolved.
T delta_position_pred[2];
T yaw_pred[1];
T vel_linear_pred[2];
T vel_yaw_pred[1];
T acc_linear_pred[2];
T init_position[2];
T init_yaw[1];
init_position[0] = (T)0.0;
init_position[1] = (T)0.0;
init_yaw[0] = (T)0.0;

@efernandez efernandez May 16, 2024

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

predict(
position1,
yaw1,
init_position,
init_yaw,
vel_linear1,
vel_yaw1,
acc_linear1,
T(dt_),
position_pred,
delta_position_pred,
yaw_pred,
vel_linear_pred,
vel_yaw_pred,
acc_linear_pred);

// rotate the position residual into the local frame
T sin_pred_inv = ceres::sin(-yaw_pred[0]);
T cos_pred_inv = ceres::cos(-yaw_pred[0]);

Eigen::Map<Eigen::Matrix<T, 8, 1>> residuals_map(residual);
residuals_map(0) = position2[0] - position_pred[0];
residuals_map(1) = position2[1] - position_pred[1];
residuals_map(0) = cos_pred_inv * delta_position_pred[0] - sin_pred_inv * delta_position_pred[1];
residuals_map(1) = sin_pred_inv * delta_position_pred[0] + cos_pred_inv * delta_position_pred[1];
residuals_map(2) = yaw2[0] - yaw_pred[0];
residuals_map(3) = vel_linear2[0] - vel_linear_pred[0];
residuals_map(4) = vel_linear2[1] - vel_linear_pred[1];
Expand Down
2 changes: 1 addition & 1 deletion fuse_models/test/test_unicycle_2d_state_cost_function.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Comment thread
jakemclaughlin6 marked this conversation as resolved.
EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-15)
Comment thread
ayrton04 marked this conversation as resolved.
Outdated
<< "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt)
<< "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt);
}
Expand Down