一、問題定義

??給定兩組三維點云：源點云 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 }^{?},t^{?}{]}^{?}\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)}$$
其中 $[?{]}_{\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{?r_i}{?\theta }\in {\mathbb{R}}^{3\times 6}$ 分為旋轉和平移部分：
$$J_i=\left[\begin{array}{cc}\frac{?r_i}{?\omega }& \frac{?r_i}{?t}\end{array}\right]$$

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

2. 旋轉部分導數
   考慮左擾動模型 $\delta \omega$：
   $$\frac{?r_i}{?\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{?r_i}{?\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{?r_i}{?\omega }=R[p_i{]}_{\times }{J}_r(\omega )$$

---

四、高斯牛頓迭代

1. 整體雅可比矩陣

對于 $N$ 個點，整體雅可比矩陣 $J\in {\mathbb{R}}^{3N\times 6}$：
$$J={\left[\begin{array}{cccc}{J}_1^{?}& J_2^{?}& ?& J_N^{?}\end{array}\right]}^{?}$$
其中 $J_i=\left[\begin{array}{cc}[R{p}_i{]}_{\times }& ?I_{3\times 3}\end{array}\right]$（簡化形式）

2. 正規方程構建

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

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

解線性系統：
$$(J^{?}J)\Delta \theta =?J^{?}r$$

3. 參數更新

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

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{(當初始估計接近真值)}$$

---