前言

　　在SLAM中，除了表达3D旋转与位移之外，我们还要对它们进行估计，因为SLAM整个过程就是在不断地估计机器人的位姿与地图。为了做这件事，需要对变换矩阵进行插值、求导、迭代等操作。例如，在经典ICP问题中，给定了两组3D点，我们要计算它们之间的变换矩阵。假设第一组的3D点为$P=\{p_i|i=[1,2,\dots ,N\left]\right\}$

，第二组3D点为$Q=\{q_i|i=[1,2,\dots ,N\left]\right\}$，那我们实际要做的事情是求一个欧氏变换$T$，使得$T$

满足：

$$\begin{array}{}\forall i,q_i=T{p}_i& \text{(1)}\end{array}$$

　　注意这里使用了齐次坐标表示。通常，这许多个匹配过的点是通过特征匹配得到的，构成了一个超定方程。而由于噪声的存在，这个方程往往是无解的。因此我们转而计算一个最小二乘：
$$\begin{array}{}\underset{T}{min}u\left(T\right)=\sum _{i=1}^N{\parallel q_i-T{p}_i\parallel }^2& \text{(2)}\end{array}$$

　　这时问题就来了：如果用迭代方式求解这个优化时（尽管可以不用迭代方式来求），如何求目标函数$u$

相对于$T$的导数呢？首先，$T$只有6 个自由度，最好能够在一个六维空间表达它，那么$u\left(T\right)$相对于这个六维空间的导数（雅可比矩阵）是一个$6\times 6$的矩阵。其次，$T$

对于乘法是封闭的，但对加法不封闭，即任意两个变换矩阵相加后并不是一个变换矩阵，这主要是因为旋转矩阵对加法是不封闭的。

　　出于这两个原因，我们希望有更好的数学工具帮助我们做这些事，而李群与李代数理论正好提供了这样的工具。李群与李代数广泛地用于机器人与计算机视觉领域，并在机器人动力学推导上占据重要地位。不过，由于SLAM不涉及过多的动力学推导。我们重点介绍它在SLAM中相关的几个重要的结果，而略去许多数学性质的证明。特别地，重点介绍$SO\left(3\right)$

和$SE\left(3\right)$

这两个李群与对应的李代数。

---

 李代数基础

　　首先，我们来讨论较为简单的三维旋转群。为了说明它的结构，首先介绍群的概念。

群

　　群（Group）是一种集合加上一种运算的代数结构，记作$(A,\cdot )$

。其中$A$代表集合，$\cdot$是定义在该集合上的二元运算。那么，如果这个运算满足以下几个条件，则称$G=(A,\cdot )$

为群。

• 封闭性: $\forall a_1,a_2,a_1\cdot a_2\in A$

• 
  

　　读者可以记作“封结幺逆”（谐音凤姐咬你），并可以把一些常见的群放进去验证。例如整数的加法（幺元为0），去掉0后的有理数的乘法（幺元为1）。对于矩阵，可以找到一些常见的矩阵群，例如：

• 一般线性群$GL\left(n\right)$

指$n\times n$ 也就是所谓的旋转矩阵群，其中$SO\left(2\right)$ 和$SO\left(3\right)$

• 最为常见。正式的记法是：

$$\begin{array}{}SO\left(n\right)=\{R\in R^{n\times n}|{RR}^T=I,det(R)=1\}& \text{(3)}\end{array}$$

• 特殊欧氏群$SE\left(n\right)$

也就是前面提到的$n$维欧氏变换，如$SE\left(2\right)$和$SE\left(3\right)$。这里给出$SE\left(3\right)$

• 的记法：

$$\begin{array}{}SE\left(3\right)=\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|R\in SO(3),t\in R^3\}& \text{(4)}\end{array}$$

　　群结构保证了在群上的运算具有良好的性质，而群论则研究群的各种结构和性质，但我们在此不多加介绍。感兴趣的读者可以参考任意一本近世代数教材。

　　李群是指具有连续性质的群。并且，一般连续群上的运算还是无限可微，乃至解析的（解析比无限可微更强，它还要求任意点邻域的泰勒展开都收敛）。这个问题在20世纪初被称为希尔伯特第五问题，并已得到了解决。而李群，则指实数空间上的连续群。常见的李群包括上边提到的$GL\left(n\right),SO\left(n\right),SE\left(n\right)$

，以及其他的如酉群$U\left(n\right)$，辛群$Sp\left(2n\right)$

等等。

---

 三维旋转群$SO\left(3\right)$

　　三维旋转群$SO\left(3\right)$

是特殊正交群$SO\left(n\right)$在$n=3$时的特例，它们可以用来描述三维空间的旋转，其元素都是$3\times 3$ 的正交且行列式为$+1$的矩阵。假设有这样一个矩阵$R$，满足$R{R}^T＝I$。现在，考虑它随时间发生变化，即从$R$ 变成了$R\left(t\right)$，仍有$R\left(t\right)R(t{)}^T=I$

