四元数用来表示旋转时，必须是单位四元数 ，也就是它的模长（范数）必须等于 1。
∣ q ∣ = w 2 + x 2 + y 2 + z 2 = 1 |q|=\sqrt{w^2+x^2+y^2+z^2}=1 ∣q∣=w2+x2+y2+z2 =1
1. normalize () 的作用：只修正「模长」
q.normalize() 的数学操作：把四元数的四个分量，全部除以它自身的模长。
效果：强制让新四元数的模长 = 1。仅此而已。
举个例子：
假设运算之后四元数变成： q = 1.02 , ; 0.11 , ; 0.11 , ; 0.11 q=1.02,;0.11,;0.11,;0.11 q=1.02,;0.11,;0.11,;0.11
它的模长 ∣ q ∣ > 1 |q|>1 ∣q∣>1，不再是单位四元数，已经不能正确代表旋转。
调用normalize()，把四个数整体缩放，让模重新回到 1。
✅它做的：整体缩放四个数字，保证模 = 1
❌它不会修改四个分量之间的相对比例关系！也就是角度信息不会被修正。
2. 为什么会出现模长偏离 1？
代码里这行：
cpp
Quatd q = pose.unit_quaternion() * Quatd(1, 0.5*omega[0]*dt,0.5*omega[1]*dt,0.5*omega[2]*dt);增量四元数我们直接写的是 Quatd(1, 0.5ωdt)。
这个增量 1 , 1 2 ω Δ t 1,\\frac12\\omega\\Delta t 1,21ωΔt 本身就不是严格单位四元数！
计算它的模长：
只要角速度不为 0，这个增量四元数模就大于 1。
每次乘法之后，四元数模长会慢慢偏离 1，数值上发生 "膨胀"。
👉如果不做normalize()，模长越来越大，彻底失去旋转的物理含义。
所以必须调用normalize()，把模强行拉回 1，解决数值膨胀问题。
3. 重点：为什么 normalize 不能消除角度的截断误差？
角度误差来自哪里？
我们用的是一阶近似公式：
q k + 1 ≈ q k ⊗ 1 , 1 2 ω Δ t q_{k+1}\approx q_k \otimes \left1,\\frac12\\boldsymbol\\omega\\Delta t\\right qk+1≈qk⊗1,21ωΔt
真实完整的增量四元数应该是：
Δ q = cos θ 2 , ; n sin θ 2 \Delta q=\left\\cos\\frac{\\theta}{2},; n\\sin\\frac{\\theta}{2}\\right Δq=cos2θ,;nsin2θ
一阶近似是把 cos θ 2 ≈ 1 \cos\frac{\theta}{2}\approx1 cos2θ≈1， sin θ 2 ≈ θ 2 \sin\frac{\theta}{2}\approx\frac{\theta}{2} sin2θ≈2θ。
这一步就已经产生角度上的近似偏差，四个分量本身的相对比例就已经错了。
normalize()只是把这一组已经比例错误的四个数字整体缩放。
缩放只会全部同乘一个系数，不会改变 w,x,y,z 互相之间的比例 。
比例错了 → 代表的旋转角度依然是错的！缩放救不了这个错误。
通俗比喻帮助理解
假设真实正确四元数应该是：
w=0.98，x=0.14，y=0，z=0（对应某个旋转角度）一阶近似算出来得到一组错误比例：
w=1.00，x=0.15，y=0，z=0这组数字本身比例就不对，代表的旋转角度已经偏了。
normalize()只会把这四个数统一除以模长，把模变成 1，但w:x的比值依旧是错的，角度依旧不对。
👉normalize 修复的是「大小（模）」，修复不了「内部比例（角度）」，这一点是非常重要的，也是非常容易混淆的。
4. 结合 motion.cpp 实验现象
当--use_quaternion=true，角速度很大：
-
每一步一阶近似带来角度截断误差；
-
q.normalize()保证模长 = 1，不会出现数值爆炸； -
但是每一步角度都有一点点错，误差不断累积；
-
最后的现象：轨迹螺旋漂移，不能闭合圆环。
即便每一步都 normalize，漂移依旧存在！
5. 最后归纳
-
单位四元数表示旋转要求： ∣ q ∣ = 1 |q|=1 ∣q∣=1；
normalize()将四元数所有分量同除以模长，强制使模长等于 1，解决数值运算带来的模长漂移。 -
normalize()只是对四元数整体做缩放，不会改变 w,x,y,z 之间的相对比例。 -
一阶泰勒近似产生的是分量比例上的角度误差 ，缩放无法修正该误差；所以即使调用
normalize()，大增量下姿态依旧会漂移。
6. 工程启示
-
四元数一阶近似只适合单步旋转角度很小（高频 IMU，dt 很小）的场景；
-
如果单步旋转角度大，不要用 1 , 1 2 ω Δ t 1,\\frac12\\omega\\Delta t 1,21ωΔt 近似，要用完整指数映射计算正确的增量四元数。
7.为什么表示旋转的时候必须是单位四元数呢？
四元数旋转公式：
p ′ = q p q − 1 p' = qpq^{-1} p′=qpq−1
p p p 是三维点写成的纯虚四元数 0 , x , y , z T 0,x,y,z^T 0,x,y,zT， q q q 用来做旋转。
- 回顾四元数的逆
四元数求逆公式：
q − 1 = q ∗ ∣ q ∣ 2 q^{-1}=\frac{q^*}{|q|^2} q−1=∣q∣2q∗
-
q ∗ q^* q∗ 是共轭（虚部取反）
-
∣ q ∣ |q| ∣q∣ 是四元数模长。
情况 1： q q q 是单位四元数， ∣ q ∣ = 1 |q|=1 ∣q∣=1
此时 ∣ q ∣ 2 = 1 |q|^2=1 ∣q∣2=1，所以：
q − 1 = q ∗ q^{-1}=q^* q−1=q∗
旋转公式简化成大家常见写法：
p ′ = q p q ∗ p'=qpq^* p′=qpq∗
这个运算只会改变向量方向，不改变向量长度 ，正好对应「旋转」的物理含义：旋转不伸缩物体，只改变朝向。
情况 2：如果模长 ∣ q ∣ ≠ 1 |q| \neq 1 ∣q∣=1（大于 1 或者小于 1）
把 q q q 拆成：
q = s ⋅ q u q = s \cdot q_u q=s⋅qu
-
s = ∣ q ∣ s=|q| s=∣q∣ 是模长（标量缩放系数）
-
q u q_u qu 是单位四元数（ ∣ q u ∣ = 1 |q_u|=1 ∣qu∣=1，代表真正的旋转）
代入旋转公式 p ′ = q p q − 1 p'=q p q^{-1} p′=qpq−1：
👉看上去标量 s 好像抵消了？
但是！上面这个是数学理论；代码里我们不是这么干的！
⚠关键：代码中的坑（Eigen/sad 库现实行为）
在 C++ Eigen 中，Eigen::Quaterniond 并没有自动帮你带上 q p q − 1 q p q^{-1} qpq−1 的完整运算 。
Eigen 重载的operator*做旋转，内部默认假设输入是单位四元数 ，直接执行：
p ′ = q p q ∗ p' = qpq^* p′=qpq∗
而不是 q p q − 1 q p q^{-1} qpq−1！
当 ∣ q ∣ = s ≠ 1 |q|=s \neq1 ∣q∣=s=1，代码实际执行：
结果多乘了 s 2 s^2 s2！
-
如果 ∣ q ∣ > 1 |q|>1 ∣q∣>1， s > 1 s>1 s>1， s 2 > 1 s^2>1 s2>1：向量被放大，发生拉伸；
-
如果 ∣ q ∣ < 1 |q|<1 ∣q∣<1， s < 1 s<1 s<1， s 2 < 1 s^2<1 s2<1：向量被缩小，发生压缩。
✅旋转的定义：只改变方向，不改变矢量长度。
一旦模长不等于 1，使用库的旋转接口，会附带缩放效果 ，不再是纯旋转！
这就是工程上，表达旋转必须强制单位四元数的根本原因。
举数字例子
真实旋转：点 p = ( 1 , 0 , 0 ) p=(1,0,0) p=(1,0,0)，绕 Z 轴转 90°，正确结果 ( 0 , 1 , 0 ) (0,1,0) (0,1,0)，长度还是 1。
现在取一个非单位四元数 ：
q = 1.1 ⋅ q u q=1.1\cdot q_u q=1.1⋅qu，模长 s = 1.1 > 1 s=1.1>1 s=1.1>1。
用 Eigen q * p做旋转，输出结果会是 ( 0 , 1.21 , 0 ) (0, 1.21, 0) (0,1.21,0)。
方向对了，但是长度变成 1.21，被放大了！这已经不是旋转，是「旋转 + 放大」。
- 回到 motion.cpp 代码场景
cpp
Quatd q = pose.unit_quaternion() * Quatd(1, 0.5*omega[0]*dt,0.5*omega[1]*dt,0.5*omega[2]*dt);
// 这里算出来的q模长>1，如果不normalize！
pose.so3() = SO3(q);SO3(q)内部会拿这个四元数去构造旋转矩阵。
非单位四元数构造旋转矩阵，得到的矩阵不再是正交矩阵 ， R T R ≠ I R^TR\neq I RTR=I。
-
正交矩阵性质：旋转矩阵必须满足 R T R = I , det ( R ) = 1 R^TR=I,\det(R)=1 RTR=I,det(R)=1；
-
四元数模长偏离 1 → 生成的矩阵丧失正交性；
-
后续做坐标变换 v w = R w b v b v_w=R_{wb}v_b vw=Rwbvb，向量会被莫名其妙放大 / 缩小，仿真直接崩坏。
👉所以代码必须调用q.normalize()，强制 ∣ q ∣ = 1 |q|=1 ∣q∣=1，保证从四元数生成的矩阵是合法旋转矩阵。
-
区分两件完全不同的事情（非常容易混淆）
-
normalize () 处理的：模长（缩放因子 s）
解决：四元数乘法带来的数值膨胀，保证 ∣ q ∣ = 1 |q|=1 ∣q∣=1，保证矩阵正交，消除附带缩放。
✔能修好。
-
一阶近似带来的：角度误差（w,x,y,z 的相对比例错了）
来源：把 cos θ 2 ≈ 1 , sin θ 2 ≈ θ 2 \cos\frac{\theta}{2}\approx1,\sin\frac{\theta}{2}\approx\frac{\theta}{2} cos2θ≈1,sin2θ≈2θ，近似导致四个分量之间比例不对。
❌
normalize()只是整体缩放所有分量，不会改变互相之间比例，修不了角度偏差。
比喻：
一张照片，①图片整体放大（模长问题）；②图片里面人物五官比例扭曲（角度截断误差）。
normalize()只能把整张图缩放到原始大小，修复不了五官扭曲。
-
总结
-
数学理论上，完整旋转公式 p ′ = q p q − 1 p'=q p q^{-1} p′=qpq−1，模长会被抵消，任意非零四元数都可以表达同一个旋转。
-
但是工程库 (Eigen、sad) 的四元数旋转接口默认直接使用 p ′ = q p q ∗ p'=q p q^* p′=qpq∗，省略除以 ∣ q ∣ 2 |q|^2 ∣q∣2**。**
- 如果 ∣ q ∣ ≠ 1 |q|\neq1 ∣q∣=1，运算结果会额外乘上 ∣ q ∣ 2 |q|^2 ∣q∣2，产生向量缩放，不再是纯旋转。
-
用四元数构造旋转矩阵时，只有单位四元数，才能输出满足 R T R = I R^TR=I RTR=I 的正交旋转矩阵；模长大于或者小于 1，生成矩阵丧失正交性，坐标变换会附带拉伸 / 压缩。
-
normalize()把四元数全部分量同除以自身模长，强制 ∣ q ∣ = 1 |q|=1 ∣q∣=1，消除数值累积带来的缩放问题；但它只做整体缩放，不能修复一阶近似带来的分量比例（角度）误差。 -
反思
问：为什么表示旋转的四元数要求必须是单位四元数？
答：
理论上完整公式 p ′ = q p q − 1 p'=qpq^{-1} p′=qpq−1 会抵消模长；但工程上 Eigen 等库简化为 p ′ = q p q ∗ p'=qpq^* p′=qpq∗。
如果四元数模长不等于 1，旋转运算会引入额外缩放，不再是纯旋转。同时只有单位四元数，转换得到的旋转矩阵才满足正交矩阵条件 R T R = I R^TR=I RTR=I。
normalize 只能修正模长，不能修正近似带来的角度误差。
8.代码与四元数一阶近似公式一一对应拆解
公式：
q k + 1 ≈ q k ⊗ Δ q , Δ q = 1 1 2 ω Δ t \boldsymbol{q}_{k+1} \approx q_k \otimes \Delta q,\quad \Delta q=\begin{bmatrix} \boldsymbol 1 \\ \dfrac12\boldsymbol\omega\Delta t \end{bmatrix} qk+1≈qk⊗Δq,Δq=121ωΔt
四元数存储格式：
Quatd(w, x, y, z)
第 1 个参数：实部 w
第 2、3、4 参数：虚部 x , y , z x,y,z x,y,z，对应向量部分 1 2 ω Δ t = 1 2 ω x Δ t , ; ω y Δ t , ; ω z Δ t \frac12\boldsymbol\omega\Delta t=\frac12\\omega_x\\Delta t,;\\omega_y\\Delta t,;\\omega_z\\Delta t 21ωΔt=21ωxΔt,;ωyΔt,;ωzΔt
代码
cpp
Quatd q = pose.unit_quaternion() * Quatd(1, 0.5*omega[0]*dt,0.5*omega[1]*dt,0.5*omega[2]*dt);把代码拆成两部分：
-
pose.unit_quaternion()→ q k \boldsymbol q_k qk，上一时刻旧的四元数 -
Quatd(1, 0.5*omega[0]*dt,0.5*omega[1]*dt,0.5*omega[2]*dt)→ 增量四元数 Δ q \boldsymbol{\Delta q} Δq -
*：Eigen 的Quaterniond重载的*运算符，代表**四元数乘法 ** ⊗ \otimes ⊗
逐项分量对齐
Δ q = ; w = 1 , x = 1 2 ω x Δ t , y = 1 2 ω y Δ t , z = 1 2 ω z Δ t ; \Delta q=\Big;w=1,\\quad x=\\frac12\\omega_x\\Delta t,\\quad y=\\frac12\\omega_y\\Delta t,\\quad z=\\frac12\\omega_z\\Delta t;\\Big Δq=;w=1,x=21ωxΔt,y=21ωyΔt,z=21ωzΔt;
| 公式符号 | 代码里面的值 | 说明 |
|---|---|---|
| Δ q . w \Delta q.w Δq.w | 1 |
增量四元数实部，一阶近似令 cos ( θ / 2 ) ≈ 1 \cos(\theta/2)\approx 1 cos(θ/2)≈1 |
| Δ q . x \Delta q.x Δq.x | 0.5 * omega[0] * dt |
虚部 x 分量 1 2 ω x Δ t \frac12\omega_x \Delta t 21ωxΔt |
| Δ q . y \Delta q.y Δq.y | 0.5 * omega[1] * dt |
虚部 y 分量 1 2 ω y Δ t \frac12\omega_y \Delta t 21ωyΔt |
| Δ q . z \Delta q.z Δq.z | 0.5 * omega[2] * dt |
虚部 z 分量 1 2 ω z Δ t \frac12\omega_z \Delta t 21ωzΔt |
然后四元数相乘：
q k + 1 ; = ; q k ; ⊗ ; Δ q q_{k+1} ;=; q_k ;\otimes; \Delta q qk+1;=;qk;⊗;Δq
对应代码：
cpp
q = 旧四元数 * 增量四元数;⚠注意：这里的
*不是普通标量乘法，是四元数乘法 ，Eigen 库内部已经重载实现四元数的 ⊗ \otimes ⊗运算。
得到相乘结果q之后：
cpp
q.normalize();
pose.so3() = SO3(q);-
q.normalize()：把相乘之后的四元数缩放为单位四元数； -
SO3(q)：用更新后的四元数构造旋转，赋值给 pose 的旋转部分，得到 q k + 1 q_{k+1} qk+1。
你对不上的最常见 3 个坑
坑 1：分不清四元数构造顺序 Quatd(w, x, y, z)
很多人误以为是Quatd(x,y,z,w)，把实部放最后，那就完全错位。
Eigen / 本书 sad 库：第一个参数是实部 w！
Quatd(1, a,b,c)→ w = 1 , ; x = a , ; y = b , ; z = c w=1,;x=a,;y=b,;z=c w=1,;x=a,;y=b,;z=c，正好匹配 Δ q = 1 , 1 2 ω Δ t \Delta q=1,\\frac12\\omega\\Delta t Δq=1,21ωΔt。
坑 2：把*当成普通数字相乘
cpp
A * B如果 A、B 是普通 double，就是标量乘；
如果 A、B 是Quatd对象，operator*被重载，执行**四元数乘法 ** ⊗ \otimes ⊗，不是简单逐项相乘。
坑 3：数学公式简写 1 , 1 2 ω Δ t \left1,\\frac12\\omega\\Delta t\\right 1,21ωΔt 格式含义
Δ q = 1 , 1 2 ω Δ t \Delta q=\left1,\\frac12\\boldsymbol\\omega\\Delta t\\right Δq=1,21ωΔt
这个简写格式含义：
-
第一个元素：实部 w = 1 w=1 w=1
-
后面矢量部分：虚部向量 1 2 ω Δ t \frac12\boldsymbol\omega\Delta t 21ωΔt
对应 C++ 构造函数入参顺序：
Quatd(w, x, y, z)，一一对应。
完整展开公式‑代码对照
离散一阶近似（来自四元数微分方程 q ˙ = 1 2 q 0 , ω T \dot q=\frac12 q0,\\omega^T q˙=21q0,ωT）
cpp
// q_k：上一时刻姿态四元数
Quatd q_k = pose.unit_quaternion();
// 增量Δq = [1, 0.5ωₓdt ,0.5ωᵧdt ,0.5ω_z dt ]
Quatd delta_q(1,
0.5*omega[0]*dt,
0.5*omega[1]*dt,
0.5*omega[2]*dt);
// 四元数乘法 q_{k+1}= q_k ⊗ Δq
Quatd q_k1 = q_k * delta_q;
q_k1.normalize(); // 强制单位四元数，修复模长漂移
pose.so3() = SO3(q_k1); // 更新姿态上面这段等价于原来一行代码，拆开写更容易看懂对应关系。
补充：这个近似到底舍弃了什么？
真实完整增量四元数（精确）：
Δ q r e a l = cos θ 2 , ; n sin θ 2 \Delta q_{real}=\left\\cos\\frac{\\theta}{2},; \\boldsymbol n\\sin\\frac{\\theta}{2}\\right Δqreal=cos2θ,;nsin2θ
一阶近似做两个近似：
当 θ = ∣ ω Δ t ∣ \theta=|\omega\Delta t| θ=∣ωΔt∣ 比较大，这两个等式偏差变大，引入角度截断误差 ；
normalize()只缩放整体，不能修正这部分近似。