【ICP点云配准】从数学原理到手写C++——SVD求解与多分辨率优化

前言

• 最近在项目中做移动机器人定位时，遇到了一个问题：TF 给的里程计位姿有累积误差，时间一长机器人就"飘"了，而 GPS 在室内又不可用。这时候就需要一种基于激光点云的实时定位方法来修正位姿。
• 刚好之前我们在【基于OctoMap与地面约束的三维A*路径规划方法实现】 中构建了点云地图，现在手里有了一份先验全局地图，自然想到：能不能让机器人用当前帧点云和全局地图做匹配 ，反推自己在地图里的精确位姿？这就引出了定位领域最经典的方法------基于 ICP 的点云定位。
• 本文将从 ICP 的数学公式推导出发，到手写 C++ 实现 ，包含 SVD 闭式求解、离群点剔除、多分辨率加速和自适应退火等工程优化，最终给出一个完整的 ROS 实时定位节点（ICPLocalizer）。

---

0 前置数学知识

0-1 旋转矩阵与 SO(3)

• 在三维空间中，旋转可以用一个 3 × 3 3\times3 3×3 的矩阵 R R R 表示。但并不是随便一个 3 × 3 3\times3 3×3 矩阵都能当旋转矩阵------它必须属于特殊正交群 SO(3)，满足两个约束：

R T R = I （正交性） , det ⁡ ( R ) = + 1 （排除镜面反射） R^T R = I \quad\text{（正交性）},\qquad \det(R) = +1 \quad\text{（排除镜面反射）} RTR=I（正交性）,det(R)=+1（排除镜面反射）

• 正交性 R T R = I R^T R = I RTR=I 的含义： R R R 的三列互相正交且都是单位向量。说人话就是：

旋转不改变向量的长度，也不改变向量之间的夹角------它只是在"转动"坐标系。

• det ⁡ ( R ) = + 1 \det(R) = +1 det(R)=+1 排除了 det ⁡ ( R ) = − 1 \det(R) = -1 det(R)=−1 的情况（镜面反射/翻转），保证是"纯旋转"。这个条件在从矩阵中提取旋转时需要特别注意------很多代数方法能解出正交矩阵，但不保证行列式为 + 1 +1 +1，需要额外修正。

• 举个例子你就懂了。绕 z z z 轴旋转 45 ∘ 45^\circ 45∘ 的旋转矩阵：

R z ( 45 ∘ ) = cos ⁡ 45 ∘ − sin ⁡ 45 ∘ 0 sin ⁡ 45 ∘ cos ⁡ 45 ∘ 0 0 0 1 = 2 2 − 2 2 0 2 2 2 2 0 0 0 1 ≈ 0.707 − 0.707 0 0.707 0.707 0 0 0 1 R_z(45^\circ) = \begin{bmatrix} \cos 45^\circ & -\sin 45^\circ & 0 \\ \sin 45^\circ & \cos 45^\circ & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & 0 \\ \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 \\ 0 & 0 & 1 \end{bmatrix} \approx \begin{bmatrix} 0.707 & -0.707 & 0 \\ 0.707 & 0.707 & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz(45∘)= cos45∘sin45∘0−sin45∘cos45∘0001 = 22 22 0−22 22 0001 ≈ 0.7070.7070−0.7070.7070001

• 验证两个条件：

  • 正交性 ： R T R = 0.707 0.707 0 − 0.707 0.707 0 0 0 1 0.707 − 0.707 0 0.707 0.707 0 0 0 1 = 1 0 0 0 1 0 0 0 1 = I R^T R = \begin{bmatrix}0.707&0.707&0\\-0.707&0.707&0\\0&0&1\end{bmatrix} \begin{bmatrix}0.707&-0.707&0\\0.707&0.707&0\\0&0&1\end{bmatrix} = \begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix} = I RTR= 0.707−0.70700.7070.7070001 0.7070.7070−0.7070.7070001 = 100010001 =I ✓
  • 行列式 ： det ⁡ ( R ) = ( 0.707 2 + 0.707 2 ) × 1 = 1 \det(R) = (0.707^2 + 0.707^2) \times 1 = 1 det(R)=(0.7072+0.7072)×1=1 ✓（用前两列叉积点乘第三列也可验证）

• 拿一个向量试试效果。设 v = 1 0 0 v = \begin{bmatrix}1\\0\\0\end{bmatrix} v= 100 （沿 x 轴的单位向量），旋转后：

  R ⋅ v = 0.707 0.707 0 R \cdot v = \begin{bmatrix}0.707\\0.707\\0\end{bmatrix} R⋅v= 0.7070.7070

  向量长度： 0.707 2 + 0.707 2 + 0 2 = 1 \sqrt{0.707^2 + 0.707^2 + 0^2} = 1 0.7072+0.7072+02 =1，和旋转前一样。旋转不改变长度，这就是正交性的几何含义。

• 再看一个反例 ------镜面反射矩阵（沿 x x x 轴翻转）：

  M = − 1 0 0 0 1 0 0 0 1 M = \begin{bmatrix} -1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} M= −100010001

  它也满足 M T M = I M^T M = I MTM=I（正交），但 det ⁡ ( M ) = − 1 \det(M) = -1 det(M)=−1。如果把它当旋转用，会把右手坐标系变成左手坐标系------这不是真实的物理旋转，而是翻转了整个空间。这就是为什么 SVD 求出 R = V U T R = VU^T R=VUT 后必须检查行列式并修正： V U T VU^T VUT 保证正交，但不保证是旋转还是反射。

0-2 SVD 奇异值分解

• SVD（Singular Value Decomposition） 是矩阵分析中最核心的分解方法之一。对于任意 m × n m\times n m×n 矩阵 A A A，都可以分解为：

A = U ⋅ Σ ⋅ V T A = U \cdot \Sigma \cdot V^T A=U⋅Σ⋅VT

其中：

• U U U（ m × m m\times m m×m）：左奇异向量矩阵 ，列是 A A T AA^T AAT 的特征向量，彼此正交，构成 A A A 行空间的一组标准正交基。

• Σ \Sigma Σ（ m × n m\times n m×n）：奇异值对角矩阵 ，对角元素 σ i ≥ 0 \sigma_i \geq 0 σi≥0 从大到小排列，每个 σ i \sigma_i σi 表示矩阵沿对应主轴方向的"伸缩强度"。

• V V V（ n × n n\times n n×n）：右奇异向量矩阵 ，列是 A T A A^T A ATA 的特征向量，彼此正交，构成 A A A 列空间的一组标准正交基。

• 几何直觉：任意矩阵代表的线性变换，本质上是 "旋转 → 缩放 → 再旋转" 三步：

A ⋅ x = U ⋅ ( Σ ⋅ ( V T ⋅ x ) ) A \cdot x = U \cdot (\Sigma \cdot (V^T \cdot x)) A⋅x=U⋅(Σ⋅(VT⋅x))

• 拆开来看： V T V^T VT 先把 x x x 旋转到"标准方向"， Σ \Sigma Σ 沿各坐标轴拉伸/压缩， U U U 再把结果旋转到最终位置。
• 说人话就是：

SVD 把一个复杂的矩阵变换拆成"两组正交坐标轴 + 一组缩放系数"三样东西。它最常用的场景是：给你两个空间，想找一个最优旋转把一边转到另一边------SVD 能直接给你答案。

• 举个例子你就懂了。假设有一个 3 × 3 3\times3 3×3 矩阵：

A = 3 2 2 2 3 − 2 0 0 1 A = \begin{bmatrix} 3 & 2 & 2 \\ 2 & 3 & -2 \\ 0 & 0 & 1 \end{bmatrix} A= 3202302−21

• 对它做 SVD 分解，得到三样东西：

