Class NormalPriorOrientation3DCostFunctor

Defined in File normal_prior_orientation_3d_cost_functor.hpp

Class Documentation

class NormalPriorOrientation3DCostFunctor

Create a prior cost function on a 3D orientation variable (quaternion)

The cost function is of the form:

        ||                         ||^2

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

where the matrix A and the vector b are fixed, and q is the variable being measured, represented as a quaternion. 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 NormalPriorOrientation3DCostFunctor(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 (quaternion_x, quaternion_y, quaternion_z)
b – [in] The orientation measurement or prior in order (w, x, y, z)

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