Skip to content

Commit

Permalink
Revert formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
Jake McLaughlin committed May 15, 2024
1 parent de94366 commit 50a30ba
Showing 1 changed file with 7 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
{
}

Expand Down

0 comments on commit 50a30ba

Please sign in to comment.