Class NormalDeltaPose3DCostFunctor
Defined in File normal_delta_pose_3d_cost_functor.hpp
Class Documentation
-
class NormalDeltaPose3DCostFunctor
Implements a cost function that models a difference between 3D pose variables.
A single pose involves two variables: a 3D position and a 3D orientation. This cost function computes the difference using standard 3D transformation math:
cost(x) = || A * [ q1^-1 * (p2 - p1) - b(0:2) ] ||^2 || [ AngleAxis(b(3:6)^-1 * q1^-1 * q2) ] ||
where p1 and p2 are the position variables, q1 and q2 are the quaternion orientation variables, and the matrix A and the vector b are fixed. 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).
Note that the cost function’s quaternion components are only concerned with the imaginary components (qx, qy, qz).
Public Functions
-
NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d &A, const fuse_core::Vector7d &b)
Constructor.
- Parameters:
A – [in] The residual weighting matrix, most likely the square root information matrix in order (dx, dy, dz, dqx, dqy, dqz)
b – [in] The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz)
-
NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d &A, const fuse_core::Vector7d &b)