U = 1 2 1 2 0 1 2 − 1 2 0 0 0 1 , Σ = 5 0 0 0 1 0 0 0 1 , V T = 1 2 1 2 0 1 2 − 1 2 0 0 0 1 U = \begin{bmatrix} \frac{1}{\sqrt{2}} & \frac{1}{\sqrt{2}} & 0 \\ \frac{1}{\sqrt{2}} & -\frac{1}{\sqrt{2}} & 0 \\ 0 & 0 & 1 \end{bmatrix},\quad \Sigma = \begin{bmatrix} 5 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix},\quad V^T = \begin{bmatrix} \frac{1}{\sqrt{2}} & \frac{1}{\sqrt{2}} & 0 \\ \frac{1}{\sqrt{2}} & -\frac{1}{\sqrt{2}} & 0 \\ 0 & 0 & 1 \end{bmatrix} U= 2 12 102 1−2 10001 ,Σ= 500010001 ,VT= 2 12 102 1−2 10001

• 验算一下： U Σ V T U \Sigma V^T UΣVT 确实等于 A A A（你可以自己乘一遍验证）。

• 来看这三样东西分别是什么：

  • U U U （左奇异向量）：三列分别是 1 / 2 1 / 2 0 \begin{bmatrix}1/\sqrt{2}\\1/\sqrt{2}\\0\end{bmatrix} 1/2 1/2 0 、 1 / 2 − 1 / 2 0 \begin{bmatrix}1/\sqrt{2}\\-1/\sqrt{2}\\0\end{bmatrix} 1/2 −1/2 0 、 0 0 1 \begin{bmatrix}0\\0\\1\end{bmatrix} 001 ，两两正交且都是单位向量，构成"目标空间"的一组标准正交基。
  • Σ \Sigma Σ （奇异值）：对角线上是 5 , 1 , 1 5, 1, 1 5,1,1，从大到小排列。第一个奇异值 5 远大于后两个，说明矩阵 A A A 沿第一个"主轴方向"的伸缩强度是其他方向的 5 倍------这个方向主导了整个变换。
  • V T V^T VT （右奇异向量转置）：三行同样是单位正交向量，构成"原始空间"的一组标准正交基。注意这里 V V V 和 U U U 长得一样（因为 A A A 恰好对称），但一般情况下 U U U 和 V V V 是不同的。

• 把这个 SVD 拆成"旋转 → 缩放 → 旋转"三步来看：

  • 第一步 V T x V^T x VTx ：把输入向量 x x x 旋转到"标准方向"（让坐标轴对齐 A A A 的主轴）
  • 第二步 Σ ( V T x ) \Sigma(V^T x) Σ(VTx)：沿三个主轴分别拉伸 5 倍、1 倍、1 倍（z 轴方向不变）
  • 第三步 U ( Σ V T x ) U(\Sigma V^T x) U(ΣVTx)：把拉伸后的结果再旋转到最终位置

• 这就是 SVD 的核心威力------任何矩阵的作用，本质上就是换一组坐标轴（V^T），沿轴拉伸/压缩（Σ），再换回原来的坐标轴（U） 。后面 ICP 推导中，我们会用这个性质从交叉协方差矩阵 H H H 中直接"榨取"出最优旋转。

• 总结一下，SVD 在工程中主要干四件事：

用途	怎么做	典型场景
求最优旋转	对 M M M 做 SVD， Ω = V U T \Omega = VU^T Ω=VUT	正交 Procrustes、ICP 配准
降维 / 主成分分析（PCA）	取前 k k k 个奇异值对应的 U U U 列	数据压缩、特征提取、点云法向量估计
求伪逆	A + = V Σ + U T A^+ = V\Sigma^+ U^T A+=VΣ+UT（ Σ + \Sigma^+ Σ+ 是非零奇异值取倒数）	解超定/欠定线性方程组
低秩近似	只保留最大的几个奇异值，其余置零	矩阵压缩、去噪、推荐系统

• 本文最关心的是第一行------用 SVD 从一堆点对中"榨"出最优旋转矩阵。接下来的 0-4 节会展开讲这个问题，第 2 章则把它用到 ICP 的位姿求解中。

0-3 矩阵的迹（Trace）

• 矩阵的迹定义为对角元素之和： tr ( A ) = ∑ i A i i \text{tr}(A) = \sum_i A_{ii} tr(A)=∑iAii
• 迹最重要的性质是循环置换不变性：

tr ( A B ) = tr ( B A ) \text{tr}(AB) = \text{tr}(BA) tr(AB)=tr(BA)

• 反复使用这条性质，可以推出更长的循环链：

tr ( A B C ) = tr ( B C A ) = tr ( C A B ) \text{tr}(ABC) = \text{tr}(BCA) = \text{tr}(CAB) tr(ABC)=tr(BCA)=tr(CAB)

• 迹经常出现在"求和转矩阵"的场景中。比如，若有一堆向量 a i , b i a_i, b_i ai,bi 和一个公共矩阵 R R R：

∑ i a i T R b i = tr ( R ∑ i b i a i T ) \sum_i a_i^T R b_i = \text{tr}\left(R \sum_i b_i a_i^T\right) i∑aiTRbi=tr(Ri∑biaiT)

• 这个等式把标量求和变成了矩阵乘法加取迹------它能把"最小化一堆点对的距离"这类几何问题转化成"最大化某个矩阵的迹"这种代数问题，而迹的最大化往往有漂亮的闭式解。

0-4 正交 Procrustes 问题

• 正交 Procrustes 问题是最优化理论中的一个经典问题：

给定两组已中心化的点 { p i } \{p_i\} {pi} 和 { q i } \{q_i\} {qi}（一一对应），找到一个正交矩阵 Ω \Omega Ω，最小化 ∑ ∥ Ω p i − q i ∥ 2 \sum \|\Omega p_i - q_i\|^2 ∑∥Ωpi−qi∥2

• 名字来源：Procrustes 是希腊神话中的拦路大盗，他有一张铁床，抓到路人就绑上去------长了砍腿、短了拉长，直到和床"完美匹配"。这个名字非常形象：我们也是在"强行把一组点旋转到和另一组点对齐"。

• 说人话就是：

你手里有两组点，你知道它们一一对应（第 i 个 p 就对应第 i 个 q），而且它们之间只差一个旋转。问题是：怎么把这个旋转找出来？Procrustes 告诉你：构造 M → SVD → Ω = VU^T，三步搞定。

• 举个例子你就懂了。考虑二维平面上两组点：

p 1 = 1 0 ,    p 2 = 0 1 q 1 = 0 1 ,    q 2 = − 1 0 p_1 = \begin{bmatrix}1\\0\end{bmatrix},\; p_2 = \begin{bmatrix}0\\1\end{bmatrix} \qquad q_1 = \begin{bmatrix}0\\1\end{bmatrix},\; q_2 = \begin{bmatrix}-1\\0\end{bmatrix} p1=10,p2=01q1=01,q2=−10

• 用眼睛看就知道： { q i } \{q_i\} {qi} 是 { p i } \{p_i\} {pi} 绕原点逆时针旋转 90 ∘ 90^\circ 90∘ 的结果。旋转矩阵应该是：

R 90 ∘ = cos ⁡ 90 ∘ − sin ⁡ 90 ∘ sin ⁡ 90 ∘ cos ⁡ 90 ∘ = 0 − 1 1 0 R_{90^\circ} = \begin{bmatrix}\cos 90^\circ & -\sin 90^\circ \\ \sin 90^\circ & \cos 90^\circ\end{bmatrix} = \begin{bmatrix}0 & -1 \\ 1 & 0\end{bmatrix} R90∘=cos90∘sin90∘−sin90∘cos90∘=01−10

• 现在假装不知道，用 Procrustes 方法来推。第一步，构造矩阵 M M M：

