Skip to content
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.

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

Rotate position in motion model constraint #365

Open
wants to merge 25 commits into
base: devel
Choose a base branch
from
Open
Changes from 1 commit
Commits
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
Prev Previous commit
Next Next commit
Revert formatting
Jake McLaughlin committed May 15, 2024
commit 50a30ba96d6a23784e2a8c5391c0dbbc538af39e
Original file line number Diff line number Diff line change
@@ -113,7 +113,9 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
* computed for the parameters where jacobians[i] is not NULL.
* @return The return value indicates whether the computation of the residuals and/or jacobians was successful or not.
*/
bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const override
{
double position_pred_x;
double position_pred_y;
@@ -148,9 +150,9 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
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;

residuals[2] = parameters[6][0] - yaw_pred;
residuals[3] = parameters[7][0] - vel_linear_pred_x;
residuals[4] = parameters[7][1] - vel_linear_pred_y;
@@ -276,7 +278,9 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
fuse_core::Matrix8d A_; //!< The residual weighting matrix, most likely the square root information matrix
};

Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A)
Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) :
dt_(dt),
A_(A)
{
}