前言

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

---

0 前置数学知识

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

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

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

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

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

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

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

$$R_z(45^{\circ })=\left[\begin{array}{ccc}\cos 45^{\circ }& -\sin 45^{\circ }& 0\\ \sin 45^{\circ }& \cos 45^{\circ }& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}\frac{\sqrt{2}}{2}& -\frac{\sqrt{2}}{2}& 0\\ \frac{\sqrt{2}}{2}& \frac{\sqrt{2}}{2}& 0\\ 0& 0& 1\end{array}\right]\approx \left[\begin{array}{ccc}0.707& -0.707& 0\\ 0.707& 0.707& 0\\ 0& 0& 1\end{array}\right]$$

• 验证两个条件：

  • 正交性：$R^{T}R=\left[\begin{array}{ccc}0.707& 0.707& 0\\ -0.707& 0.707& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}0.707& -0.707& 0\\ 0.707& 0.707& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]=I$ ✓
  • 行列式：$\det (R)=(0.707^2+0.707^2)\times 1=1$ ✓（用前两列叉积点乘第三列也可验证）

• 拿一个向量试试效果。设 $v=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$（沿 x 轴的单位向量），旋转后：
  $$R\cdot v=\left[\begin{array}{c}0.707\\ 0.707\\ 0\end{array}\right]$$
  向量长度：$\sqrt{0.707^2+0.707^2+0^2}=1$，和旋转前一样。旋转不改变长度，这就是正交性的几何含义。

• 再看一个反例——镜面反射矩阵（沿 $x$ 轴翻转）：
  $$M=\left[\begin{array}{ccc}-1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$
  它也满足 $M^{T}M=I$（正交），但 $\det (M)=-1$。如果把它当旋转用，会把右手坐标系变成左手坐标系——这不是真实的物理旋转，而是翻转了整个空间。这就是为什么 SVD 求出 $R=V{U}^T$ 后必须检查行列式并修正：$V{U}^T$ 保证正交，但不保证是旋转还是反射。

0-2 SVD 奇异值分解

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

$$A=U\cdot \Sigma \cdot V^T$$

其中：

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

• $\Sigma$（$m\times n$）：奇异值对角矩阵，对角元素 ${\sigma }_i\ge 0$ 从大到小排列，每个 ${\sigma }_i$ 表示矩阵沿对应主轴方向的"伸缩强度"。

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

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

$$A\cdot x=U\cdot (\Sigma \cdot (V^T\cdot x))$$

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

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

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

$$A=\left[\begin{array}{ccc}3& 2& 2\\ 2& 3& -2\\ 0& 0& 1\end{array}\right]$$

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

$$U=\left[\begin{array}{ccc}\frac{1}{\sqrt{2}}& \frac{1}{\sqrt{2}}& 0\\ \frac{1}{\sqrt{2}}& -\frac{1}{\sqrt{2}}& 0\\ 0& 0& 1\end{array}\right],\Sigma =\left[\begin{array}{ccc}5& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],V^T=\left[\begin{array}{ccc}\frac{1}{\sqrt{2}}& \frac{1}{\sqrt{2}}& 0\\ \frac{1}{\sqrt{2}}& -\frac{1}{\sqrt{2}}& 0\\ 0& 0& 1\end{array}\right]$$

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

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

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

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

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

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

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

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

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

0-3 矩阵的迹（Trace）

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

$$\text{tr}(AB)=\text{tr}(BA)$$

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

$$\text{tr}(ABC)=\text{tr}(BCA)=\text{tr}(CAB)$$

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

$$\sum _i{a}_i^{T}R{b}_i=\text{tr}\left(R\sum _i{b}_i{a}_i^T\right)$$

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

0-4 正交 Procrustes 问题

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

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

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

• 说人话就是：

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

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

$$p_1=\left[\begin{array}{c}1\\ 0\end{array}\right],p_2=\left[\begin{array}{c}0\\ 1\end{array}\right]q_1=\left[\begin{array}{c}0\\ 1\end{array}\right],q_2=\left[\begin{array}{c}-1\\ 0\end{array}\right]$$

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

$$R_{90^{\circ }}=\left[\begin{array}{cc}\cos 90^{\circ }& -\sin 90^{\circ }\\ \sin 90^{\circ }& \cos 90^{\circ }\end{array}\right]=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$$

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

$$M=\sum _{i=1}^2{p}_i{q}_i^T=p_1{q}_1^T+p_2{q}_2^T$$

$$=\left[\begin{array}{c}1\\ 0\end{array}\right]\left[\begin{array}{cc}0& 1\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]\left[\begin{array}{cc}-1& 0\end{array}\right]$$

$$=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]+\left[\begin{array}{cc}0& 0\\ -1& 0\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]$$

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