。在等式两边对时间求导，得到：

$$\begin{array}{}\dot{R}\left(t\right)R(t{)}^T+R\left(t\right)\dot{R}(t{)}^T=0& \text{(5)}\end{array}$$

　　于是：
$$\begin{array}{}\dot{R}\left(t\right)R(t{)}^T=-{\left(\dot{R}\right(t\left)R(t{)}^T\right)}^T& \text{(6)}\end{array}$$

　　可以看出$\dot{R}\left(t\right)R(t{)}^T$

是一个反对称矩阵。注意到对于任意一个$3\times 3$的反对称矩阵，我们记它为$A$。由于$A^T=-A$，所以它主对角线元素必为$0$，而非对角线元素则只有三个自由度。我们可以把它对应到一个向量$a=[a_1,a_2,a_3{]}^T$中去：
$$\begin{array}{}{a}^{\wedge }=A=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]& \text{(7)}\end{array}$$

　　其中${}^{}$

符号表示由向量转换为矩阵，反之我们也可以用符号${}^{}$定义由矩阵转换为向量的方式：
$$\begin{array}{}{A}^{\vee }=a& \text{(8)}\end{array}$$

　　注意到这样定义的好处之一，是它与叉积的兼容性。我们可以直接把矩阵与任意向量的乘积$Ab$

写成 $a\times b$

。读者可以自行验证这个兼容性。除此之外，这样定义的向量还有一些较好的性质，后文会提到。

　　现在，由于$\dot{R}\left(t\right)R(t{)}^T$

是一个反对称矩阵，我们可以找到一个三维向量$\varphi \left(t\right)\in R^3$

与之对应。于是有：

$$\begin{array}{}\dot{R}\left(t\right)R(t{)}^T=\varphi (t{)}^{\wedge }& \text{(9)}\end{array}$$

　　左右各右乘$R\left(t\right)$

，由于$R$为正交阵，有：
$$\begin{array}{}\dot{R}\left(t\right)=\varphi \left(t{)}^{\wedge }R\right(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]R(t)& \text{(10)}\end{array}$$

　　可以看到，每对旋转矩阵求一次导数，只需左乘一个$\varphi$

矩阵即可。由于$\varphi$反映了$R$的导数性质，故称它在$SO\left(3\right)$的正切空间(tangent space)上。同时，将上式类比于一个关于$R$

的微分方程，可得：

$$\begin{array}{}R\left(t\right)=\exp \left(\varphi \right(t{)}^{\wedge })R\left(t_0\right)& \text{(11)}\end{array}$$

　　由此我们可以引出两个概念。（1）求$\varphi$

的方法以及它的结构？——$\varphi$是对应到$SO\left(3\right)$上的李代数$so\left(3\right)$；（2）$\exp \left(\varphi \right)$

如何计算？——李群与李代数间的指数/对数映射。下面我们一一加以介绍。

---

 什么是李代数

　　对于$SO\left(3\right)$

和$SE\left(3\right)$，李代数可定义于李群的正切空间上，描述了李群中元素局部性质，分别把它们记作小写的$so\left(3\right)$和$se\left(3\right)$

。首先，给出通用的李代数的定义。

　　李代数由一个集合$V$

，一个数域$F$和一个二元运算$\left[\right]$组成。如果它们满足以下几条性质，称$(V,F,[\left]\right)$ 为一个李代数，记作$g$

。

• 封闭性 $\forall X,Y\in V,\left[XY\right]\in V$

有 $$[aX+bY,Z]=a\left[XZ\right]+b\left[YZ\right][Z,aX+bY]=a\left[ZX\right]+b\left[ZY\right]$$

• 
  

　　从表面上来看，李代数所需要的性质还是挺多的。其中二元运算被称为李括号。相比于群中的较为简单的二元运算，李括号表达了两个集合元素的差异。它不要求结合律，而满足反对称性，以及元素和自己做李括号之后为零的性质。作为类比，三维向量$R^3$

上定义的叉积$\times$是一种李括号，因此$g=(R^3,R,\times )$

构成了一个李代数。读者可以尝试将叉积的性质代入到上面四条性质中。

三维旋转群与对应的李代数
　　$SO\left(3\right)$

对应的李代数是定义在$R^3$上的向量，我们记作$\varphi$（注意这是个向量，虽然希腊字母的粗体不明显）。根据前面的推导，每个$\varphi$

都可以生成一个反对称矩阵：

$$\begin{array}{}\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in R^{3\times 3}& \text{(12)}\end{array}$$

　　在此定义下，两个向量${\varphi }_1,{\varphi }_2$

的李括号为：

