Eigen中四元数的赋值法则（结合Ceres中对四元数的更新方法）

赋值法则（结合Ceres中对四元数的更新方法）"/>

Eigen中四元数的赋值法则（结合Ceres中对四元数的更新方法）

参考文献

Eigen四元数 初始化的坑！！！一天的教训！

一、注意点

Eigen中四元数存储顺序是xyz w \color{#FF0000}{w} w
Ceres中四元数存储顺序是 w \color{#FF0000}{w} wxyz

二、三种赋值方法

1. 直接赋值
   Eigen会默认直接赋值法的存储顺序为 w \color{#FF0000}{w} wxyz，写法为：

Quaterniond q1(1,2,3,4);

2. Vector4d赋值
   Eigen会默认Vector4d赋值的存储顺序为xyz w \color{#FF0000}{w} w，写法为：

Quaterniond q2(Vector4d(1, 2, 3, 4));

3. 数组赋值
   Eigen会默认数组赋值的存储顺序为xyz w \color{#FF0000}{w} w，写法为：

double tmp_q[4] = {1,2,3,4};
Eigen::Map<Eigen::Quaterniond> q3(tmp_q);

4.三种方法的输出结果

可以看到：

• 如果我们人为定义的顺序为 w \color{#FF0000}{w} wxyz，则使用直接法，Eigen输出的结果为xyz w \color{#FF0000}{w} w；
• 如果我们人为定义的顺序为xyz w \color{#FF0000}{w} w，则使用Vector4d赋值或数组赋值，Eigen输出的结果为xyz w \color{#FF0000}{w} w；

三、ceres中对四元数更新的定义

这一小节主要对aloam中ceres对四元数加法的定义做一个总结

1. 四元数更新的定义
   q a = q b ⊗ [ cos ⁡ ∣ ϕ ∣ ϕ ∣ ϕ ∣ sin ⁡ ∣ ϕ ∣ ] \boldsymbol{q}_a=\boldsymbol{q}_b\otimes\left[\begin{array}{c}\cos|{\phi}|\\ \frac{\phi}{|{\phi}|}\sin{|\phi|}\end{array}\right] qa​=qb​⊗[cos∣ϕ∣∣ϕ∣ϕ​sin∣ϕ∣​]
   其中 ϕ \phi ϕ为微小扰动。
2. 四元数的构建
   
   可以看到A-LOAM中两种赋值方式和第二小结所述一样
3. 对ceres中四元数的更新进行定义

• 用eigen格式进行更新

ceres::LocalParameterization *q_parameterization =new ceres::EigenQuaternionParameterization();
problem.AddParameterBlock(para_q, 4, q_parameterization);

加法的定义看上图的注释中对plus的解释

• 用ceres的格式进行更新