Class NormalDeltaOrientation3DCostFunctor

Defined in File normal_delta_orientation_3d_cost_functor.hpp

Class Documentation

class NormalDeltaOrientation3DCostFunctor

Implements a cost function that models a difference between 3D orientation variables (quaternion)

The cost function is of the form:

        ||                                  ||^2

cost(x) = || A * AngleAxis(b^-1 * q1^-1 * q2) || || ||

where the matrix A and the vector b are fixed, and q1 and q2 are the variables, represented as quaternions. The AngleAxis function converts a quaternion into a 3-vector of the form theta*k, where k is the unit vector axis of rotation and theta is the angle of rotation. The A matrix is applied to the angle-axis 3-vector.

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

inline NormalDeltaOrientation3DCostFunctor(const fuse_core::Matrix3d &A, const fuse_core::Vector4d &b)

Construct a cost function instance.

Parameters:

A – [in] The residual weighting matrix, most likely the square root information matrix in order (x, y, z)
b – [in] The measured change between the two orientation variables

template<typename T> inline bool operator()(const T *const orientation1, const T *const orientation2, T *residuals) const: Evaluate the cost function. Used by the Ceres optimization engine.