$$\begin{array}{}[{\varphi }_1,{\varphi }_2]={\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1& \text{(13)}\end{array}$$

　　读者可以去验证该定义下的李括号满足上面的几条性质。由于$\varphi$

与反对称矩阵关系很紧密，在不引起歧义的情况下，就说$so\left(3\right)$的元素是3维向量或者3维反对称矩阵，不加区别：
$$\begin{array}{}so\left(3\right)=\{\Phi ={\varphi }^{\wedge }\in R^{3\times 3}|\varphi \in R^3\}& \text{(14)}\end{array}$$

　　反对称矩阵有一些重要的性质，重点包括以下两条：

$$\begin{array}{}\varphi {\varphi }^T={\varphi }^{\wedge }{\varphi }^{\wedge }+\parallel \varphi {\parallel }^2{I}_{3\times 3}& \text{(15)}\end{array}$$

　　当$\varphi$

为单位向量时，进而有：
$$\begin{array}{}\varphi {\varphi }^T={\varphi }^{\wedge }{\varphi }^{\wedge }+I1& \text{(16)}\end{array}$$

　　以及

$$\begin{array}{}{\varphi }^{\wedge }{\varphi }^{\wedge }{\varphi }^{\wedge }=-{\varphi }^{\wedge }& \text{(17)}\end{array}$$

　　这两条性质读者也可以自行验证，我们在指数映射中会用到。

　　至此，我们已清楚了$so\left(3\right)$

的结构。它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。现在来考虑$\exp \left({\varphi }^{\wedge }\right)$

是如何计算的，为此我们引入指数映射。

---

 指数映射

　　首先，回忆任意矩阵的指数映射。它可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。
$$\begin{array}{}\exp \left(A\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n& \text{(18)}\end{array}$$

　　同样地，对$so\left(3\right)$

中任意一元素$\varphi$

，我们亦可按此方式定义它的指数映射：

$$\begin{array}{}\exp \left({\varphi }^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& \text{(19)}\end{array}$$

　　现在我们来仔细看看它的含义。由于$\varphi$

是三维向量，我们可以定义它的模长和它的方向，分别记作$\theta$和$a$（注意这里记号是有含义的，此时$a$

是一个单位长度的向量），那么按照上式，可以推出如下公式，注意中间使用了上面讲到了两个反对称矩阵的性质：

$$\begin{array}{cc}\exp \left({\varphi }^{\wedge }\right)& =\exp \left(\theta a^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\theta a^{\wedge }\right)}^n\\ =I+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2{a}^{\wedge }{a}^{\wedge }+\frac{1}{3!}{\theta }^3{a}^{\wedge }{a}^{\wedge }{a}^{\wedge }+\frac{1}{4!}{\theta }^4{\left(a^{\wedge }\right)}^4+...\\ =a{a}^T-a^{\wedge }{a}^{\wedge }+\theta a^{\wedge }+\frac{1}{2!}\theta a^{\wedge }{a}^{\wedge }-\frac{1}{3!}{\theta }^3{a}^{\wedge }+\frac{1}{4!}{\theta }^4{\left(a^{\wedge }\right)}^4+...\\ =a{a}^T+\left(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...\right)a^{\wedge }-\left(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-...\right)a^{\wedge }{a}^{\wedge }\\ =a^{\wedge }{a}^{\wedge }+I+\sin \theta a^{\wedge }-\cos \theta a^{\wedge }{a}^{\wedge }\\ =(1-\cos \theta )a^{\wedge }{a}^{\wedge }+I+\sin \theta a^{\wedge }\\ =\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }\end{array}$$

　　最后我们得到了一个似曾相识的式子：
$$\begin{array}{}\exp \left(\theta a\right)=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }& \text{(20)}\end{array}$$

　　回忆前一节内容，它和罗德里格斯公式（参观本系列第一篇）如出一辄。这表明，$so\left(3\right)$

实际上就是由所谓的旋转向量组成的空间。特别地，当转轴取一定顺序时，李代数$so\left(3\right)$还会变为对应的欧拉角。通过罗德里格斯公式或者指数映射，我们把$R^3$ 中的一个向量对应到了一个位于$SO\left(3\right)$

中的3D旋转。

　　反之，如果定义对数映射，我们也能把$SO\left(3\right)$

中的元素对应到$so\left(3\right)$

中：

$$\begin{array}{}\varphi =\ln {\left(R\right)}^{\vee }={\left(\sum _{n=0}^{\infty }\frac{{\left(-1\right)}^n}{n+1}{\left(R-I\right)}^{n+1}\right)}^{\vee }& \text{(21)}\end{array}$$

　　其中${}^{}$

表示从反对称矩阵到向量的对应关系，为${}^{}$

的逆运算。

　　读者可能会问，指数映射性质如何呢？它是一个双射吗？很遗憾，它只是一个满射。每个$SO\left(3\right)$

中的元素，都可以找到$so\left(3\right)$中至少一个与之对应；但是可能存在多个$so\left(3\right)$中的元素，对应到同一个$SO\left(3\right)$元素上。至少对于旋转角$\theta$

