一、问题定义

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

求解变换矩阵 $T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$ (其中 $R\in SO(3)$, $t\in {\mathbb{R}}^3$)，最小化残差：
$$r_i(\theta )=q_i-(R{p}_i+t)\in {\mathbb{R}}^3\text{\hspace{1em}(1)}$$
参数向量 $\theta =[{\omega }^{\top },t^{\top }{]}^{\top }\in {\mathbb{R}}^6$，$\omega$ 为旋转向量（李代数 $\mathfrak{so}(3)$ 元素）。

目标函数：
$$\underset{\theta }{\min }\frac{1}{2}\sum _{i=1}^N\Vert r_i(\theta ){\Vert }^2\text{\hspace{1em}(2)}$$

二、李代数基础

旋转矩阵 $R$ 与李代数 $\omega$ 通过指数映射关联：
$$R=\exp ([\omega {]}_{\times })\text{\hspace{1em}(3)}$$
其中 $[\cdot {]}_{\times }$ 为反对称矩阵算子：
$$[\omega {]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]\text{\hspace{1em}(4)}$$

对扰动 $\delta \omega$ 的一阶近似：
$$\exp ([\delta \omega {]}_{\times })\approx I+[\delta \omega {]}_{\times }\text{\hspace{1em}(5)}$$

三、雅可比矩阵推导

雅可比矩阵 $J_i=\frac{\partial r_i}{\partial \theta }\in {\mathbb{R}}^{3\times 6}$ 分为旋转和平移部分：
$$J_i=\left[\begin{array}{cc}\frac{\partial r_i}{\partial \omega }& \frac{\partial r_i}{\partial t}\end{array}\right]$$

1. 平移部分导数
   直接微分得：
   $$\frac{\partial r_i}{\partial t}=-I_{3\times 3}\text{\hspace{1em}(6)}$$

2. 旋转部分导数
   考虑左扰动模型 $\delta \omega$：
   $$\frac{\partial r_i}{\partial \omega }=\underset{\delta \omega \to 0}{\lim }\frac{\stackrel{\text{扰动后残差}}{\overbrace{q_i-\exp ([\delta \omega {]}_{\times })R{p}_i-t}}-\stackrel{\text{原残差}}{\overbrace{(q_i-R{p}_i-t)}}}{\delta \omega }$$

代入一阶近似：
$$\exp ([\delta \omega {]}_{\times })R\approx (I+[\delta \omega {]}_{\times })R$$

残差变化量：
$$\Delta r_i\approx -[\delta \omega {]}_{\times }R{p}_i$$

利用叉积性质 $[a{]}_{\times }b=-[b{]}_{\times }a$：
$$[\delta \omega {]}_{\times }(R{p}_i)=-[R{p}_i{]}_{\times }\delta \omega$$

因此：
$$\frac{\partial r_i}{\partial \omega }=[R{p}_i{]}_{\times }$$

精确化处理（引入右雅可比矩阵）
当 $\Vert \omega \Vert$ 较大时，需引入右雅可比矩阵 $J_r(\omega )$ 修正：
$$J_r(\omega )=I_{3\times 3}+\frac{1-\cos \Vert \omega \Vert }{\Vert \omega {\Vert }^2}[\omega {]}_{\times }+\frac{\Vert \omega \Vert -\sin \Vert \omega \Vert }{\Vert \omega {\Vert }^3}[\omega {]}_{\times }^2$$
最终导数形式：
$$\frac{\partial r_i}{\partial \omega }=R[p_i{]}_{\times }{J}_r(\omega )$$

---

四、高斯牛顿迭代

1. 整体雅可比矩阵

对于 $N$ 个点，整体雅可比矩阵 $J\in {\mathbb{R}}^{3N\times 6}$：
$$J={\left[\begin{array}{cccc}{J}_1^{\top }& J_2^{\top }& \cdots & J_N^{\top }\end{array}\right]}^{\top }$$
其中 $J_i=\left[\begin{array}{cc}[R{p}_i{]}_{\times }& -I_{3\times 3}\end{array}\right]$（简化形式）

2. 正规方程构建

近似 Hessian 矩阵：
$$H\approx J^{\top }J\in {\mathbb{R}}^{6\times 6}$$

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

解线性系统：
$$(J^{\top }J)\Delta \theta =-J^{\top }r$$

3. 参数更新

$${\theta }_{k+1}={\theta }_k+\Delta \theta$$
其中 $\Delta \theta =[\delta {\omega }^{\top },\delta t^{\top }{]}^{\top }$

4. 李代数更新

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

---

五、理论优势分析

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

2. 导数一致性：右雅可比矩阵 $J_r(\omega )$ 在 $\Vert \omega \Vert \to 0$ 时满足：
   $$\underset{\Vert \omega \Vert \to 0}{\lim }{J}_r(\omega )=I_{3\times 3}$$
   保证小角度近似与大角度精确的统一

3. 收敛性保障：局部满足
   $$\Vert r({\theta }_{k+1}){\Vert }_2<\Vert r({\theta }_k){\Vert }_2\text{(当初始估计接近真值)}$$

---