Class NormalPriorPose2D

Inheritance Relationships

Base Type

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

Class Documentation

class NormalPriorPose2D : public ceres::SizedCostFunction<ceres::DYNAMIC, 2, 1>

Create a prior cost function on both the position and orientation variables at once.

The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that applies a prior constraint on both the position and orientation variables at once.

The cost function is of the form: 

        ||    [  x - b(0)] ||^2
cost(x) = ||A * [ y - b(1)] || || [yaw - b(2)] ||

Here, the matrix A can be of variable size, thereby permitting the computation of errors for partial measurements. The vector b is a fixed-size 3x1. 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

NormalPriorPose2D(const fuse_core::MatrixXd &A, const fuse_core::Vector3d &b)

Construct a cost function instance.

The residual weighting matrix can vary in size, as this cost functor can be used to compute costs for partial vectors. The number of rows of A will be the number of dimensions for which you want to compute the error, and the number of columns in A will be fixed at 3. For example, if we just want to use the values of x and yaw, then A will be of size 2x3.

Parameters:

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

  • b – [in] The pose measurement or prior in order (x, y, yaw)

virtual ~NormalPriorPose2D() = default

Destructor.

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const

Compute the cost values/residuals, and optionally the Jacobians, using the provided variable/parameter values.