关于四元数归一化

最近一直在调试VINS-GPS代码，启动rviz终端报错：
刚开始只是觉得时rviz显示问题，但却影响了定位输出结果，经过一顿摸索，最后定位在了四元数归一化的问题，归一化后，问题顺利解决。
有时真是定位问题比解决问题难多了

下面说一说四元数归一化吧

归一化的意义：
四元数归一化：对四元数的单位化，单位化的四元数可以表示一个旋转.
规范化四元数作用：
1.表征旋转的四元数应该是规范化的四元数，但是由于计算误差等因素, 计算过程中四元数会逐渐失去规范化特性，因此必须对四元数做规范化处理
2.意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换.
3.由于误差的引入，使得计算的变换四元数的模不再等于1，变换四元数失去规范性，因此需要再次更新四元数

Q.normalize();

四元数定义

四元数仅是3D姿态的一种表达方式，我们用一个单位四元数表达原本用旋转矩阵表示的三维旋转。这样做一个直接的好处是省空间。一个旋转阵有9个分量，但只有三个自由度。但是，用三个数来描述会出现奇异的情况，欧拉角就是一个例子。而四元数比三维向量多了一个分量，从而可以无奇异地表示各种姿态。

这里需要说明一下，四元数表现形式有点混乱，四元数实部在前的是Hamilton，反之，实部在后的是JPL，JPL一般在航空领域用的比较多。

对于这两种四元数，可以看成左手系和右手系的区别，JPL是在随运动坐标系下观察绝对静止物体的相对位置，而Hamilton在全局坐标系下观察运动物体的空间变换。具体关系如下表：

这里先明确用Hamilton的四元数。
四元数是Hamilton找到的一种扩展的复数。一个四元数拥有一个实部和三个虚部：

四元数的运算

从这里也能看出四元数归一化的意义：
(1) 逆即是其共轭：由于四元数的逆为其共轭除以模长的平方，那么当归一化之后模长为1时，四元数的逆即是其共轭四元数
(2) 保持 R矩阵的正交性质
在此插一个R正交的证明：

旋转矩阵描述了坐标系之间的旋转变换，由 3 × 3 3\times 3 3×3矩阵描述。旋转矩阵是一个正交矩阵。

注意

1）
代码：

#include <Eigen/Core>
#include <iostream>
#include <Eigen/Geometry>
 
using namespace std;
 
int main(){
    Eigen::Quaterniond q = {0.1,0.35,0.2,0.3};
    q.normalize();
    Eigen::Matrix3d R = q.toRotationMatrix();
    cout << R << endl << endl;
 
//    cout << R << endl;
    Eigen::Matrix3d R_transpose = R.transpose();
    cout << R_transpose << endl << endl;
 
    Eigen::Matrix3d R_inverse = R.inverse();
 
    cout << R_inverse << endl << endl;
 
    Eigen::Matrix3d R_zhengjiao = R * R.transpose();
    cout << R_zhengjiao << endl << endl;
 
    return 0;
}

若是没有归一化，则输出：

2）四元数转为旋转矩阵使用前最好归一化，否则可能导致旋转矩阵不正交

定义一个四元数：Eigen::Quaterniond 四元数名称

四元数归一化：

四元数名称.normalize()
四元数名称 = 四元数名称.normalized()

Eigen::Matrix3d R = q.normalized().toRotationMatrix();