M = ∑ i = 1 2 p i q i T = p 1 q 1 T + p 2 q 2 T M = \sum_{i=1}^{2} p_i q_i^T = p_1 q_1^T + p_2 q_2^T M=i=1∑2piqiT=p1q1T+p2q2T

= 1 0 0 1 + 0 1 − 1 0 = \begin{bmatrix}1\\0\end{bmatrix}\begin{bmatrix}0&1\end{bmatrix} + \begin{bmatrix}0\\1\end{bmatrix}\begin{bmatrix}-1&0\end{bmatrix} =1001+01−10

= 0 1 0 0 + 0 0 − 1 0 = 0 1 − 1 0 = \begin{bmatrix}0&1\\0&0\end{bmatrix} + \begin{bmatrix}0&0\\-1&0\end{bmatrix} = \begin{bmatrix}0 & 1 \\ -1 & 0\end{bmatrix} =0010+0−100=0−110

• 第二步，对 M M M 做 SVD。 观察一下： M = 0 1 − 1 0 M = \begin{bmatrix}0&1\\-1&0\end{bmatrix} M=0−110，它的两列是 0 − 1 \begin{bmatrix}0\\-1\end{bmatrix} 0−1 和 1 0 \begin{bmatrix}1\\0\end{bmatrix} 10，互相正交且都是单位向量------ M M M 本身已经是一个旋转矩阵（顺时针 90 ∘ 90^\circ 90∘）。因此 SVD 非常简单：

U = M = 0 1 − 1 0 , Σ = 1 0 0 1 , V = 1 0 0 1 U = M = \begin{bmatrix}0&1\\-1&0\end{bmatrix},\quad \Sigma = \begin{bmatrix}1&0\\0&1\end{bmatrix},\quad V = \begin{bmatrix}1&0\\0&1\end{bmatrix} U=M=0−110,Σ=1001,V=1001

• 第三步，读出最优旋转：

Ω = V U T = 1 0 0 1 0 − 1 1 0 = 0 − 1 1 0 \Omega = V U^T = \begin{bmatrix}1&0\\0&1\end{bmatrix} \begin{bmatrix}0&-1\\1&0\end{bmatrix} = \begin{bmatrix}0 & -1 \\ 1 & 0\end{bmatrix} Ω=VUT=100101−10=01−10

• 这就是 R 90 ∘ R_{90^\circ} R90∘！SVD 把藏在 M M M 里的旋转矩阵"提取"了出来。验证一下：

Ω p 1 = 0 − 1 1 0 1 0 = 0 1 = q 1    ✓ \Omega p_1 = \begin{bmatrix}0&-1\\1&0\end{bmatrix}\begin{bmatrix}1\\0\end{bmatrix} = \begin{bmatrix}0\\1\end{bmatrix} = q_1 \;\checkmark Ωp1=01−1010=01=q1✓

Ω p 2 = 0 − 1 1 0 0 1 = − 1 0 = q 2    ✓ \Omega p_2 = \begin{bmatrix}0&-1\\1&0\end{bmatrix}\begin{bmatrix}0\\1\end{bmatrix} = \begin{bmatrix}-1\\0\end{bmatrix} = q_2 \;\checkmark Ωp2=01−1001=−10=q2✓

• 完美匹配。注意这个例子中 M M M 碰巧本身就是旋转矩阵（SVD 退化了），一般情况下 M M M 不是正交的，但 SVD 同样能从 M M M 中提取出最优 Ω \Omega Ω。核心流程就是：构造 M = ∑ p i q i T M = \sum p_i q_i^T M=∑piqiT → SVD 得 U , V U, V U,V → Ω = V U T \Omega = VU^T Ω=VUT → 检查 det ⁡ ( Ω ) \det(\Omega) det(Ω) 并修正。

• 该问题的闭式解由 Schönemann (1966) 给出，Arun et al. (1987) 将其引入点云配准领域。不需要迭代，不需要梯度下降，SVD 一步到位。后面 ICP 推导的 2-3、2-4 节，本质上就是把配准问题化简后套这个流程。

---

1 基于ICP的点云定位

1-1 定位问题的形式化

• 在移动机器人定位中，我们手里通常有两样东西：
  • 全局地图（target）：事先建好的点云地图（比如通过 SLAM 或点云累积得到），固定在 world/map 坐标系下
  • 当前帧扫描（source）：激光雷达实时采集的一帧点云，在雷达自身坐标系（laser_livox）下
• 定位要解决的问题就是：当前雷达在 map 坐标系下的位姿 T T T 是什么？
• 这就变成了一个点云配准问题：找到一个 T T T，把当前扫描变换到 map 坐标系后，和地图尽可能重合。而这个 T T T，就是我们要求解的机器人位姿。
• ICP（Iterative Closest Point）由 Besl & McKay 在 1992 年提出，是解决这类配准问题最经典的算法。用在定位场景中：
  • 输入：source（当前扫描）、target（先验地图）、init_guess（TF/里程计给的粗略位姿）
  • 输出：精修的位姿 T T T（即机器人在 map 下的精确位置和姿态）
• 说人话就是：

你已经有一张"世界地图"了，现在雷达扫到一帧点云，TF 告诉你"机器人大概在这个位置附近"。ICP 做的事情就是拿当前扫描去和地图比对，把你"吸"到地图上最吻合的那个精确位置------这本质上就是用点云做实时定位。

1-2 核心思想

• 基于 ICP 的定位，本质上是一个 "猜测---匹配---修正"的迭代过程：

  1. 猜测（Predict） ：用当前的位姿估计 T T T（初始来自TF/里程计），把当前扫描点云变换到 map 坐标系下
  2. 匹配（Match）：对变换后的每个扫描点，在地图中找最近邻------"这个扫描点最可能对应地图里的哪个点"
  3. 修正（Correct） ：找到最优旋转 R R R 和平移 t t t，最小化所有匹配点对的距离平方和------"把扫描整体'掰'到和地图最贴合的位置"

• 修正完拿到新位姿后，回到步骤 1 重新匹配，反复迭代直到位姿不再显著变化。整个定位闭环是：

TF 给初值 → ICP 精修位姿 → 发布 /icp_pose 和 TF → 下一帧扫描到来，用上一帧的结果做初值 → ...

---

2 数学推导

2-1 问题建模

• ICP 的优化目标非常直观------最小化配准后所有对应点对的欧氏距离平方和：

min ⁡ R , t ∑ i = 1 K ∥ R ⋅ p i + t − q i ∥ 2 \min_{R,t} \sum_{i=1}^{K} \| R \cdot p_i + t - q_i \|^2 R,tmini=1∑K∥R⋅pi+t−qi∥2

其中：

• p i p_i pi：source 点云中的点（原始坐标，不动）

• q i q_i qi：target 点云中 p i p_i pi 的最近邻对应点

• R ∈ S O ( 3 ) R \in SO(3) R∈SO(3)：旋转矩阵（ 3 × 3 3\times3 3×3，满足 R T R = I R^TR=I RTR=I 且 det ⁡ ( R ) = 1 \det(R)=1 det(R)=1）

• t ∈ R 3 t \in \mathbb{R}^3 t∈R3：平移向量

• K K K：有效对应点对的数量

• 这个目标函数里， R R R 和 t t t 耦合在一起。但注意一个关键观察：旋转不改变质心之间的相对关系。利用这一点，我们可以把平移先分离出去，把问题拆成"先求旋转、再求平移"两步。

2-2 质心与去心坐标

• 首先计算两帧点云中匹配点的质心（均值）：

p ˉ = 1 K ∑ i = 1 K p i , q ˉ = 1 K ∑ i = 1 K q i p̄ = \frac{1}{K}\sum_{i=1}^{K} p_i,\qquad q̄ = \frac{1}{K}\sum_{i=1}^{K} q_i pˉ=K1i=1∑Kpi,qˉ=K1i=1∑Kqi

• 然后把每个点减去质心，得到"去心坐标"：