，我们知道它具有周期性。

　　$SO\left(3\right)$

与$so\left(3\right)$

的结论似乎在我们意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似，而指数映射即是罗德里格斯公式。旋转向量可以视为旋转矩阵的导数，指导如何在旋转矩阵中进行微积分运算。

---

 三维欧氏群与对应的李代数

　　下面我们来介绍三维欧氏群$SE\left(3\right)$

以及对应的李代数$se\left(3\right)$。有了前面的基础，我们可以直接介绍它们的结构及运算了。$SE\left(3\right)$

的结构已经在前面介绍群的时候给出：

$$\begin{array}{}SE\left(3\right)=\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|R\in SO(3),t\in R^3\}& \text{(22)}\end{array}$$

　　每个变换矩阵有六个四由度，故对应的李代数位于$R^6$

中：
$$\begin{array}{}se\left(3\right)=\{\Xi ={\xi }^{\wedge }\in R^{4\times 4}|\xi \in R^6\}& \text{(23)}\end{array}$$

　　但是${}^{}$

不再对应到一个反对称关系，而是：
$$\begin{array}{}{\xi }^{\wedge }={\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]}^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]=\Xi & \text{(24)}\end{array}$$

　　可以看到，$\xi$

的前三维为旋转向量，后三维为平移向量，其定义也十分的直观。该李代数对应于微分方程：

$$\begin{array}{}\dot{T}\left(t\right)={\xi }^{\wedge }\left(t\right)T\left(t\right)& \text{(25)}\end{array}$$

　　因此
$$\begin{array}{}T\left(t\right)=\exp \left(\xi \right(t{)}^{\wedge }\left)T\right(t)& \text{(26)}\end{array}$$

　　那么$se\left(3\right)$

上的指数映射如何呢？略加推导可得：

$$\begin{array}{}\exp \left({\xi }^{\wedge }\right)& =\left[\begin{array}{cc}\sum _{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n& \sum _{n=0}^{\infty }\frac{1}{\left(n+1\right)!}{\left({\varphi }^{\wedge }\right)}^n\rho \\ 0^T& 1\end{array}\right]& =\left[\begin{array}{cc}\Phi & J\rho \\ 0^T& 1\end{array}\right]& \text{(27)}& \text{(28)}\end{array}$$

　　左上角的$\Phi$

是我们熟知的$so\left(3\right)$中的元素，前文已经介绍过了。而右上角的$J$则可整理为（设$\varphi =\theta a$

）：

$$\begin{array}{}J=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }& \text{(29)}\end{array}$$

　　因此我们就得到了$se\left(3\right)$

的指数映射的关系。 其对数映射亦可类比推得。

---

 小结

　　最后，我们对之前介绍的李群李代数进行一个简单的小结。概而言之，李群有以下两个重要用处：

• 李代数表达的正切空间，具有和对应李群相同的自由度。
• 指数映射能把正切空间中任意向量正好映射到原李群。

 　　下篇中，我们将教大家用Eigen和Sophus库处理变换矩阵与李代数。敬请期待。

参考资料

[1]. Yi Ma, An Invitation to 3D Vision. 2001.

[2]. Timothy D. Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2015.

结合律: $\forall a_1,a_2,a_3,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$

幺元: $\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$

逆: $\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

的可逆矩阵，它们对矩阵乘法成群。

特殊正交群$SO\left(n\right)$

双线性 $\forall X,Y,Z\in V,a,b\in F,$

 

自反性 $\forall X\in V,\left[XX\right]=0$

雅可比等价 $\forall X,Y,Z\in V,[X,[YZ\left]\right]+[Z,[YX\left]\right]+[Y,[ZX\left]\right]$

clc;
clear;



syms x;
f = sin(x)/x;
xpoint = 2;
ypoint = subs(f, x, xpoint);
T6 = taylor(f, x, xpoint, Order=3);
fp = fplot(f, 'b-');
fp.LineWidth = 3;
hold on;
ft=fplot(T6, 'm--');
ft.LineWidth = 2;
scatter(xpoint, ypoint, 300, "filled", 'r');
hold on;
hold off;
xlim([-4 4]);
grid on;


legend("sin(x)/x", ...
       "approximation of sin(x)/x", ...
       "x=2", Location="northeast")
title("Taylor Series Expansion")

迭代最近点 (ICP) 是一种广泛使用的经典计算机视觉算法，用于二维或三维点云配准。顾名思义，它通过迭代地改进和最小化两个点云之间的空间差异或平方误差之和。这是通过估计最优刚体变换（旋转）来实现的。

迭代最近点 (ICP)算法是一种广泛应用的经典计算机视觉算法，用于二维或三维点云配准。顾名思义，它通过迭代的方式改进并最小化两个点云之间的空间差异或平方误差之和。其实现方式是估计最优的刚体变换（旋转和平移）矩阵，从而更好地将“源”或参考点集与同一坐标系中的“目标”点集对齐。ICP 算法在医学成像、三维空间建模、即时定位与地图构建 (SLAM)、自动驾驶汽车和机器人等领域有着广泛的应用。

