Class NormalPriorOrientation3DCostFunctor
Class Documentation
-
class NormalPriorOrientation3DCostFunctor
Create a prior cost function on a 3D orientation variable (quaternion)
The cost function is of the form:
cost(x) = || A * AngleAxis(b^-1 * q) || || |||| ||^2
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)
-
inline NormalPriorOrientation3DCostFunctor(const fuse_core::Matrix3d &A, const fuse_core::Vector4d &b)