使用Eigen进行位姿计算

1. 概述

在组合导航和slam等领域中，位姿计算是基础内容，使用Eigen来完成这一过程会大大减小工作量。姿态表示方法包括旋转向量、四元数、旋转矩阵、欧拉角，在使用Eigen之前，需要理清它们之间的关系。关于他们之间关系的基础介绍，网上已经有很多了，在此列出几篇博客，供参考：
https://blog.csdn.net/yang__jing/article/details/82316093
https://blog.csdn.net/u012706484/article/details/79147805
https://blog.csdn.net/qianniu_/article/details/79558199
鉴于此，本文章重点介绍的是我自己在使用过程中遇到的一些坑，供大家参考。

2. 使用方法

下面这张图描述了基本的转换关系，图片来自博客　https://blog.csdn.net/u011092188/article/details/77430988

下面罗列了一些我自己使用过程中发现的一些注意事项：

• Eigen库里面重载的赋值运算符不能在声明的同时通过拷贝赋值。即Quaterniond Q1 = Q2是错误的。原因在于他的内部代码屏蔽了拷贝构造函数.。
• 欧拉角只与旋转矩阵有直接的转换关系，如果想转四元数或者旋转向量，只能通过旋转矩阵过渡一下。
• 所有的旋转都是逆时针为正，如果有做导航的朋友，往往北偏东为正，所以不要搞混。
• Eigen的旋转矩阵表示的是从旋转后的坐标系到旋转前坐标系的转换。即如果载体当前坐标系为A，初始参考坐标系为O，则Eigen的旋转矩阵R表示的是R_AO，即从A坐标系到O坐标系的转换关系，如果在A坐标系中有一个向量r_A，则它在O坐标系下的表示为r_O = R_AO * r_A。
• 和旋转矩阵一样，四元数和旋转向量表示的也是从旋转后的坐标系到旋转前坐标系的转换，因此在