高斯牛顿法求解三维变换矩阵的数学推导

目录

• 一、问题定义
• 二、李代数基础
• 三、雅可比矩阵推导
• 四、高斯牛顿迭代
• • 1. 整体雅可比矩阵
  • 2. 正规方程构建
  • 3. 参数更新
  • 4. 李代数更新
• 五、理论优势分析

一、问题定义

  给定两组三维点云：源点云 P={ p i ∈ R 3 } i = 1 N P = \\{p_i \\in \\mathbb{R}^3\\}_{i=1}^N P={pi​∈R3}i=1N​，目标点云 Q={ q i ∈ R 3 } i = 1 N Q = \\{q_i \\in \\mathbb{R}^3\\}_{i=1}^N Q={qi​∈R3}i=1N​。

求解变换矩阵 T= [ R t 0 1 ] T = \\begin{bmatrix} R & t \\\\ 0 & 1 \\end{bmatrix} T=[R0​t1​] (其中 $R\in SO(3)$, t∈ R 3 t \\in \\mathbb{R}^3 t∈R3)，最小化残差：
r i ( θ ) = q i − ( R p i + t ) ∈ R 3 (1) r_i(\\theta) = q_i - (Rp_i + t) \\in \\mathbb{R}^3 \\tag{1} ri​(θ)=qi​−(Rpi​+t)∈R3(1)
参数向量 θ=[ ω ⊤ , t ⊤ ] ⊤ ∈ R 6 \\theta = [\\omega^\\top, t^\\top]^\\top \\in \\mathbb{R}^6 θ=[ω⊤,t⊤]⊤∈R6，$\omega$ 为旋转向量（李代数 $\mathfrak{so}(3)$ 元素）。

目标函数：
min ⁡ θ 1 2 ∑ i = 1 N ∥ r i ( θ ) ∥ 2 (2) \\min_\\theta \\frac{1}{2} \\sum_{i=1}^N \\| r_i(\\theta) \\|^2 \\tag{2} θmin​21​i=1∑N​∥ri​(θ)∥2(2)

二、李代数基础

旋转矩阵 $R$ 与李代数 $\omega$ 通过指数映射关联：
R = exp ⁡ ( [ ω ] × ) (3) R = \\exp([\\omega]_\\times) \\tag{3} R=exp([ω]×​)(3)
其中 [⋅ ] × [\\cdot]_\\times [⋅]×​ 为反对称矩阵算子：
[ ω ] × = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] (4) [\\omega]_\\times = \\begin{bmatrix} 0 & -\\omega_z & \\omega_y \\\\ \\omega_z & 0 & -\\omega_x \\\\ -\\omega_y & \\omega_x & 0 \\end{bmatrix} \\tag{4} [ω]×​= ​0ωz​−ωy​​−ωz​0ωx​​ωy​−ωx​0​ ​(4)

对扰动 $\delta \omega$ 的一阶近似：
exp ⁡ ( [ δ ω ] × ) ≈ I + [ δ ω ] × (5) \\exp([\\delta\\omega]_\\times) \\approx I + [\\delta\\omega]_\\times \\tag{5} exp([δω]×​)≈I+[δω]×​(5)

三、雅可比矩阵推导

雅可比矩阵 J i = ∂ r i ∂ θ ∈ R 3 × 6 J_i = \\frac{\\partial r_i}{\\partial \\theta} \\in \\mathbb{R}^{3\\times6} Ji​=∂θ∂ri​​∈R3×6 分为旋转和平移部分：
J i = [ ∂ r i ∂ ω ∂ r i ∂ t ] J_i = \\begin{bmatrix} \\frac{\\partial r_i}{\\partial \\omega} & \\frac{\\partial r_i}{\\partial t} \\end{bmatrix} Ji​=[∂ω∂ri​​​∂t∂ri​​​]

1. 平移部分导数
   直接微分得：
   ∂ r i ∂ t = − I 3 × 3 (6) \\frac{\\partial r_i}{\\partial t} = -I_{3\\times3} \\tag{6} ∂t∂ri​​=−I3×3​(6)

2. 旋转部分导数
   考虑左扰动模型 $\delta \omega$：
   ∂ r i ∂ ω = lim ⁡ δ ω → 0 q i − exp ⁡ ( [ δ ω ] × ) R p i − t ⏞ 扰动后残差 − ( q i − R p i − t ) ⏞ 原残差 δ ω \\frac{\\partial r_i}{\\partial \\omega} = \\lim_{\\delta\\omega \\to 0} \\frac{ \\overbrace{ q_i - \\exp([\\delta\\omega]_\\times)Rp_i - t }^{\\text{扰动后残差}} - \\overbrace{ (q_i - Rp_i - t) }^{\\text{原残差}} }{\\delta\\omega} ∂ω∂ri​​=δω→0lim​δωqi​−exp([δω]×​)Rpi​−t ​扰动后残差​−(qi​−Rpi​−t) ​原残差​​

代入一阶近似：
exp ⁡ ( [ δ ω ] × ) R ≈ ( I + [ δ ω ] × ) R \\exp([\\delta\\omega]_\\times)R \\approx (I + [\\delta\\omega]_\\times)R exp([δω]×​)R≈(I+[δω]×​)R