ICP 最早由 Besl 和 McKay 正式提出，其概念可以追溯到 1992 年。自那时以来，ICP 已经发展了很多，衍生出了许多变体，并进行了显著的改进，主要针对鲁棒的对应匹配、加权误差度量和最近邻搜索的加速结构。

在本文中，我们将概述 ICP、其流程、变体和实际应用案例，并提供一个使用 Open3D 的演示示例。

如果您是刚刚开始接触 3D 计算机视觉的人，我们的系列文章将指导您了解3D 视觉和重建的基础知识。

1. 基于高斯散射和NeRF的3D重建
2. 立体和单目深度估计
3. 理解相机标定和几何原理。
4. 单目视觉SLAM
5. DUSt3R：密集三维重建
6. MASt3R 和 MASt3R-SfM
7. VGGT：VGGT：视觉几何接地变压器
8. MASt3R-SLAM：实时密集SLAM

1. 什么是点云配准？
2. 迭代最近点 (ICP) 流水线
   1. 步骤 1：初始化
   2. 第二步：寻找对应关系
   3. 步骤 3：估算转换
   4. 步骤 4：应用转换
   5. 步骤 5：检查收敛性
3. ICP绩效的实际考量
4. 常见ICP变异
5. ICP代码详解
   1. 使用 Open3D
   2. Python 中的简单实现
6. ICP的实际应用案例
7. 结论
8. 参考

什么是点云配准？

点云本质上是三维坐标系中的一组顶点，表示为 P = {p_1, p_2, …, p_N} 。为了构建物体（例如雕像）的三维重建，我们首先需要从多个角度拍摄场景。点云的来源可以是激光雷达、RGB 相机或立体相机，每次扫描都会生成一个独立的点云。

难点在于如何将从不同视角或不同时间间隔拍摄的物体多次扫描的特征组合成一个连贯的整体，避免出现间隙、变形或异常值。为了从中创建完整的几何体，我们需要将它们精细地拼接在一起。在同一个坐标系中，找到使源点云与目标点云对齐的精确旋转和平移的过程称为配准。

它首先对变换进行初始猜测，然后逐步改进，直到满足收敛准则。ICP 的直观思想是，如果我们知道源点云和目标点云之间的对应点，那么在数学上找到最佳变换就非常简单。另一方面，如果我们知道正确的变换，那么找到对应点也很容易，而且它们会完美对齐。但我们两者都不知道，因此 ICP 会在估计对应点和基于初始对应点估计变换之间交替进行。如果两个点云来自同一源，我们就无需考虑缩放问题。

尽管 ICP 的概念很简单，但它的有效性取决于初始化质量、误差度量的选择以及异常值剔除策略等因素。

迭代最近点 (ICP) 流水线

标准ICP算法包含以下步骤：

步骤 1：初始化

ICP 不是从零开始；它从对变换的初始估计开始，大致将源云与目标云对齐。

这里展示的是应用初始变换后的源云。初始猜测的质量至关重要；糟糕的初始化会导致ICP收敛到错误的算法（局部最小值），甚至根本无法收敛。

最初的猜测可能来自：

• 传感器先验信息（移动扫描仪上的 GPS/IMU 数据）
• 用户手动放置，
• 匹配云朵之间的独特特征。
• 全局配准算法的输出（提供粗略的对齐结果）。我们初始化计数器。

第二步：寻找对应关系

对于当前迭代变换后的源点云中的每个点，找到目标点云中距离它最近的点。“最近”通常由欧氏距离定义。此步骤建立了一组相关的对应点对。然而，为点云中的每个点找到匹配点对的计算量非常大。为了克服这一瓶颈，最常用的方法是在静态目标点云上构建kd 树。kd 树递归地划分三维空间，从而实现高效的最近邻搜索。这通过从源点云中找到查询点的最近邻来缩小搜索空间，从而显著加快了处理速度。

步骤 3：估算转换

给定上一步找到的匹配点对集合，该算法计算使这些点对最佳对齐的刚体变换（）。通过最小化变换后的源点与其对应目标点之间的平方欧氏距离之和。

该优化问题具有闭式解。寻找最优旋转和平移的常用且稳健的方法是使用奇异值分解 (SVD)。首先计算对应源点 ( ) 的质心（平均位置）。然后计算质心点：和。

交叉方差矩阵是将这些中心化后的对应对的外积相加而得到的。

对该矩阵应用奇异值分解 (SVD)可得到三个矩阵，

。

这里，

• 并且是表示旋转/反射的正交矩阵
• 是一个对角矩阵，对角线上只有一个值。