p ~ i = p i − p ˉ , q ~ i = q i − q ˉ \tilde{p}_i = p_i - p̄,\qquad \tilde{q}_i = q_i - q̄ p~i=pi−pˉ,q~i=qi−qˉ

• 为什么要去质心？因为旋转只关心"方向"，不关心"中心"。去掉质心后，平移量 t t t 可以从旋转中解耦出来，问题简化为：

min ⁡ R ∑ i = 1 K ∥ R ⋅ p ~ i − q ~ i ∥ 2 \min_{R} \sum_{i=1}^{K} \| R \cdot \tilde{p}_i - \tilde{q}_i \|^2 Rmini=1∑K∥R⋅p~i−q~i∥2

• 说人话就是：

先把两团点的中心对齐，剩下的就只剩下旋转了。求完旋转后，平移 = 目标质心 − 旋转后的源质心。

2-3 交叉协方差矩阵

• 展开上述目标函数：

∑ i = 1 K ∥ R p ~ i − q ~ i ∥ 2 = ∑ ( p ~ i T p ~ i + q ~ i T q ~ i − 2 q ~ i T R p ~ i ) \sum_{i=1}^{K} \| R\tilde{p}_i - \tilde{q}_i \|^2 = \sum (\tilde{p}_i^T \tilde{p}_i + \tilde{q}_i^T \tilde{q}_i - 2\tilde{q}_i^T R \tilde{p}_i) i=1∑K∥Rp~i−q~i∥2=∑(p~iTp~i+q~iTq~i−2q~iTRp~i)

• 前两项与 R R R 无关，因此最小化原式等价于最大化：

∑ i = 1 K q ~ i T R p ~ i = tr ( R ∑ i = 1 K p ~ i q ~ i T ) \sum_{i=1}^{K} \tilde{q}_i^T R \tilde{p}i = \text{tr}\left(R \sum{i=1}^{K} \tilde{p}_i \tilde{q}_i^T\right) i=1∑Kq~iTRp~i=tr(Ri=1∑Kp~iq~iT)

• 这一步的转换用到的正是我们在 0-3 节讲的迹的循环置换性质 ： tr ( A B ) = tr ( B A ) \text{tr}(AB) = \text{tr}(BA) tr(AB)=tr(BA)。把标量 q ~ i T R p ~ i \tilde{q}_i^T R \tilde{p}_i q~iTRp~i 看成一个 1 × 1 1\times1 1×1 矩阵（它自己就是自己的迹），循环移位一下： q ~ i T ( R p ~ i ) = tr ( q ~ i T R p ~ i ) = tr ( R p ~ i q ~ i T ) \tilde{q}_i^T (R \tilde{p}_i) = \text{tr}(\tilde{q}_i^T R \tilde{p}_i) = \text{tr}(R \tilde{p}_i \tilde{q}_i^T) q~iT(Rp~i)=tr(q~iTRp~i)=tr(Rp~iq~iT)，再对 i i i 求和就把 R R R 提到了迹外面。这样就把"一堆点对的几何匹配"转化成了"一个矩阵的代数优化"。

• 定义交叉协方差矩阵 H H H （ 3 × 3 3\times3 3×3）：

H = ∑ i = 1 K p ~ i q ~ i T H = \sum_{i=1}^{K} \tilde{p}_i \tilde{q}_i^T H=i=1∑Kp~iq~iT

• 写成矩阵形式更直观------把所有去心坐标拼成 3 × K 3\times K 3×K 矩阵：

H = P ~ ⋅ Q ~ T H = \tilde{P} \cdot \tilde{Q}^T H=P~⋅Q~T

其中 P ~ = p \~ 1 , . . . , p \~ K \tilde{P} = \\tilde{p}_1, ..., \\tilde{p}_K P~=p\~1,...,p\~K， Q ~ = q \~ 1 , . . . , q \~ K \tilde{Q} = \\tilde{q}_1, ..., \\tilde{q}_K Q~=q\~1,...,q\~K。

• H H H 的几何意义：它的每个元素 H m n H_{mn} Hmn 表示"source 在第 m m m 维和 target 在第 n n n 维的相关性"。 H H H 的 SVD 分解会告诉我们"哪个旋转方向让两帧点云最对齐"。

2-4 SVD 求解最优旋转

• 对 H H H 进行奇异值分解：

H = U ⋅ Σ ⋅ V T H = U \cdot \Sigma \cdot V^T H=U⋅Σ⋅VT

其中 U , V U, V U,V 是 3 × 3 3\times3 3×3 正交矩阵， Σ \Sigma Σ 是对角阵（奇异值）。

• 关键一步： 最优旋转矩阵为：

R = V ⋅ U T R = V \cdot U^T R=V⋅UT

• 为什么？这正好是我们在 0-4 节提到的正交 Procrustes 问题的闭式解：构造 H H H → SVD → R = V U T R = VU^T R=VUT，一步到位，不需要梯度下降。直观理解：

  • U U U 的列是 H H H 的左奇异向量（target 空间的"主轴"）
  • V V V 的列是 H H H 的右奇异向量（source 空间的"主轴"）
  • V U T VU^T VUT 正好是"把 source 主轴旋转到 target 主轴"的最小二乘最优旋转

• 行列式修正： SVD 不能保证 det ⁡ ( R ) = + 1 \det(R) = +1 det(R)=+1（可能出现 det ⁡ ( R ) = − 1 \det(R) = -1 det(R)=−1，即包含了镜面反射）。修正方法：

if det ⁡ ( V U T ) < 0 : R = V ⋅ diag ( 1 , 1 , − 1 ) ⋅ U T \text{if } \det(VU^T) < 0: \quad R = V \cdot \text{diag}(1, 1, -1) \cdot U^T if det(VUT)<0:R=V⋅diag(1,1,−1)⋅UT

这相当于把 V V V 的最后一列取反，保证 R R R 是纯旋转。

2-5 平移求解

• 有了 R R R 之后， t t t 一步到位：

t = q ˉ − R ⋅ p ˉ t = q̄ - R \cdot p̄ t=qˉ−R⋅pˉ

• 这就是"目标质心 − 旋转后的源质心"，物理意义非常直白：先旋转 source 对准方向，再平移质心重合。

2-6 算法流程总结

步骤	操作	对应公式
①	用当前 T T T 变换 source，在 target 中找每个点的最近邻	q i = KNN ( R p i + t ) q_i = \text{KNN}(R p_i + t) qi=KNN(Rpi+t)
②	计算匹配点对的质心	p ˉ , q ˉ p̄, q̄ pˉ,qˉ
③	去心坐标 → 构建交叉协方差矩阵	H = P ~ Q ~ T H = \tilde{P} \tilde{Q}^T H=P~Q~T
④	SVD 分解 H H H → 最优 R R R	R = V U T R = VU^T R=VUT（含 det 修正）
⑤	计算最优 t t t	t = q ˉ − R p ˉ t = q̄ - R p̄ t=qˉ−Rpˉ
⑥	更新变换矩阵，判断收敛	∣ Δ R M S E ∣ < ϵ |\Delta RMSE| < \epsilon ∣ΔRMSE∣<ϵ 则停止

---

3 C++ 实现

3-1 代码结构总览

• C++ 实现包含两大部分：
  1. my_icp() --- ICP 核心算法函数（独立于 ROS，可直接复用）
  2. ICPLocalizer 类 --- ROS 节点封装（订阅点云 → ICP 配准 → 发布位姿和点云）
• 核心优化点：
  • OpenMP 并行化最近邻搜索
  • 统计离群点剔除（median + 3σ）
  • 自适应搜索半径退火
  • 多分辨率 coarse→fine 两级配准

3-2 对应点搜索（最近邻）

• 回顾 2-6 节的算法流程，每一轮迭代的第一步是：用当前位姿 T T T 把 source 点云变换到 map 坐标系，然后在 target（全局地图）中为每个变换后的点找最近邻，形成"一一对应"的点对。这就是 ICP 的"匹配"环节。

