Class Unicycle2DStateCostFunction

Inheritance Relationships

Base Type

  • public ceres::SizedCostFunction< 8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2 >

Class Documentation

class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2>

Create a cost function for a 2D state vector.

The state vector includes the following quantities, given in this order: x position y position yaw (rotation about the z axis) x velocity y velocity yaw velocity x acceleration y acceleration

The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that applies a prior constraint on both the entire state vector.

The cost function is of the form: 

        ||    [        x_t2 - proj(x_t1)       ] ||^2
cost(x) = || [ y_t2 - proj(y_t1) ] || || [ yaw_t2 - proj(yaw_t1) ] || ||A * [ x_vel_t2 - proj(x_vel_t1) ] || || [ y_vel_t2 - proj(y_vel_t1) ] || || [ 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

cost(X) = (X - mu)^T S^{-1} (X - mu)

where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the square root information matrix (the inverse of the covariance).

Public Functions

Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d &A)

Construct a cost function instance.

Parameters:

  • dt – [in] The time delta across which to generate the kinematic model cost

  • A – [in] The residual weighting matrix, most likely the square root information matrix in order (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc)

inline bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override

Evaluate the cost function. Used by the Ceres optimization engine.

Parameters:

  • parameters – [in] - Parameter blocks: 0 : position1 - First position (array with x at index 0, y at index 1) 1 : yaw1 - First yaw 2 : vel_linear1 - First linear velocity (array with x at index 0, y at index 1) 3 : vel_yaw1 - First yaw velocity 4 : acc_linear1 - First linear acceleration (array with x at index 0, y at index 1) 5 : position2 - Second position (array with x at index 0, y at index 1) 6 : yaw2 - Second yaw 7 : vel_linear2 - Second linear velocity (array with x at index 0, y at index 1) 8 : vel_yaw2 - Second yaw velocity 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1)

  • residual – [out] - The computed residual (error)

  • jacobians – [out] - Jacobians of the residuals wrt the parameters. Only computed if not NULL, and only computed for the parameters where jacobians[i] is not NULL.

Returns:

The return value indicates whether the computation of the residuals and/or jacobians was successful or not.