然后计算最优旋转矩阵。通常需要进行特殊检查：如果的行列式为 -1（表示反射而非正确的旋转），则通常需要先将最后一列的符号取反，然后再重新计算，以确保它是一个有效的旋转矩阵。一旦找到最优旋转，就可以使用质心轻松计算最优平移。

步骤 4：应用转换

然后应用新计算的变换来更新源点云。通常，变换是相对于原始源点云累积的，而不是迭代地变换点本身，以避免潜在的漂移。

步骤 5：检查收敛性

该算法会检查是否满足终止条件。常见的收敛准则包括：

• 迭代之间均方误差（在步骤 3 中计算）的相对变化低于预定义的阈值。
• 两次迭代之间变换参数（R 和 t）的相对变化低于阈值。
• 已达到最大迭代次数。

如果满足收敛准则，算法终止并输出最终变换（）。否则，递增（即），并返回步骤 2。找到两个云之间最佳的匹配，并找到对应关系。基于此，尝试优化平移和旋转。

ICP绩效的实际考量

虽然流程看起来很简单，但有几个因素会严重影响 ICP 在现实世界中的有效性：

• 初始化和局部最小值：如前所述，ICP 是一种局部优化算法。它在初始猜测的最近邻域内寻找最佳对齐方式。如果初始猜测较差，ICP 很容易陷入局部最小值——一个看起来局部最优的对齐方式可能全局正确。确保一个合理的初始位置至关重要，这通常可以通过粗略的初始对齐或全局配准技术来实现。因此，初始姿态的选择方式应确保源点和目标点在开始时有足够的重叠。

• 异常值敏感性：标准最小二乘误差度量对异常值（由噪声、不重叠区域或动态对象引起的错误对应关系）非常敏感。少数错误匹配就可能显著扭曲变换估计。缓解此问题的策略包括：
  • 距离阈值法：拒绝距离超过一定值的配对。
  • 百分比拒收：剔除错误率最大的固定比例的配对。
  • 稳健误差指标：使用对异常值不太敏感的指标，例如绝对差值之和（L1 范数）或更高级的稳健函数（例如 Huber 损失）。
  • 随机抽样一致性（RANSAC）：迭代地选择最小的配对子集来估计变换，并找到由最多内点支持的变换。

• 计算成本：即使使用kd树，处理大型点云也十分耗时。以下技术可以缓解这一问题：
  • 点采样：仅使用源点云中的点子集（例如，随机采样、均匀采样或选择高曲率区域中的点）。
  • 多分辨率方法：首先对点云的降采样版本执行 ICP 以进行粗略配准，然后逐步提高分辨率以进行精细配准。

常见ICP变异

基本的ICP算法（通常称为点对点ICP算法）启发了许多旨在提高收敛速度和精度的变体：

• 点对点 ICP：这是经典的或标准的 ICP 算法，它通过在目标云中找到源云中每个点的最近点来建立对应关系。目标是最小化这些对应点之间欧氏距离平方和。虽然算法简单直接，但在某些情况下收敛速度可能较慢，并且它完全依赖于点之间的邻近性。

• 点到平面 ICP 算法：这种常用的变体算法并非最小化对应点之间的直接距离，而是最小化每个源点到其对应目标点处切平面的距离。这需要估计目标点云（有时也包括源点云）的表面法线。误差度量也会随之改变，以反映这种点到平面的距离。这种方法通常能更有效地利用底层表面几何形状，从而实现更快的收敛速度和更高的精度，尤其适用于具有平坦或结构化表面的场景。

• 平面到平面（广义ICP-G-ICP）：该方法通过对源云和目标云中的不确定性和局部平面性（使用协方差矩阵）进行建模，进一步扩展了上述思想。它有效地最小化了局部平面区域之间的距离，与点到点或点到平面方法相比，通常对噪声和点密度变化具有更强的鲁棒性。

• 对称ICP：在标准ICP算法中，通常只寻找源点到目标点的对应关系。对称ICP算法在优化过程中同时考虑源点到目标点和目标点到源点两个方向的对应关系。这有时可以提高稳定性，并防止对齐结果向某一方向漂移。

ICP代码详解

首先，我们将通过 Open3D 示例可视化 ICP。然后，我们将用 Python 实现一个非常简单的 ICP 版本，以理解其底层机制。

使用 Open3D

导入依赖项

导入数据集

我们将从 Open3D ICP 演示数据集中加载两个示例点云（源点云和目标点云），每个点云都代表从不同视角捕获的房间的 3D 扫描。

注册前可视化输入

初始注册信息来自全球注册系统

以下函数可视化了经过粗略初始对齐变换后的目标点云和源点云。目标点云和源点云分别用青色和黄色表示。两个点云重叠越多、越紧密，对齐结果就越好。

该函数 evaluate_registration 计算两个主要指标：

• fitness该指标衡量重叠区域（内点对应点数/源点数）。数值越高越好。
• inlier_rmse它衡量所有内点对应关系的均方根误差 (RMSE)。数值越低越好。