• 说人话就是：当前位姿下，每个扫描点"看到"的是地图里的哪个点？我们粗暴地假设------离它最近的哪个地图点就是它的"真身"。

• 这个步骤的计算量是整个 ICP 里最大的：如果有 N N N 个 source 点，地图有 M M M 个点，暴力逐一比对是 O ( N M ) O(NM) O(NM)。工程上用 KDTree （k 维空间索引树）把 target 组织起来，每次最近邻查询降到 O ( log ⁡ M ) O(\log M) O(logM)，总复杂度 O ( N log ⁡ M ) O(N\log M) O(NlogM)。

• 对照代码：

// Step 1: 变换 source + 找对应点
// 公式: p_i' = R·p_i + t
Eigen::MatrixXf src_t = T.block<3,3>(0,0) * src_mat;
src_t.colwise() += T.block<3,1>(0,3);

// 在 target KDTree 中找最近邻
#pragma omp parallel
{
    #pragma omp for nowait
    for (size_t i = 0; i < N; i++) {
        pcl::PointXYZ pt(src_t(0,i), src_t(1,i), src_t(2,i));
        std::vector<int> idx(1);
        std::vector<float> d2(1);
        if (kdtree.nearestKSearch(pt, 1, idx, d2) > 0 && d2[0] < d2_max) {
            src_indices.push_back(i);      // source 中的点序号
            tgt_indices.push_back(idx[0]); // target 中的对应点序号
        }
    }
}

• 代码里做了两个工程处理：
  • 搜索半径裁剪 ：距离超过 d2_max 的点对直接丢弃------即使它是"最近的"，也远得不像是正确的对应。这相当于对匹配加上了一个 Gate："太远的不算"。
  • OpenMP 并行：每个线程维护自己的匹配列表，互不干扰，最后一次性合并，避免锁竞争。在 8 核机器上这一步能加速 5~6 倍。

3-3 统计离群点剔除

• 即使限定了搜索半径，仍可能有一些错误的对应点对（比如边缘点匹配到了远处的点）。在 SVD 求解之前，先做一次统计滤波。

// 统计离群点剔除：去掉距离 > median + 3×MAD 的对应点
std::vector<float> dists(K);
for (size_t k = 0; k < K; k++) {
    float dx = src_t(0,si) - target[ti].x;
    float dy = src_t(1,si) - target[ti].y;
    float dz = src_t(2,si) - target[ti].z;
    dists[k] = std::sqrt(dx*dx + dy*dy + dz*dz);
}

// 中位数
std::nth_element(dists.begin(), dists.begin() + K/2, dists.end());
float median = dists[K/2];

// MAD → σ 估计
std::vector<float> abs_dev(K);
for (size_t k = 0; k < K; k++) abs_dev[k] = std::abs(dists[k] - median);
std::nth_element(abs_dev.begin(), abs_dev.begin() + K/2, abs_dev.end());
float sigma = 1.4826f * abs_dev[K/2];  // MAD × 1.4826 ≈ σ（高斯假设下）

float thresh = median + 3.0f * sigma;
// 保留距离 <= thresh 的点对

• 为什么用 MAD 而不是直接算方差？因为均值方差容易被离群点"拉偏"------MAD 对异常值鲁棒。
• 说人话就是：

先找到所有匹配距离的中位数（"正常匹配大概多远"），再把那些距离离谱的点对扔掉，只保留"大多数"的匹配去算 SVD。

3-4 SVD 求解 R, t（核心）

• 对照我们在 2-2 ~ 2-5 节的推导，这里逐行翻译为 C++：

// ============================================================
// Step 2: SVD 求解最优 R, t  (Arun et al., 1987)
//
// 最小化:  min Σ || R·p_i + t - q_i ||²
//
// 质心:        p̄ = (1/K)Σ p_i,    q̄ = (1/K)Σ q_i
// 去心坐标:    p̃_i = p_i - p̄,     q̃_i = q_i - q̄
// 交叉协方差:  H = Σ p̃_i · q̃_i^T   = P̃ · Q̃^T    (3×3)
// SVD:         H = U · Σ · V^T
// 最优旋转:    R = V · U^T   (det修正: R = V·diag(1,1,-1)·U^T)
// 最优平移:    t = q̄ - R · p̄
// ============================================================

// 提取匹配点对 P(source原始点), Q(target对应点)
Eigen::MatrixXf P(3, K), Q(3, K);
for (size_t k = 0; k < K; k++) {
    size_t si = src_idx[k], ti = tgt_idx[k];
    P(0,k) = source[si].x; P(1,k) = source[si].y; P(2,k) = source[si].z;
    Q(0,k) = target[ti].x; Q(1,k) = target[ti].y; Q(2,k) = target[ti].z;
}

// 计算质心（公式 2-2）
Eigen::Vector3f p_mean = P.rowwise().mean();  // p̄
Eigen::Vector3f q_mean = Q.rowwise().mean();  // q̄

// 去心坐标（公式 2-2）
Eigen::MatrixXf Pc = P.colwise() - p_mean;    // P̃  (3×K)
Eigen::MatrixXf Qc = Q.colwise() - q_mean;    // Q̃  (3×K)

// 交叉协方差矩阵 H = P̃ · Q̃^T（公式 2-3）
Eigen::Matrix3f H = Pc * Qc.transpose();

// SVD: H = U·Σ·V^T（公式 2-4）
Eigen::JacobiSVD<Eigen::Matrix3f> svd(H,
    Eigen::ComputeFullU | Eigen::ComputeFullV);

// 最优旋转 R = V · U^T（公式 2-4）
Eigen::Matrix3f R_opt = svd.matrixV() * svd.matrixU().transpose();

// det(R) = +1 修正（公式 2-4）
if (R_opt.determinant() < 0) {
    Eigen::Matrix3f V = svd.matrixV();
    V.col(2) *= -1;   // diag(1, 1, -1)
    R_opt = V * svd.matrixU().transpose();
}

// 最优平移 t = q̄ - R·p̄（公式 2-5）
Eigen::Vector3f t_opt = q_mean - R_opt * p_mean;

// 更新变换矩阵
T.block<3,3>(0,0) = R_opt;
T.block<3,1>(0,3) = t_opt;

• 注意一个工程细节：这里直接用 SVD 结果替代 T，而不是叠加增量。为什么？因为每次迭代都基于原始的 source 坐标（不是上一次变换后的坐标），SVD 给出的就是当前对应关系下的全局最优旋转------这叫"直接用绝对位姿"，比"每次都算一个 ΔT 叠加"更稳定，误差不会累积。

3-5 收敛判断与自适应退火

• 读到这你可能会问：SVD 不是已经求出最优 R , t R, t R,t 了吗，为什么还要迭代？

• 关键点在于：SVD 求出的"最优"，是针对当前这组对应关系 的。但当我们把新的 R , t R, t R,t 应用到 source 上之后，变换后的点位置变了，它们在 target 中的最近邻也跟着变了------对应关系被刷新了。新对应关系下，刚才那个 R , t R, t R,t 就不再是最优的了。

• 这就好像一个"鸡生蛋、蛋生鸡"的循环：

  对应关系 → SVD 求位姿 → 位姿变了 → 对应关系也变了 → 需要重新 SVD → ...

• 每轮迭代，对应关系和位姿交替优化、互相靠近，直到两者都稳定下来------这就是 ICP 的 EM（Expectation-Maximization）本质：E 步 找对应（固定位姿），M 步求位姿（固定对应），反复交替。

• 既要迭代，就需要回答两个问题：什么时候停？ 和 每轮搜索半径要不要变？

