Class NormalPriorOrientation3DEulerCostFunctor
Class Documentation
-
class NormalPriorOrientation3DEulerCostFunctor
Create a prior cost function on a 3D orientation variable using Euler roll, pitch, and yaw measurements.
The functor can compute the cost of a subset of the axes, in the event that we are not interested in all the Euler angles in the variable.
So, for example, if b_ = [ measured_yaw ] [ measured_roll ]
then the cost function is of the form:
cost(x) = || A * [ yaw(x) - b_(0) ] ||^2 || [ roll(x) - b_(1) ] ||
where the matrix A and the vector b are fixed and (roll, pitch, yaw) are the components of the 3D orientation variable.
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 Types
-
using Euler = fuse_variables::Orientation3DStamped::Euler
Public Functions
-
inline NormalPriorOrientation3DEulerCostFunctor(const fuse_core::MatrixXd &A, const fuse_core::VectorXd &b, const std::vector<Euler> &axes = {Euler::ROLL, Euler::PITCH, Euler::YAW})
Construct a cost function instance.
- Parameters:
A – [in] The residual weighting matrix, most likely the square root information matrix. Its order must match the values in
axes.b – [in] The orientation measurement or prior. Its order must match the values in
axes.axes – [in] The Euler angle axes for which we want to compute errors. Defaults to all axes.
-
using Euler = fuse_variables::Orientation3DStamped::Euler