Class NormalDeltaPose2D

Inheritance Relationships

Base Type

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

Class Documentation

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

Implements a cost function that models a difference between pose variables.

A single pose involves two variables: a 2D position and a 2D orientation. The generic NormalDelta cost function only supports a single variable type, and computes the difference using per-element subtraction. This cost function computes the difference using standard 2D transformation math:

delta = [R1 | t1]^-1 * [R2 | t2]

where R1 and R2 are 2D rotation matrices formed from the 2D orientation variables, and t1 and t2 are the position variables in vector form. Once the delta is computed, the difference between the computed delta and the expected delta uses simple per-element subtraction. 

        ||    [ delta.x   - b(0)] ||^2
cost(x) = ||A * [ delta.y - b(1)] || || [ delta.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

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

Constructor.

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 exposed pose difference in order (x, y, yaw)

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.