• 收敛判断： 比较前后两次 RMSE（均方根误差）的变化。在定位场景下，这有一个非常实际的用途------机器人停着不动的时候，位姿几乎不变，再迭代下去只是白白烧 CPU。当 RMSE 变化小于阈值时直接 break，把算力省下来给别的节点。如果机器人正在运动，RMSE 会持续下降，那就会跑到最大迭代次数。

// Step 4: 收敛判断
Eigen::MatrixXf Pa = (R_opt * P).colwise() + t_opt;  // 配准后的 source 点
float rmse = std::sqrt((Pa - Q).colwise().squaredNorm().mean());
float delta = std::abs(prev_rmse - rmse);
prev_rmse = rmse;

if (delta < tol && it > 3) break;  // RMSE 不再显著下降，收敛

• 自适应搜索半径退火： 搜索半径不是固定的，而是逐步缩小：

// 退火：搜索半径逐步缩小
cur_max_dist = std::max(cur_max_dist * 0.85f, max_dist_0 * 0.33f);

• 设计思想：迭代初期（初值不太准时）用较大的搜索半径，允许更多点对上；迭代后期（位姿已经较准确）缩小半径，只保留高质量的对应点。这就好像模拟退火------"温度"逐轮降低。
• 注意下限是初始半径的 1/3，不会无限缩小导致无匹配。

3-6 多分辨率 Coarse-to-Fine

• 单层 ICP 有个问题：点太多 → 算得慢；体素降采样太大 → 丢了细节。多分辨率策略用两级配准解决这个矛盾：

// ====== 多分辨率 ICP ======
// Pass 1: 粗配准 (大 voxel=0.2m, 大搜索半径, 30 次迭代)
auto [Tc, fc, rc, itc, tc] = my_icp(scan_coarse, map_, init, max_dist_, 30, 1e-5f);

// Pass 2: 精配准 (小 voxel=0.1m, 小搜索半径, 20 次迭代)
auto [Tf, ff, rf, itf, tf] = my_icp(scan_fine, map_, Tc, max_dist_ * 0.5f, 20, 1e-6f);

• 说人话就是：

先用比较稀疏的点云（大格子）快速把位姿拉到靠谱的范围，再用稠密的点云（小格子）做精细修正。这样既快又准。

3-7 完整代码

• 以下是完整的 C++ ICP 函数实现（ROS 节点部分见文末）：

/**
 * ====================================
 * 优化:
 *   1. OpenMP 并行对应点搜索  (速度)
 *   2. 统计离群点剔除 (精度)
 *   3. 多分辨率 coarse→fine (速度+精度)
 *   4. 自适应搜索半径退火
 *
 *
 * 订阅: /scan2 (PointCloud2)
 * 发布: /icp_pose, /icp_aligned_cloud, /map_cloud (latch)
 * TF:   map -> laser_livox_icp
 */

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <omp.h>
#include <algorithm>
#include <cmath>

// ============================================================
// Point-to-Point ICP (SVD, Arun 1987) ------ 优化版
// ============================================================
/**
 * 优化版 point-to-point ICP
 *
 * 算法流程:
 *   1. 变换 source (用当前 T)，在 target 找最近邻 → 对应点对
 *   2. 统计离群点剔除: 距离 > median + 3σ 的丢弃
 *   3. SVD 求解最优 R, t (直接替换 T，不叠加)
 *   4. 自适应搜索半径退火
 *   5. 收敛判断
 *
 * @return {T, fitness, rmse, iterations, time_ms}
 */
