Class Unicycle2DStateCostFunctor

Class Documentation

class Unicycle2DStateCostFunctor

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

Unicycle2DStateCostFunctor(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)

template<typename T>
bool operator()(const T *const position1, const T *const yaw1, const T *const vel_linear1, const T *const vel_yaw1, const T *const acc_linear1, const T *const position2, const T *const yaw2, const T *const vel_linear2, const T *const vel_yaw2, const T *const acc_linear2, T *residual) const

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

Parameters:

  • position1 – [in] - First position (array with x at index 0, y at index 1)

  • yaw1 – [in] - First yaw

  • vel_linear1 – [in] - First linear velocity (array with x at index 0, y at index 1)

  • vel_yaw1 – [in] - First yaw velocity

  • acc_linear1 – [in] - First linear acceleration (array with x at index 0, y at index 1)

  • position2 – [in] - Second position (array with x at index 0, y at index 1)

  • yaw2 – [in] - Second yaw

  • vel_linear2 – [in] - Second linear velocity (array with x at index 0, y at index 1)

  • vel_yaw2 – [in] - Second yaw velocity

  • acc_linear2 – [in] - Second linear acceleration (array with x at index 0, y at index 1)

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