$$U=M=\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right],\Sigma =\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],V=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$

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

$$\Omega =V{U}^T=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$$

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

$$\Omega p_1=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\left[\begin{array}{c}1\\ 0\end{array}\right]=\left[\begin{array}{c}0\\ 1\end{array}\right]=q_1✓$$

$$\Omega p_2=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]=\left[\begin{array}{c}-1\\ 0\end{array}\right]=q_2✓$$

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

• 该问题的闭式解由 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$，把当前扫描变换到 map 坐标系后，和地图尽可能重合。而这个 $T$，就是我们要求解的机器人位姿。
• ICP（Iterative Closest Point）由 Besl & McKay 在 1992 年提出，是解决这类配准问题最经典的算法。用在定位场景中：
  • 输入：source（当前扫描）、target（先验地图）、init_guess（TF/里程计给的粗略位姿）
  • 输出：精修的位姿 $T$（即机器人在 map 下的精确位置和姿态）
• 说人话就是：

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

1-2 核心思想

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

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

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

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

---

2 数学推导

2-1 问题建模

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

$$\underset{R,t}{\min }\sum _{i=1}^K\Vert R\cdot p_i+t-q_i{\Vert }^2$$

其中：

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

• $q_i$：target 点云中 $p_i$ 的最近邻对应点

• $R\in SO(3)$：旋转矩阵（$3\times 3$，满足 $R^{T}R=I$ 且 $\det (R)=1$）

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

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

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

2-2 质心与去心坐标

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

$$\bar{p}=\frac{1}{K}\sum _{i=1}^K{p}_i,\bar{q}=\frac{1}{K}\sum _{i=1}^K{q}_i$$

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

$${\tilde{p}}_i=p_i-\bar{p},{\tilde{q}}_i=q_i-\bar{q}$$

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

$$\underset{R}{\min }\sum _{i=1}^K\Vert R\cdot {\tilde{p}}_i-{\tilde{q}}_i{\Vert }^2$$

• 说人话就是：

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

2-3 交叉协方差矩阵

• 展开上述目标函数：

$$\sum _{i=1}^K\Vert R{\tilde{p}}_i-{\tilde{q}}_i{\Vert }^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)$$

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

$$\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)$$

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

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

$$H=\sum _{i=1}^K{\tilde{p}}_i{\tilde{q}}_i^T$$

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

$$H=\tilde{P}\cdot {\tilde{Q}}^T$$

其中 $\tilde{P}=[{\tilde{p}}_1,...,{\tilde{p}}_K]$，$\tilde{Q}=[{\tilde{q}}_1,...,{\tilde{q}}_K]$。

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

2-4 SVD 求解最优旋转

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

$$H=U\cdot \Sigma \cdot V^T$$

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

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

$$R=V\cdot U^T$$

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

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

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

$$\text{if }\det (V{U}^T)<0:R=V\cdot \text{diag}(1,1,-1)\cdot U^T$$

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

2-5 平移求解

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

$$t=\bar{q}-R\cdot \bar{p}$$

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

2-6 算法流程总结

步骤	操作	对应公式
①	用当前 $T$ 变换 source，在 target 中找每个点的最近邻	$q_i=\text{KNN}(R{p}_i+t)$
②	计算匹配点对的质心	$\bar{p},\bar{q}$
③	去心坐标 → 构建交叉协方差矩阵	$H=\tilde{P}{\tilde{Q}}^T$
④	SVD 分解 $H$ → 最优 $R$	$R=V{U}^T$（含 det 修正）
⑤	计算最优 $t$	$t=\bar{q}-R\bar{p}$
⑥	更新变换矩阵，判断收敛	$|\Delta 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$ 把 source 点云变换到 map 坐标系，然后在 target（全局地图）中为每个变换后的点找最近邻，形成"一一对应"的点对。这就是 ICP 的"匹配"环节。

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

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

• 对照代码：

// 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$ 了吗，为什么还要迭代？

• 关键点在于：SVD 求出的"最优"，是针对当前这组对应关系的。但当我们把新的 $R,t$ 应用到 source 上之后，变换后的点位置变了，它们在 target 中的最近邻也跟着变了——对应关系被刷新了。新对应关系下，刚才那个 $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$ 的 SVD 一步给出最优旋转 $R$，平移 $t=\bar{q}-R\bar{p}$ 一步到位，无需梯度下降。
3. 工程优化保证实时性：OpenMP 并行搜索 + 统计离群点剔除（median + MAD）+ 多分辨率 coarse-to-fine + 搜索半径退火，让定位既快又稳。
4. 完整可部署：给出了从点云累积建图（Python）到 ICP 实时定位节点（C++）的完整代码，发布位姿和 TF，可直接接入导航栈使用。

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