std::tuple<Eigen::Matrix4f, float, float, int, double>
my_icp(const pcl::PointCloud<pcl::PointXYZ>& source,
       const pcl::PointCloud<pcl::PointXYZ>& target,
       const Eigen::Matrix4f& init_guess,
       float max_dist_0 = 1.5f, int max_iter = 50, float tol = 1e-6f)
{
    ros::WallTime t0 = ros::WallTime::now();

    // ---- 给 target 建 KDTree ----
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(target.makeShared());

    Eigen::Matrix4f T = init_guess;
    float cur_max_dist = max_dist_0;
    float prev_rmse = std::numeric_limits<float>::max();
    size_t last_K = 0;

    // source 存为矩阵，避免每次迭代重复复制
    size_t N = source.size();
    Eigen::MatrixXf src_mat(3, N);
    for (size_t i = 0; i < N; i++) {
        src_mat(0,i)=source[i].x; src_mat(1,i)=source[i].y; src_mat(2,i)=source[i].z;
    }

    int n_threads = omp_get_max_threads();

    int it;
    for (it = 0; it < max_iter; it++)
    {
        // ========================================================
        // Step 1: 变换 source + 找对应点
        // 公式: p_i' = R·p_i + t
        // ========================================================
        Eigen::MatrixXf src_t = T.block<3,3>(0,0) * src_mat;
        src_t.colwise() += T.block<3,1>(0,3);

        // 退火：搜索半径逐步缩小
        cur_max_dist = std::max(cur_max_dist * 0.85f, max_dist_0 * 0.33f);
        float d2_max = cur_max_dist * cur_max_dist;

        // ---- OpenMP 并行最近邻搜索 ----
        std::vector<std::vector<int>> thread_src(n_threads), thread_tgt(n_threads);

        #pragma omp parallel
        {
            int tid = omp_get_thread_num();
            auto& ls = thread_src[tid]; ls.reserve(N / n_threads + 64);
            auto& lt = thread_tgt[tid]; lt.reserve(N / n_threads + 64);

            #pragma omp for nowait
            for (size_t i = 0; i < N; i++) {
                pcl::PointXYZ pt(src_t(0,i), src_t(1,i), src_t(2,i));
                std::vector<int> idx(1);
                std::vector<float> d2(1);
                if (kdtree.nearestKSearch(pt, 1, idx, d2) > 0 && d2[0] < d2_max) {
                    ls.push_back(i);
                    lt.push_back(idx[0]);
                }
            }
        }

        // 合并各线程结果
        std::vector<int> src_idx, tgt_idx;
        for (int t = 0; t < n_threads; t++) {
            src_idx.insert(src_idx.end(), thread_src[t].begin(), thread_src[t].end());
            tgt_idx.insert(tgt_idx.end(), thread_tgt[t].begin(), thread_tgt[t].end());
        }

        size_t K = src_idx.size();
        if (K < 10) {
            if (K < 3) break;         // 太少，放弃
            cur_max_dist *= 1.5f;     // 放宽半径重试
            continue;
        }

        // ---- 统计离群点剔除 ----
        // 去掉距离 > median + 3×MAD 的对应点
        if (K > 50) {
            std::vector<float> dists(K);
            for (size_t k = 0; k < K; k++) {
                size_t si = src_idx[k], ti = tgt_idx[k];
                float dx = src_t(0,si)-target[ti].x;
                float dy = src_t(1,si)-target[ti].y;
                float dz = src_t(2,si)-target[ti].z;
                dists[k] = std::sqrt(dx*dx + dy*dy + dz*dz);
            }

            // 中位数
            std::nth_element(dists.begin(), dists.begin()+K/2, dists.end());
            float median = dists[K/2];

            // MAD → σ 估计
            std::vector<float> abs_dev(K);
            for (size_t k=0;k<K;k++) abs_dev[k]=std::abs(dists[k]-median);
            std::nth_element(abs_dev.begin(), abs_dev.begin()+K/2, abs_dev.end());
            float sigma = 1.4826f * abs_dev[K/2];

            float thresh = median + 3.0f * sigma;
            std::vector<int> fs, ft;
            fs.reserve(K); ft.reserve(K);
            for (size_t k=0;k<K;k++) {
                if (dists[k] <= thresh) {
                    fs.push_back(src_idx[k]);
                    ft.push_back(tgt_idx[k]);
                }
            }
            if (fs.size() > 10) { src_idx.swap(fs); tgt_idx.swap(ft); K = src_idx.size(); }
        }

        // ========================================================
        // Step 2: SVD 求解最优 R, t  (Arun et al., 1987)
        //
        // 最小化:  min Σ || R·p_i + t - q_i ||²
        //
        // 质心:        p̄ = (1/K)Σ p_i,    q̄ = (1/K)Σ q_i
        // 去心坐标:    p̃_i = p_i - p̄,     q̃_i = q_i - q̄
        // 交叉协方差:  H = Σ p̃_i · q̃_i^T      (3×3)
        // SVD:         H = U · Σ · V^T
        // 最优旋转:    R = V · U^T   (det修正: R = V·diag(1,1,-1)·U^T)
        // 最优平移:    t = q̄ - R · p̄
        // ========================================================

        // 提取匹配点对 P(原始source), Q(target)
        Eigen::MatrixXf P(3, K), Q(3, K);
        for (size_t k=0;k<K;k++) {
            size_t si=src_idx[k], ti=tgt_idx[k];
            P(0,k)=source[si].x; P(1,k)=source[si].y; P(2,k)=source[si].z;
            Q(0,k)=target[ti].x; Q(1,k)=target[ti].y; Q(2,k)=target[ti].z;
        }

        Eigen::Vector3f p_mean = P.rowwise().mean();  // p̄
        Eigen::Vector3f q_mean = Q.rowwise().mean();  // q̄

        Eigen::MatrixXf Pc = P.colwise() - p_mean;    // P̃  (3×K)
        Eigen::MatrixXf Qc = Q.colwise() - q_mean;    // Q̃  (3×K)

        // H = P̃ · Q̃^T
        Eigen::Matrix3f H = Pc * Qc.transpose();

        // SVD: H = U·Σ·V^T
        Eigen::JacobiSVD<Eigen::Matrix3f> svd(H,
            Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix3f R_opt = svd.matrixV() * svd.matrixU().transpose();

        // det(R) = +1 修正
        if (R_opt.determinant() < 0) {
            Eigen::Matrix3f V = svd.matrixV(); V.col(2) *= -1;
            R_opt = V * svd.matrixU().transpose();
        }

        // t = q̄ - R·p̄
        Eigen::Vector3f t_opt = q_mean - R_opt * p_mean;

        // Step 3: 更新 T（直接替换为完整变换）
        T.block<3,3>(0,0) = R_opt;
        T.block<3,1>(0,3) = t_opt;

        // Step 4: 收敛判断
        Eigen::MatrixXf Pa = (R_opt * P).colwise() + t_opt;
        float rmse = std::sqrt((Pa - Q).colwise().squaredNorm().mean());
        float delta = std::abs(prev_rmse - rmse);
        prev_rmse = rmse;
        last_K = K;

        if (delta < tol && it > 3) { it++; break; }
    }

    double ms = (ros::WallTime::now() - t0).toSec() * 1000.0;
    float fitness = (N > 0) ? float(last_K) / N : 0.0f;
    return {T, fitness, prev_rmse, it, ms};
}


// ============================================================
// ROS 节点
// ============================================================
class ICPLocalizer
{
public:
    ICPLocalizer(ros::NodeHandle& nh, const std::string& map_path)
      : nh_(nh)
    {
        float voxel_map, voxel_scan_coarse, voxel_scan_fine, max_dist;
        nh_.param<float>("voxel_map", voxel_map, 0.1f);
        nh_.param<float>("voxel_scan_coarse", voxel_scan_coarse_, 0.2f);
        nh_.param<float>("voxel_scan_fine", voxel_scan_fine_, 0.1f);
        nh_.param<float>("max_dist", max_dist_, 1.5f);

        ROS_INFO("=== ICPLocalizer (optimized pt2pt) ===");
        ROS_INFO("  map: %s", map_path.c_str());
        ROS_INFO("  voxel_map=%.2f  coarse=%.2f  fine=%.2f  max_dist=%.2f",
                 voxel_map, voxel_scan_coarse_, voxel_scan_fine_, max_dist_);
        ROS_INFO("  OpenMP threads: %d", omp_get_max_threads());

        // ---- 加载地图 ----
        pcl::PointCloud<pcl::PointXYZ> map_full;
        if (pcl::io::loadPCDFile(map_path, map_full) == -1) {
            ROS_ERROR("Failed: %s", map_path.c_str());
            ros::shutdown(); return;
        }
        ROS_INFO("  raw: %lu pts", map_full.size());

        pcl::CropBox<pcl::PointXYZ> crop;
        pcl::PointCloud<pcl::PointXYZ> map_cropped;
        crop.setInputCloud(map_full.makeShared());
        crop.setMin(Eigen::Vector4f(-50,-50,-5,1));
        crop.setMax(Eigen::Vector4f( 50, 50,10,1));
        crop.filter(map_cropped);

        pcl::VoxelGrid<pcl::PointXYZ> vg;
        vg.setInputCloud(map_cropped.makeShared());
        vg.setLeafSize(voxel_map, voxel_map, voxel_map);
        vg.filter(map_);
        ROS_INFO("  map after crop+voxel: %lu pts", map_.size());

        // TF
        tf_buffer_ = std::make_unique<tf2_ros::Buffer>();
        tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);

        // Pubs/subs
        sub_scan_ = nh.subscribe("/scan2", 1, &ICPLocalizer::callback, this);
        pub_pose_  = nh.advertise<geometry_msgs::PoseStamped>("/icp_pose", 1);
        pub_cloud_ = nh.advertise<sensor_msgs::PointCloud2>("/icp_aligned_cloud", 1);
        pub_map_   = nh.advertise<sensor_msgs::PointCloud2>("/map_cloud", 1, true);
        publishMap();

        ROS_INFO("Ready. Waiting for /scan2 ...");
    }

private:
    void publishMap() {
        sensor_msgs::PointCloud2 msg;
        pcl::toROSMsg(map_, msg);
        msg.header.frame_id="map"; msg.header.stamp=ros::Time::now();
        pub_map_.publish(msg);
    }

    Eigen::Matrix4f getInitialGuess(const std::string& fid, const ros::Time& stamp) {
        try {
            auto t = tf_buffer_->lookupTransform("map", fid, stamp, ros::Duration(0.1));
            tf2::Quaternion q(t.transform.rotation.x,t.transform.rotation.y,
                              t.transform.rotation.z,t.transform.rotation.w);
            tf2::Matrix3x3 R(q);
            Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
            for(int r=0;r<3;r++) for(int c=0;c<3;c++) T(r,c)=R[r][c];
            T(0,3)=t.transform.translation.x;
            T(1,3)=t.transform.translation.y;
            T(2,3)=t.transform.translation.z;
            return T;
        } catch (tf2::TransformException& e) {
            ROS_WARN_STREAM_THROTTLE(5,"TF fail: "<<e.what());
            return prev_T_;
        }
    }

    void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
    {
        pcl::PointCloud<pcl::PointXYZ> scan_raw;
        pcl::fromROSMsg(*msg, scan_raw);
        if (scan_raw.size() < 100) return;

        // Crop ±30m
        pcl::PointCloud<pcl::PointXYZ> scan_cropped;
        pcl::CropBox<pcl::PointXYZ> crop;
        crop.setInputCloud(scan_raw.makeShared());
        crop.setMin(Eigen::Vector4f(-30,-30,-5,1));
        crop.setMax(Eigen::Vector4f( 30, 30,10,1));
        crop.filter(scan_cropped);

        Eigen::Matrix4f init = getInitialGuess(msg->header.frame_id, msg->header.stamp);

        // ====== 多分辨率 ICP ======
        // Pass 1: 粗配准 (大 voxel, 大搜索半径, 少迭代)
        pcl::PointCloud<pcl::PointXYZ> scan_c;
        {
            pcl::VoxelGrid<pcl::PointXYZ> vg;
            vg.setInputCloud(scan_cropped.makeShared());
            vg.setLeafSize(voxel_scan_coarse_, voxel_scan_coarse_, voxel_scan_coarse_);
            vg.filter(scan_c);
        }
        auto [Tc, fc, rc, itc, tc] = my_icp(scan_c, map_, init, max_dist_, 30, 1e-5f);

        // Pass 2: 精配准 (小 voxel, 小搜索半径, 收敛阈值更低)
        pcl::PointCloud<pcl::PointXYZ> scan_f;
        {
            pcl::VoxelGrid<pcl::PointXYZ> vg;
            vg.setInputCloud(scan_cropped.makeShared());
            vg.setLeafSize(voxel_scan_fine_, voxel_scan_fine_, voxel_scan_fine_);
            vg.filter(scan_f);
        }
        auto [Tf, ff, rf, itf, tf] = my_icp(scan_f, map_, Tc, max_dist_ * 0.5f, 20, 1e-6f);

        prev_T_ = Tf;

        ROS_INFO_STREAM_THROTTLE(2,
            "ICP | coarse " << itc << "it " << int(tc) << "ms f=" << fc
            << " | fine " << itf << "it " << int(tf) << "ms f=" << ff
            << " rmse=" << rf);

        // 发布
        pcl::PointCloud<pcl::PointXYZ> aligned;
        pcl::transformPointCloud(scan_f, aligned, Tf);
        sensor_msgs::PointCloud2 cloud_msg;
        pcl::toROSMsg(aligned, cloud_msg);
        cloud_msg.header.frame_id="map"; cloud_msg.header.stamp=msg->header.stamp;
        pub_cloud_.publish(cloud_msg);

        publishPoseAndTF(Tf, msg->header.stamp);
    }

    void publishPoseAndTF(const Eigen::Matrix4f& T, const ros::Time& stamp) {
        Eigen::Quaternionf q(T.block<3,3>(0,0));

        geometry_msgs::PoseStamped pose;
        pose.header.frame_id="map"; pose.header.stamp=stamp;
        pose.pose.position.x=T(0,3);pose.pose.position.y=T(1,3);pose.pose.position.z=T(2,3);
        pose.pose.orientation.x=q.x();pose.pose.orientation.y=q.y();
        pose.pose.orientation.z=q.z();pose.pose.orientation.w=q.w();
        pub_pose_.publish(pose);

        geometry_msgs::TransformStamped tf;
        tf.header.frame_id="map"; tf.header.stamp=stamp;
        tf.child_frame_id="laser_livox_icp";
        tf.transform.translation.x=T(0,3);tf.transform.translation.y=T(1,3);
        tf.transform.translation.z=T(2,3);
        tf.transform.rotation.x=q.x();tf.transform.rotation.y=q.y();
        tf.transform.rotation.z=q.z();tf.transform.rotation.w=q.w();
        tf_broadcaster_.sendTransform(tf);
    }

    ros::NodeHandle nh_;
    pcl::PointCloud<pcl::PointXYZ> map_;
    float voxel_scan_coarse_, voxel_scan_fine_, max_dist_;

    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
    tf2_ros::TransformBroadcaster tf_broadcaster_;

    ros::Subscriber sub_scan_;
    ros::Publisher pub_pose_, pub_cloud_, pub_map_;
    Eigen::Matrix4f prev_T_ = Eigen::Matrix4f::Identity();
};