默认情况下， registration_icp 程序会运行直至收敛或达到最大迭代次数（默认为 30 次）。可以更改此设置以允许更多计算时间并进一步提高结果。

通过使用ICP进行注册

• 点对点
• 点到平面

该类 TransformationEstimationPointToPoint 提供用于计算点对点ICP目标函数的残差和雅可比矩阵的函数。该函数 `registration_icp` 以残差和雅可比矩阵作为参数，并运行点对点ICP算法以获得结果。

点对点

得分 fitness 从 0.174723 增加到 0.372450。 inlier_rmse 从 0.011771 减少到 0.007760。

点到平面

点到面 ICP 算法使用点法线。在本教程中，我们将从文件中加载法线。如果未提供法线，则可以使用顶点法线估计来计算。`registration_icp` 函数会使用不同的参数调用 TransformationEstimationPointToPlane。该类内部实现了用于计算点到面 ICP 目标函数的残差矩阵和雅可比矩阵的函数。

点到平面 ICP 在 30 次迭代内达到紧密对齐（ fitness 得分分别为 0.620972 和 inlier_rmse 0.006581）。

简单实现

将两个原本相同但经过齐次变换的正弦点云对齐。ICP算法通过最小化两个点云之间的距离，逐步使它们对齐。

导入依赖项

m函数best_fit_transfor实现了一种最小二乘最佳拟合变换算法，用于映射两个点云（A 和 B）之间的匹配点（对应关系）。它计算将 A 变换到 B 所需的旋转矩阵、平移向量和齐次变换矩阵。

步骤：

1. 检查 A 和 B 是否形状相同
2. 计算点 A 和点 B 的质心，并将这些点平移到它们的质心处。
3. 使用奇异值分解 (SVD) 计算旋转矩阵。如果旋转矩阵的行列式为负，则将 Vt 的最后一行进行反射。
4. 利用变换后的质心和旋转矩阵计算平移量
5. 利用旋转矩阵和平移向量构造齐次变换矩阵。

接下来，我们需要找到目标点云和源点云之间的最近邻点。该函数nearest_neighbor接受两个输入 src 和 dst，并使用 kd 树返回 src 中每个点到 dst 中最近邻点的欧氏距离和索引。

以下函数实现了ICP算法，迭代地更新更新后的A点和B点之间的变换关系。迭代过程持续进行，直到当前迭代和前一次迭代之间最近点距离的平均误差之差小于指定的容差值，或者达到最大迭代次数为止。输出结果是最终的齐次变换矩阵，T_final该矩阵将A点映射到B点，直至收敛。

为了可视化所有步骤的收敛过程，我们还需要存储中间步骤。

ICP的实际应用案例

• SLAM：在机器人和自动驾驶领域，ICP（集成控制点）是扫描匹配的基础。通过对齐连续的传感器扫描（例如，来自激光雷达的扫描），机器人可以估计自身的运动（里程计）并构建其环境的一致性地图。现代最先进的SLAM系统通常使用复杂的ICP变体，例如：KISS-ICP和GenZ-ICP。

“只要方法得当，传统的ICP依然强大。”

• 医学影像：将不同方式或时间拍摄的医学扫描图像进行比对，以追踪疾病的传播，计划手术或指导干预措施。

• 制造和质量控制：将制造零件的 3D 扫描与其数字 CAD 模型进行比较，以检测缺陷并确保满足公差（计量）。

• 增强/虚拟现实（AR/VR）：将虚拟对象与现实世界的点云数据对齐，可在混合现实环境（HoloLens）中实现逼真的集成和交互，确保正确的放置和缩放。

ICP的优势

• 精准微调：在给定良好起始点的情况下，能非常有效地进行精细化对准。
• 简洁性：核心迭代理念易于理解和实现。
• 稳健：增强版本（例如，点到平面）能更好地处理噪声、异常值和无特征区域。

ICP的缺点

• 需要良好的初始猜测：如果初始对齐效果不佳，则极易失败（陷入局部最小值）。如果两个点云重叠不足，则对齐可能无法收敛。
• 计算成本高：速度可能较慢，尤其对于大型点云而言。
• 对异常值/低重叠度敏感：标准版本在噪声、错误配对和有限重叠度方面表现不佳。

结论

我们深入探讨了迭代最近点算法（ICP），并理解了其数学公式如何转化为Python代码。虽然基于深度学习的方法和复杂的配准技术在复杂场景下具有优势，但ICP的核心原理提供了一种简单有效的途径，在某些特定场景下甚至优于最先进的SLAM算法。这凸显了经典的计算机视觉算法在许多应用中仍然具有重要意义，并且是基础架构。

希望您觉得这篇文章有趣。欢迎通过我们的社交媒体账号分享您的反馈意见。

