search

DAIRLab / dairlib

MIT License
65 stars 26 forks source link

Quaternion projection #269

Closed yminchen closed 2 years ago

yminchen commented 2 years ago

Problem: Given a quaternion, what is the projection to the manifold with 0 roll and 0 pitch angle?

In #266, we get the projection by zeroing out the second and the third element of the quaternion. However, we are not sure if this is the actual projection (that is, minimizing the norm of delta_quaternion which brings the original quaternion to the manifold).

As it turned out (solved by @mposa) we can write out the optimization problem and get the optimal solution analytically through KKT condition. Optimization problem: https://drive.google.com/file/d/1X0KXBXVhFMp4SBcVE_IW9zyphdXz6PEi/view?usp=sharing Solution: https://drive.google.com/file/d/1X-xatGzHjMvO3REl-eoaRKhA7xlUPkRx/view?usp=sharing

We can verify if the current implementation is doing the correct projection.

yminchen commented 2 years ago

We fleshed out the calculation on the whiteboard today, and it turned out the right projection is indeed zeroing out the second and the third element + rescaling.

Whiteboard: https://drive.google.com/file/d/1ZlQaus8ECfNCEIMGOPGDnG6p1ij-g-OM/view?usp=sharing We can see that the ratio of the first to the fourth element stays constant before and after the projection.