int main(int argc, char** argv) {
    ros::init(argc, argv, "my_icp_localizer");
    std::string map_path = (argc > 1) ? argv[1] : "merged.pcd";
    ros::NodeHandle nh("~");
    ICPLocalizer localizer(nh, map_path);
    ros::spin();
    return 0;
}

---

附录 A：点云累积保存（Python）

• 以下脚本用于订阅 /scan2 点云话题，通过 TF 变换到 map 坐标系后累积所有帧，退出时保存为 merged.pcd------作为 ICP 的 target 地图。

#!/usr/bin/env python3
"""
订阅 /scan2 (PointCloud2)，通过 TF 变换到固定坐标系后累积，
实时发布 /accumulated_cloud 供 RViz 查看，退出时保存 merged.pcd
"""

import rospy
import sensor_msgs.point_cloud2 as pc2
import numpy as np
import open3d as o3d
import tf2_ros
import tf2_sensor_msgs
from sensor_msgs.msg import PointCloud2
from std_msgs.msg import Header


class ScanAccumulator:
    def __init__(self, fixed_frame="map", pub_rate=2.0):
        self.fixed_frame = fixed_frame
        self.accumulated = o3d.geometry.PointCloud()

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        self.sub = rospy.Subscriber("/scan2", PointCloud2, self.callback)
        self.pub = rospy.Publisher("/accumulated_cloud", PointCloud2, queue_size=1)
        self.timer = rospy.Timer(rospy.Duration(1.0 / pub_rate), self.publish_cloud)
        rospy.on_shutdown(self.on_shutdown)
        rospy.loginfo(f"ScanAccumulator: /scan2 -> {fixed_frame} -> /accumulated_cloud ({pub_rate:.1f} Hz)")

    def callback(self, msg):
        try:
            transform = self.tf_buffer.lookup_transform(
                self.fixed_frame,
                msg.header.frame_id,
                msg.header.stamp,
                rospy.Duration(0.1),
            )
            msg = tf2_sensor_msgs.do_transform_cloud(msg, transform)
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException) as e:
            rospy.logwarn_throttle(10, f"TF skip: {e}")
            return

        pts = np.array(list(pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)))
        if len(pts) == 0:
            return

        cloud = o3d.geometry.PointCloud()
        cloud.points = o3d.utility.Vector3dVector(pts)

        self.accumulated += cloud
        rospy.loginfo_throttle(5, f"Accumulated {len(self.accumulated.points)} points total")

    def publish_cloud(self, event):
        if len(self.accumulated.points) == 0:
            return
        pts = np.asarray(self.accumulated.points, dtype=np.float32)
        header = Header(stamp=rospy.Time.now(), frame_id=self.fixed_frame)
        cloud2 = pc2.create_cloud_xyz32(header, pts)
        self.pub.publish(cloud2)

    def on_shutdown(self):
        if len(self.accumulated.points) == 0:
            rospy.logwarn("No points, nothing saved")
            return
        path = "merged.pcd"
        o3d.io.write_point_cloud(path, self.accumulated)
        rospy.loginfo(f"Saved {len(self.accumulated.points)} points → {path}")


if __name__ == "__main__":
    rospy.init_node("scan_accumulator")
    acc = ScanAccumulator(fixed_frame="map")
    rospy.spin()

---

总结

• 本文从移动机器人实时定位的需求出发，完整走了一遍基于 ICP 的点云定位方案------用先验地图 + 当前扫描 → 估计机器人精确位姿。从 SVD 闭式求解的数学推导，到手写 C++ 实现，再到离群点剔除、多分辨率加速、自适应退火等工程优化。

1. ICP 定位的本质：把定位问题转化为点云配准问题------"当前扫描和地图最吻合的那个位姿，就是机器人的真实位姿"，核心是迭代地"找最近对应 → SVD 求解 R,t → 更新位姿"，直到收敛。
2. SVD 求解是核心 ：利用质心去耦合平移，交叉协方差 H H H 的 SVD 一步给出最优旋转 R R R，平移 t = q ˉ − R p ˉ t = q̄ - R p̄ t=qˉ−Rpˉ 一步到位，无需梯度下降。
3. 工程优化保证实时性：OpenMP 并行搜索 + 统计离群点剔除（median + MAD）+ 多分辨率 coarse-to-fine + 搜索半径退火，让定位既快又稳。
4. 完整可部署：给出了从点云累积建图（Python）到 ICP 实时定位节点（C++）的完整代码，发布位姿和 TF，可直接接入导航栈使用。

• 如有错误，欢迎指出！
• 感谢观看！