这篇文章旨在提供一个具体的例子，让机器人专家们对李代数及其在机器人领域的应用有所了解。我最初学习李代数时，感觉网上关于李代数的解释过于浅显，缺乏清晰易懂的数学细节。希望这篇文章能够弥合这种差距。让我们开始吧。

按回车键或点击查看完整尺寸的图片

本文仅考虑迭代最近点算法（Iterative Closest Point，ICP）。为简单起见，我们仅讨论旋转问题，不涉及平移。ICP 算法通过最小化对应点之间的距离来对齐两个点云。在本例中，它优化旋转矩阵 R 以减小平方差之和。

如同许多工程实践一样，我们希望使用一阶泰勒级数展开来近似函数，展开平方项，对 ΔR 求导，令导数为零，最终将问题表示为一个正规方程。求解该方程即可得到 ΔR，进而可以用它更新 R。ICP 问题正是通过多次重复此过程（假设收敛）来求解的，因此得名“迭代”。

按回车键或点击查看完整尺寸的图片

然而，这里我们遇到了一个问题。如果我们把3x3旋转矩阵看作9个独立元素并求解，结果很可能不是旋转矩阵。旋转矩阵需要满足诸如det(R)=1和RR^T=I单位矩阵之类的约束条件。而如何在满足这些约束条件的前提下建立一个可微的代价函数，这一点并不明显。

这就是李群和李代数发挥作用的地方。旋转构造属于李群，李群是一个具有流形结构的连续变换群。流形是一个光滑的广义曲面，可以局部地用线性空间（也称为切空间）逼近。这类似于地球的曲面可以局部地逼近为一个平面；虽然地球是一个三维球体，但其表面在任何一点都感觉像是一个二维平面。

按回车键或点击查看完整尺寸的图片

关键在于切空间是线性的，而李代数能够局部地捕捉这种线性结构。如果我们能够以某种方式用李代数（具有3个自由度）来表达我们的目标函数，从而将其置于线性空间中，我们就可以（再次）构建一个优化问题，而无需考虑应用于旋转矩阵的约束。

按回车键或点击查看完整尺寸的图片

这里值得稍作停顿。为什么我们用不同的数学结构来表达最初的问题后，就不再需要担心旋转矩阵的约束了呢？这是因为这些约束是由流形或李群本身施加的。

想象一下等离子体被限制在球面上，例如地球磁层上的带电粒子。如果我们想求解它们的“流体动力学”，就无法在开放域（三维空间）中进行求解。解被限制在三维空间中的一个二维曲面上。因此，如果我们用 S²（代表球面的流形）来表达这个问题，那么问题本身就是有约束的。

类似地，我们可以想象三维旋转被限制在一个具有更复杂结构的三维几何曲面（位于九维空间中）上——即李群——形式上称为特殊正交群SO(3)。该群不仅具有流形结构，而且还具有一个光滑的群运算，该运算定义了旋转如何组合。

按回车键或点击查看完整尺寸的图片

我们之前没有讨论过的一点是，李代数被定义为李群单位元处的切空间。了解这一点可以简化我们的方程，如下所示：

按回车键或点击查看完整尺寸的图片

由于 R 属于李群 SO(3)，其导数 Ṙ 位于 R 的切空间中。由此我们可以推断，phi 是属于李代数 so(3) 的斜对称矩阵，即 SO(3) 在单位元处的切空间。

还记得我们希望用 3x1 向量而不是 3x3 矩阵来表示旋转（及其增量）吗？不难看出，3x1 向量和相应的反对称矩阵之间存在一一对应的关系。

我们可以将向量到反对称矩阵的转换定义为帽子符号 (∧)，反向转换定义为三角符号 (∨)。那么我们知道函数 g() 具有 exp(. ∧) 的形式，它将一个 3x1 的向量映射到一个 3x3 的旋转矩阵。

按回车键或点击查看完整尺寸的图片

记住矩阵指数 exp(A) 可以近似为 I + A，并回想 a^b = -a^b，其中 a^b 可以理解为两个 3x1 向量 a 和 b 的叉积。然后我们最终可以将代价函数表示为 f(R + δr) ≈ f(R) + Jδr，其中 J 等于 [Rp]^。在求解 δr 的正规方程后，我们可以更新 R ← exp(δr) R。

按回车键或点击查看完整尺寸的图片

本文到此结束。当然，还有其他更直观的方法可以解决ICP问题。但在诸如扩展卡尔曼滤波（EKF）或同步定位与建图（SLAM）等应用中，由于涉及将三维旋转作为状态向量，推导雅可比矩阵并用李代数表示协方差会很有帮助。

李群既是流形又是群，其群运算——乘法和逆运算——是光滑（无限可微）映射。换句话说，它无缝地融合了代数结构（来自群论）和几何结构（来自流形）。这种几何框架自然地与线性代数和工程数学相结合，为分析连续变换（例如旋转和刚体运动）提供了强大的工具。

如需进一步阅读，我推荐阅读“用于机器人状态估计的微观李理论”。