For 3D navigation systems, representing the orientation is always annoying. After going through several painful learning and debugging processes, I share some lessons that I learned.
The quaterion parameterization
There are several ways to represent a 3d orientation. In real system, everyone pretty much choose over the rotation matrix or the quaternion. While treating the orientation as state, the unit quaterion representation is almost inevitable. Here comes the first annoying thing: the order of parameters is the unit quaternion differs in every system. For example, in Eigen library, the quaterion constructor takes w-x-y-z order, while it stores the data in the x-y-z-w order.
Another annoying thing about unit quaternion. In fact, unit quaternion doubly covers the SO(3) space. To be precise, \(q\) and \(-q\) represent the same 3d orientation. (The "minus" orientation is actually the conjugate of \(q\), or \(q^*\).) Therefore, in the equations, there are several multiplication of 2 or devision of 2. These multiplications and divisions have no thing to do with the real data, but just the adjustment of different representation. Of course, some papers choose to write it out explicitly while some papers don't, which further confuses the beginners.
Linearization
To optimize over a minifold, the Jacobian matrix is essential to tell the variation with respect to the parameters. However, computing Jacobian matrices on a manifold is not trivial. Several papers started this by defining the linearization point. I personally follow the method here. This tutorial defines the derivative by defining addition and substraction first, which is more direct and extendable. One thing to notice is that the order of multiplication is different in different paper, but they leads to different result, since the multiplication is not communitive in general.
Lift Jacobian
This issue is particular important to ceres solver, as mentioned here. In ceres, the Jacobian is with respect to the parameterization in the manifold, or the so-called over-parameterized space. However, the Jacobian is more direcly related to the tangent space, or the local parameterized space. Therefore, in ceres, if one wants to specify the Jacobian by themself, an intermediate Jacobian is necessary to link between the over parameterized space and the local parameterized space, which is the lift Jacobian.