洗心岛1. 从相对位姿到绝对位姿：SE(3)约束的本质理解
在SLAM系统的后端优化中，位姿图优化是最核心的环节之一。当我们处理3D空间中的位姿约束时，通常会遇到两种基本形式：相对位姿约束和绝对位姿约束。相对位姿约束描述的是两个位姿节点之间的相对运动关系，而绝对位姿约束则直接将位姿节点与全局坐标系中的观测值联系起来。
绝对位姿SE(3)约束之所以重要，是因为它能够：
- 提供全局参考框架下的直接观测
- 有效防止累计误差的无限增长
- 在闭环检测中发挥关键作用
- 为系统提供绝对尺度信息
在实际工程中，我们常常需要将旋转和平移分开处理。这种分离处理的优势在于：
- 计算复杂度降低：SO(3)和R³可以分别优化
- 数值稳定性更好：避免耦合带来的病态问题
- 物理意义更明确：便于调试和分析
- 实现更灵活：可以根据需求选择是否耦合
关键提示：在实现分离优化时，必须注意旋转和平移之间的相关性不能被完全忽略，否则可能导致次优解。实践中通常采用交替优化的策略。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 数学基础：SO(3)和SE(3)的表示与运算
2.1 SO(3)群的基本性质
三维旋转群SO(3)是所有3×3实正交矩阵且行列式为1的矩阵构成的群。其关键性质包括：
- 闭合性：两个旋转矩阵的乘积仍是旋转矩阵
- 结合律：(R₁R₂)R₃ = R₁(R₂R₃)
- 单位元：存在单位矩阵I
- 逆元：对每个R存在逆矩阵R⁻¹=Rᵀ
在参数化方面，常用的表示方法有：
- 旋转矩阵：3×3矩阵，9个参数+6个约束
- 轴角表示：3个参数，紧凑但有奇异性
- 欧拉角：3个参数，直观但有万向锁问题
- 四元数：4个参数，计算效率高
2.2 SE(3)群的表示与运算
刚体运动的特殊欧氏群SE(3)可以表示为：
SE(3) =
其群运算定义为：
(R₁,t₁)·(R₂,t₂) = (R₁R₂, R₁t₂+t₁)
对应的李代数se(3)可以表示为：
ξ = [ρ,φ]ᵀ ∈ R⁶
其中φ对应旋转部分，ρ对应平移部分。指数映射将李代数映射到李群：
exp(ξ^) = (exp(φ^), Jρ)
3. 右乘形式的位姿约束建模
3.1 残差函数的定义
在右乘形式的位姿约束中，我们定义残差函数为观测值减去预测值：