残差变化量：
Δ r i ≈ − [ δ ω ] × R p i \\Delta r_i \\approx -[\\delta\\omega]_\\times Rp_i Δri​≈−[δω]×​Rpi​

利用叉积性质 [a ] × b=−[b ] × a [a]_\\times b = -[b]_\\times a [a]×​b=−[b]×​a：
[ δ ω ] × ( R p i ) = − [ R p i] × δ ω [\\delta\\omega]_\\times (Rp_i) = -[Rp_i]_\\times \\delta\\omega [δω]×​(Rpi​)=−[Rpi​]×​δω

因此：
∂ r i ∂ ω= [ R p i] × \\frac{\\partial r_i}{\\partial \\omega} = [R p_i]_\\times ∂ω∂ri​​=[Rpi​]×​

精确化处理（引入右雅可比矩阵）
当 $\Vert \omega \Vert$ 较大时，需引入右雅可比矩阵 J r (ω) J_r(\\omega) Jr​(ω) 修正：
J r ( ω ) = I 3 × 3+ 1 − cos ⁡ ∥ ω ∥ ∥ ω ∥ 2 [ ω ] × + ∥ ω ∥ − sin ⁡ ∥ ω ∥ ∥ ω ∥ 3 [ ω ] × 2 J_r(\\omega) = I_{3\\times3} + \\frac{1-\\cos\\|\\omega\\|}{\\|\\omega\\|^2}[\\omega]_\\times + \\frac{\\|\\omega\\|-\\sin\\|\\omega\\|}{\\|\\omega\\|^3}[\\omega]_\\times^2 Jr​(ω)=I3×3​+∥ω∥21−cos∥ω∥​[ω]×​+∥ω∥3∥ω∥−sin∥ω∥​[ω]×2​
最终导数形式：
∂ r i ∂ ω= R [ p i] ×J r ( ω ) \\frac{\\partial r_i}{\\partial \\omega} = R [p_i]_\\times J_r(\\omega) ∂ω∂ri​​=R[pi​]×​Jr​(ω)

四、高斯牛顿迭代

1. 整体雅可比矩阵

对于 $N$ 个点，整体雅可比矩阵 J∈ R 3 N × 6 J \\in \\mathbb{R}^{3N\\times6} J∈R3N×6：
J = [ J 1 ⊤ J 2 ⊤ ⋯ J N ⊤ ] ⊤ J = \\begin{bmatrix} J_1^\\top & J_2^\\top & \\cdots & J_N^\\top \\end{bmatrix}^\\top J=[J1⊤​​J2⊤​​⋯​JN⊤​​]⊤
其中 J i = [ [ R p i ] × − I 3 × 3 ] J_i = \\begin{bmatrix} [R p_i]_\\times & -I_{3\\times3} \\end{bmatrix} Ji​=[[Rpi​]×​​−I3×3​​]（简化形式）

2. 正规方程构建

近似 Hessian 矩阵：
H ≈ J ⊤ J ∈ R 6 × 6 H \\approx J^\\top J \\in \\mathbb{R}^{6\\times6} H≈J⊤J∈R6×6

梯度向量：
g = J ⊤ r ∈ R 6 g = J^\\top r \\in \\mathbb{R}^6 g=J⊤r∈R6

解线性系统：
( J ⊤ J ) Δ θ = − J ⊤ r (J^\\top J) \\Delta\\theta = -J^\\top r (J⊤J)Δθ=−J⊤r

3. 参数更新

θ k + 1= θ k + Δ θ \\theta_{k+1} = \\theta_k + \\Delta\\theta θk+1​=θk​+Δθ
其中 Δθ=[δ ω ⊤ ,δ t ⊤ ] ⊤ \\Delta\\theta = [\\delta\\omega^\\top, \\delta t^\\top]^\\top Δθ=[δω⊤,δt⊤]⊤

4. 李代数更新

对旋转部分执行流形上的更新：
R k + 1= exp ⁡ ( [ δ ω ] × ) R k R_{k+1} = \\exp([\\delta\\omega]_\\times) R_k Rk+1​=exp([δω]×​)Rk​
平移更新：
t k + 1= t k + δ t t_{k+1} = t_k + \\delta t tk+1​=tk​+δt

五、理论优势分析

1. 约束处理：李代数 $\mathfrak{so}(3)$ 将旋转矩阵优化转化为无约束问题
   $$\dim (\mathfrak{so}(3))=3⟹\text{避免 }SO(3)\text{ 的9参数过参数化}$$

2. 导数一致性：右雅可比矩阵 J r ( ω ) J_r(\\omega) Jr​(ω) 在 $\Vert \omega \Vert \to 0$ 时满足：
   lim ⁡ ∥ ω ∥ → 0J r ( ω ) = I 3 × 3 \\lim_{\\|\\omega\\|\\to 0} J_r(\\omega) = I_{3\\times3} ∥ω∥→0lim​Jr​(ω)=I3×3​
   保证小角度近似与大角度精确的统一

3. 收敛性保障：局部满足
   ∥ r ( θ k + 1 ) ∥ 2 < ∥ r ( θ k ) ∥ 2 (当初始估计接近真值) \\|r(\\theta_{k+1})\\|_2 < \\|r(\\theta_k)\\|_2 \\quad \\text{(当初始估计接近真值)} ∥r(θk+1​)∥2​<∥r(θk​)∥2​(当初始估计接近真值)