PCL学习九：Registration-配准

参考引用

Point Cloud Library
黑马机器人 | PCL-3D点云

1. 点云中的数学

函数求导
- 对于函数 $f(x)=x^2$
  - 其一阶导数也是 $x$ 的函数： $\frac{df}{dx}=2x$
  - 其二阶导为常数，与 $x$ 无关： $\frac{d^2f}{dx^2}=2$

1.1 方差 & 协方差矩阵

1.1.1 基本概念

方差（Variance）
- 衡量的是单个随机变量的变化（比如一个人在群体中的身高），概率论中方差用来度量随机变量和其数学期望（即均值）之间的偏离程度，方差的公式为
  $var(x)=\sigma_x^2=\frac{1}{n-1}\sum_{i=1}^n\left(x_i-\bar{x}\right)^2$
- $\bar{x}$ 是随机变量 $x$ 的均值， $n$ 为样本总数
标准差（Standard Deviation）
- 方差的算术平方根，用 $\sigma$ 表示，标准差能反映一个数据集的离散程度
协方差（Covariance）
- 衡量的是两个随机变量同时变化的离散程度(比如一个人在群体中的身高和体重)，方差是协方差的一种特殊情况
  $cov(x,y)=\sigma(x,y)=\frac{1}{n-1}\sum_{i=1}^n\left(x_i-\bar{x}\right)\left(y_i-\bar{y}\right)$
- 单个随机变量的方差 $\sigma_x^2$ 也可以通过协方差表示 $\sigma(x,x)$ ，方差和协方差有如下性质
  $\begin{array}{l}cov(x,x)=var(x)\\ \\ cov(x,y)=cov(y,x)\end{array}$

1.1.2 协方差矩阵

协方差矩阵表示一组随机变量之间的两两线性相关性，其中的每个元素代表了两个随机变量之间的协方差。根据方差、协方差的定义：假如现在有 $n$ 个样本数据，每个样本有 $d$ 个随机变量，则其协方差矩阵为
$\Sigma=\begin{bmatrix}\sigma\left(x_1,x_1\right)&\dots&\sigma\left(x_1,x_d\right)\\ \vdots&\ddots&\vdots\\ \sigma\left(x_d,x_1\right)&\dots&\sigma\left(x_d,x_d\right)\end{bmatrix}\in\mathbb{R}^{d\times d}$
主对角线上的元素为各个随机变量的方差，非对角线上的元素为两两随机变量之间的协方差，根据协方差的定义，我们可以认定：矩阵 $\Sigma$ 为对称矩阵（symmetric matrix），其大小为 $d \times d$ 。则如果有一堆二维的点坐标，其协方差矩阵为
$\Sigma=\begin{pmatrix}\operatorname{cov}(x,x)&\operatorname{cov}(x,y)\\ \operatorname{cov}(y,x)&\operatorname{cov}(y,y)\end{pmatrix}$
三维点坐标的协方差矩阵应为
$\Sigma=\begin{pmatrix}\text{cov}(x,x)&\text{cov}(x,y)&\text{cov}(x,z)\\ \text{cov}(y,x)&\text{cov}(y,y)&\text{cov}(y,z)\\ \text{cov}(z,x)&\text{cov}(z,y)&\text{cov}(z,z)\end{pmatrix}$

1.1.3 多元正态分布

假如数据集 $X$ 中的向量 $\vec{x}$ 服从多元正态分布（multivariate Gaussian distribution），且均值向量 $\vec{\mu}$ ，协方差为 $\Sigma$
$p(\vec{x})=\frac{1}{2\pi\sqrt{|\Sigma|}}\exp\left(-\frac{(\vec{x}-\vec{\mu})^{\mathrm{T}}\Sigma^{-1}(\vec{x}-\vec{\mu})}{2}\right)$
一起通过二维点分布，探究协方差矩阵与数据分布特点的关系。为了简化图像，令均值向量 $\vec{\mu}$ 为零向量，指数前的系数通常作为常量，即跟变化无关，则将多元正态分布简化为似然函数
$L(\vec{x})\propto\exp\left(-\frac12\vec{x}^\mathrm T\Sigma^{-1}\vec{x}\right)$
为了展示二维图像，则令 $\vec{x}=(x,y)^T$ ，包含 $x, y$ 方向的坐标值，则协方差为以下形式
$\Sigma=\begin{bmatrix}\sigma(x,x)&\sigma(x,y)\\ \sigma(y,x)&\sigma(y,y)\end{bmatrix}\in\mathbb{R}^{2\times2}$
这里令协方差矩阵 $\Sigma$ 为单位矩阵 $I={\begin{bmatrix}{1}&{0}\\ {0}&{1}\end{bmatrix}}$ ，即随机变量 $x, y$ 的方差均为 1，可以生成如下随机分布，共200个点
```
import numpy as np
import matplotlib.pyplot as plt
# %matplotlib inline

plt.style.use('ggplot')
plt.rcParams['figure.figsize'] = (12, 8)

# Normal distributed x and y vector with mean 0 and standard deviation 1
x = np.random.normal(0, 1, 200)
y = np.random.normal(0, 1, 200)
X = np.vstack((x, y))

plt.scatter(X[:, 0], X[:, 1])
plt.title('Generated Data')
plt.axis('equal')
plt.show()
```
此时，变量 $x, y$ 是相互独立的，协方差矩阵 $\Sigma$ 是如下形式
$C=\Sigma=\begin{bmatrix}\sigma_x^2&0\\ 0&\sigma_y^2\end{bmatrix}$
可以通过如下代码计算随机生成的点的真实协方差矩阵，根据输出结果可见其十分接近之前对随机变量 $x, y$ 方差的定义 $\sigma_{x}^{2}=\sigma_{y}^{2}=1$ ，即协方差矩阵主轴上的数据描述了其各个维度的离散程度（分布情况），值越大，分散的越开
```
# Covariance
def cov(x, y):
    xbar, ybar = x.mean(), y.mean()
    return np.sum((x - xbar)*(y - ybar))/(len(x) - 1)

# Covariance matrix
def cov_mat(X):
    return np.array([[cov(X[0], X[0]), cov(X[0], X[1])],
                        [cov(X[1], X[0]), cov(X[1], X[1])]])

# Calculate covariance matrix
print(cov_mat(X)) # (or with np.cov(X))
```
```
# 输出结果
[[ 0.99913267 -0.08235379]
    [-0.08235379  0.91284394]]
```

1.1.4 线性变换

接下来为所有点进行线性变换，来观察数据分布的变化以及协方差矩阵的变化。我们先使用以下缩放矩阵（scaling matrix）修改之前的协方差矩阵
$S=\begin{bmatrix}s_x&&0\\ 0&&s_y\end{bmatrix}$
然后使用旋转矩阵（rotation matrix）进行旋转， $\theta$ 为逆时针旋转的弧度值
$R=\begin{bmatrix}\cos(\theta)&-\sin(\theta)\\ \sin(\theta)&\cos(\theta)\end{bmatrix}$
本例设 $s_{x}={\frac{1}{2}},s_{y}=2,\theta={\frac{\pi}{6}}$ ，即水平方向缩放一半，竖直方向放大一倍，然后逆时针旋转30度
$S=\begin{bmatrix}\frac{1}{2}&0\\ 0&2\end{bmatrix},R=\begin{bmatrix}\frac{\sqrt{3}}{2}&-\frac{1}{2}\\ \frac{1}{2}&\frac{\sqrt{3}}{2}\end{bmatrix}$

只进行缩放操作

import numpy as np
import matplotlib.pyplot as plt

plt.style.use('ggplot')
plt.rcParams['figure.figsize'] = (12, 8)

# Normal distributed x and y vector with mean 0 and standard deviation 1
x = np.random.normal(0, 1, 200)
y = np.random.normal(0, 1, 200)
X = np.vstack((x, y)) # 2xn

sx, sy = 0.5, 2.0
Scale = np.array([[sx, 0], [0, sy]])

Y = Scale.dot(X)
# 原始点集
plt.scatter(X[0, :], X[1, :])
# 缩放后点
plt.scatter(Y[0, :], Y[1, :])
plt.title('Generated Data')
plt.axis('equal')
plt.show()

在这里插入图片描述

先缩放，后旋转

import numpy as np
import matplotlib.pyplot as plt

plt.style.use('ggplot')
plt.rcParams['figure.figsize'] = (12, 8)

# Normal distributed x and y vector with mean 0 and standard deviation 1
x = np.random.normal(0, 1, 200)
y = np.random.normal(0, 1, 200)
X = np.vstack((x, y)) # 2xn

sx, sy = 0.5, 2.0
Scale = np.array([[sx, 0], [0, sy]])

# Rotation matrix
theta = np.pi / 6
c, s = np.cos(theta), np.sin(theta)
Rot = np.array([[c, -s], [s, c]])

# Transformation matrix
T = Rot.dot(Scale)

# Apply transformation matrix to X
Y = T.dot(X)

# 原始点集
plt.scatter(X[0, :], X[1, :])
# 缩放、旋转后
plt.scatter(Y[0, :], Y[1, :])
plt.title('Generated Data')
plt.axis('equal')
plt.show()

# Calculate covariance matrix
print(cov_mat(Y)) # (or with np.cov(Y))

// 输出结果
[[ 1.1830068  -1.66028421]
 [-1.66028421  3.10071868]]

在这里插入图片描述

为了计算变换与协方差矩阵个关系，我们将旋转矩阵、缩放矩阵合并为同一个变换矩阵 $A = RS$ ，我们想了解的是经过线性变换后，其协方差矩阵的变换。即将所有点进行变换操作 $\vec{t}=A\vec{x}$ 之后，在 $\vec{t}$ 集合情况下的协方差矩阵 $\Sigma$ 对应的值。则将 $\vec{x}=A^{-1}\vec{t}$ 代入函数可得
$\begin{aligned}L(\vec{x})\propto\exp\left(-\frac{1}{2}(A^{-1}\vec{t})^n(A^{-1}\vec{t})\right)\\ =\exp\left(-\frac{1}{2}t^T(A^{-1})^{\text{T}}A^{-1}\vec{t})\right)\\ =\exp\left(-\frac{1}{2}t^T(A^T)^{-1}A^{-1}\vec{t})\right)\\ =\exp\left(-\frac{1}{2}t^T(AA^T)^{-1}\vec{t})\right)\end{aligned}$
- 注意，由于之前的 $\Sigma$ 为单位矩阵，故这里的 $\Sigma^{-1}$ 直接省掉
则变换后的多元正态分布的协方差矩阵为
$A=RS=\begin{bmatrix}\frac{\sqrt{3}}{2}&-\frac{1}{2}\\ \frac{1}{2}&\frac{\sqrt{3}}{2}\end{bmatrix}\begin{bmatrix}\frac{1}{2}&0\\ 0&2\end{bmatrix}=\begin{bmatrix}\frac{\sqrt{3}}{4}&-1\\ \frac{1}{4}&\sqrt{3}\end{bmatrix}$

$\Sigma=AA^T=\begin{bmatrix}\frac{\sqrt{3}}{4}&-1\\ \frac{1}{4}&\sqrt{3}\end{bmatrix}\begin{bmatrix}\frac{\sqrt{3}}{4}&\frac{1}{4}\\ -1&\sqrt{3}\end{bmatrix}=\begin{bmatrix}\frac{19}{16}&-\frac{15\sqrt{3}}{16}\\ -\frac{15\sqrt{3}}{16}&\frac{49}{16}\end{bmatrix}$

可验证与之前代码输出的协方差结果近似。现在我们已经得到协方差矩阵与缩放矩阵、旋转矩阵的关系，由于协方差矩阵是正定对称矩阵即可对角化矩阵，则接下来我们对协方差矩阵进行特征值分解，观察其特征向量矩阵、特征值矩阵与缩放矩阵、旋转矩阵的关系

1.1.5 特征分解

对于任意可对角化矩阵，都存在一个特征分解（eigen decomposition）
$\Sigma=U\Lambda U^T$
- 其中 $U$ 为特征向量矩阵，且是正交矩阵，满足 $U^{T}=U^{-1},U U^{T}=I$
- $\Lambda$ 为特征值矩阵，且是对角矩阵，其对角线上是特征值，非对角线的元素为 0
则对 $\Sigma$ 进行变换得
$\Sigma=\left(U\Lambda^{1/2}\right)\left(U\Lambda^{1/2}\right)^T$
结合变换后的多元正态分布的协方差矩阵公式可得
$\left(U\Lambda^{1/2}\right)\left(U\Lambda^{1/2}\right)^T=AA^T=RS(RS)^T=RSS^TR^T$
- 此时， $A=RS=U\Lambda^{1/2}$ ，即可发现旋转矩阵和特征向量矩阵相关，缩放矩阵与特征值矩阵相关
  $U=\pm R=\pm\begin{bmatrix}\frac{\sqrt{3}}{2}&-\frac{1}{2}\\ \frac{1}{2}&\frac{\sqrt{3}}{2}\end{bmatrix}$

$\Lambda=SS^T=\begin{bmatrix}\frac{1}{2}&0\\ 0&2\end{bmatrix}\begin{bmatrix}\frac{1}{2}&0\\ 0&2\end{bmatrix}=\begin{bmatrix}\frac{1}{4}&0\\ 0&4\end{bmatrix}$

通过以下代码对

\Sigma

进行特征分解，从而对结果进行验证

import numpy as np

sigma = np.array([
    [19 / 16, -15 * np.sqrt(3) / 16,],
    [-15 * np.sqrt(3) / 16, 49 / 16]
])

eVa, eVe = np.linalg.eig(sigma)
print("eigen value:\n", eVa)
print("eigen vector:\n", eVe)

# 结果输出（可验证该输出结果与上述一致）
eigen value - SS^T:
 [0.25 4.  ]
eigen vector - U:
 [[-0.8660254  0.5      ]
 [-0.5       -0.8660254]]

可以将特征值和特征向量分别相乘，得到变换后椭圆区域的两个基向量

# Calculate covariance matrix
C = cov_mat(Y)
print(C)  # (or with np.cov(Y))

eVa, eVe = np.linalg.eig(C)

plt.scatter(Y[:, 0], Y[:, 1])
for value, eigen in zip(eVa, eVe.T):
    plt.plot(
        [0, 3 * np.sqrt(value) * eigen[0]],
        [0, 3 * np.sqrt(value) * eigen[1]],
         lw=5)

plt.title('Transformed Data')
plt.axis('equal')
plt.show()

在这里插入图片描述

总结

一个多元正态分布的概率密度函数中，其协方差矩阵 $\Sigma$ 包含了分布的旋转角度和缩放信息，其均值向量 $\vec{\mu}$ 包含了密度中心的位置信息

1.2 雅可比矩阵

将多个维度作为整体，来理解函数与变量的关系。雅可比矩阵即通过向量来描述高维度的数据在空间中的表现。通过向量将每一个函数的变量变化（偏导数）合并为一个整体，在高维空间去观察变化。 $∣ J ∣$ 是其在梯度方向的向量的模，该向量指向其梯度的上坡方向

1.2.1 雅可比（Jacobian）

假如求一个二元函数 $f (x, y)$ 的导数时，会先去求出每一个变量的偏导数 $\frac{\partial f}{\partial x}$ 和 $\frac{\partial f}{\partial y}$ ，然后将两个偏导数放到一个行向量中，即是一个雅可比（Jacobian），简记为 $J$ 。通过不同 $x ， y$ 参数所组成的向量在空间中的长度（模）和方向，可以理解不同参数对应结果的变化关系
梯度（gradient）为 0 的地方可能有三个
- 顶点（Maximum Point）、谷点（Minimum Point）、鞍点（Saddle Point）

在这里插入图片描述

1.2.2 雅可比矩阵

是多个多元函数的一阶偏导数以一定方式排列形成的矩阵，其体现了可微方程与给出点的最优线性逼近。可以理解为多元函数对各个变量的导数
设 $f:\mathbb{R}_n\to\mathbb{R}_m$ 是一个函数，它的输入是向量 $\mathbf{x}\in\mathbb{R}_n$ ，输出是向量 $\mathbf{y}=f(\mathbf{x})\in\mathbb{R}_m$
$\begin{cases}y_1=f_1\left(x_1,\ldots,x_n\right)\\ y_2=f_2\left(x_1,\ldots,x_n\right)\\ \ldots\\ y_m=f_m\left(x_1,\ldots,x_n\right)\end{cases}$
则雅可比矩阵是一个 $m \times n$ 矩阵
$\mathbf{J}=\begin{bmatrix}\frac{\partial f}{\partial x_1}&\cdots&\frac{\partial f}{\partial x_n}\end{bmatrix}=\begin{bmatrix}\frac{\partial f_1}{\partial x_1}&\cdots&\frac{\partial f_1}{\partial x_n}\\ \vdots&\ddots&\vdots\\ \frac{\partial f_m}{\partial x_1}&\cdots&\frac{\partial f_m}{\partial x_n}\end{bmatrix}$
例如
$\begin{matrix}u(x,y)&=x-2y\\ \text{v}(x,y)&=3y-2x\end{matrix}$
- 自变量为 $x 、 y$ ，因变量为 $u 、 v$ ，则雅可比矩阵为两个雅克比向量的合体

$J_u=\begin{bmatrix}\partial u/\partial x&\partial u/\partial y\end{bmatrix}$

$J_v=\begin{bmatrix}\partial v/\partial x&\partial v/\partial y\end{bmatrix}$

$J=\begin{bmatrix}\partial u/\partial x&\partial u/\partial y\\ \partial v/\partial x&\partial v/\partial y\end{bmatrix}$

在这里插入图片描述

1.2.3 雅可比寻找极值

当函数进入极大值或极小值时，其梯度会比较平缓，接近或等于 0。当极值只有一个时，找到对应的 $x ， y$ 位置比较容易
当函数比较复杂时，极值可能有多个，会有全局最高的山顶、全局最低的山谷、局部山顶和局部的山谷。这些地方的梯度也都是 0。如果将 $x ， y$ 等于 0，则无法确定是哪个极值
如果任意选择一点，无法知道最高峰和最低谷的位置。但是可以知道其最近的较高点和较低点。雅克比 Vector 的方向指向了最近的较高点，其模表述了幅度。即可以高效地找到局部的较高和较低点，但不能找到全局的最高点和最低点

求函数 $f(x,y,z)=x^2z+5x-2y^3$ 的雅可比向量？

$J=[2zx+5,-6y^2,x^2]$

1.3 黑塞矩阵

黑塞矩阵（Hessian matrix，又称海森矩阵）是一个自变量为向量的实值函数的二阶偏导数组成的方阵。即可以理解为对 Jacobian Vector 再次进行求导（把每一项偏导项再做一次求导）其矩阵是对称矩阵。
那么如何在雅可比 $J$ 处于极值情况下，判断这个极值是最大值、最小值、还是鞍值？这就需要使用黑塞矩阵了，对于二次函数，黑塞矩阵为常数矩阵，与参数无关，令 $J = 0$
- 当 $H$ 为正定矩阵时，此时函数对应极小值
- 当 $H$ 为负定矩阵时，此时函数对应极大值
- 当 $H$ 为不定矩阵时，此时函数无极值点
- 当 $H$ 为半正定或半负定矩阵时，无法确定极值点

如果一个多维函数如下
$y=f\left(x_1,\ldots,x_n\right)$
假如此函数的所有二阶导数都存在，则其黑塞矩阵为
$H(f)_{ij}(x)=D_i D_j f(x)$
- 其一阶偏导为向量，称作梯度（gradient）， $x=x_1,\ldots,x_n$
  $\nabla f(\boldsymbol{x})=\begin{bmatrix}\cfrac{\partial f}{\partial x_1}\\ \cfrac{\partial f}{\partial x_2}\\ \vdots\\ \cfrac{\partial f}{\partial x_n}\end{bmatrix}$
$\delta$ 即为偏导数（partial），按照如下形式求每一项的二阶偏导
$\nabla\left(\frac{\partial f}{\partial x_i}\right)=\begin{bmatrix}\frac{\partial}{\partial x_1}\frac{\partial f}{\partial x_i}\quad\frac{\partial}{\partial x_2}\frac{\partial f}{\partial x_i}\quad\cdots\quad\frac{\partial}{\partial x_n}\frac{\partial f}{\partial x_i}\end{bmatrix}$
则可得 $f (x)$ 的二阶偏导
$H(x)=\nabla^2f(x)=\begin{bmatrix}\frac{\partial}{\partial x_1}\frac{\partial f}{\partial x_1}&\frac{\partial}{\partial x_2}\frac{\partial f}{\partial x_1}&\cdots&\frac{\partial}{\partial x_n}\frac{\partial f}{\partial x_1}\\ \frac{\partial}{\partial x_1}\frac{\partial f}{\partial x_2}&\frac{\partial}{\partial x_2}\frac{\partial f}{\partial x_2}&\cdots&\frac{\partial}{\partial x_n}\frac{\partial f}{\partial x_2}\\ \vdots&\vdots&\ddots&\vdots\\ \frac{\partial}{\partial x_1}\frac{\partial f}{\partial x_n}&\frac{\partial}{\partial x_2}\frac{\partial f}{\partial x_n}&\cdots&\frac{\partial}{\partial x_n}\frac{\partial f}{\partial x_n}\end{bmatrix}$
即 $H (x)$ 为方阵
$\begin{bmatrix}\frac{\partial^2f}{\partial x_1^2}&\frac{\partial^3f}{\partial x_1\partial x_2}&\cdots&\frac{\partial^2f}{\partial x_1\partial x_n}\\ \frac{\partial x_1^2}{\partial x_2\partial x_1}&\frac{\partial^3f}{\partial x_2^2}&\cdots&\frac{\partial^2f}{\partial x_2\partial x_n}\\ \vdots&\vdots&\ddots&\vdots\\ \frac{\partial^3f}{\partial x_n\partial x_1}&\frac{\partial^2f}{\partial x_n\partial x_2}&\cdots&\frac{\partial^2f}{\partial x_n^2}\end{bmatrix}$
黑塞矩阵的对称性
$\boldsymbol H_{ij}=\frac{\partial}{\partial x_j}\frac{\partial f}{\partial x_i}=\frac{\partial}{\partial x_i}\frac{\partial f}{\partial x_j}=\boldsymbol H_{ji}$

求函数 $f(x,y)=-x^2-4y^2+3x+9y-3xy$ 的黑塞矩阵

$\nabla f(x,y)=\begin{bmatrix}\frac{\partial f}{\partial x}\\ \frac{\partial f}{\partial y}\end{bmatrix}=\begin{bmatrix}-2x+3-3y\\ -8y+9-3x\end{bmatrix}$
$H(x)=\nabla^2f(x)=\begin{bmatrix}\frac{\partial}{\partial x}\frac{\partial f}{\partial x}&\frac{\partial}{\partial y}\frac{\partial f}{\partial x}\\ \frac{\partial}{\partial x}\frac{\partial f}{\partial y}&\frac{\partial}{\partial y}\frac{\partial f}{\partial y}\end{bmatrix}=\begin{bmatrix}\frac{\partial(-2x+3-3y)}{\partial x}&\frac{\partial(-2x+3-3y)}{\partial y}\\ \frac{\partial(-8y+9-3x)}{\partial x}&\frac{\partial(-8y+9-3x)}{\partial y}\end{bmatrix}=\begin{bmatrix}-2&-3\\ -3&-8\end{bmatrix}$

2. 点云配准原理

2.1 点云配准需求场景

随着计算机辅助设计技术的发展，通过实物模型产生数字模型的逆向工程技术获得了越来越广泛的应用，与此同时，硬件设备的日趋完善也为数字模型操作提供了足够的技术支持

由于三维扫描仪设备受到测量方式和被测物体形状的条件限制，一次扫描往往只能获取到局部的点云信息，进而需要进行多次扫描，然后每次扫描时得到的点云都有独立的坐标系，不可以直接进行拼接。在逆向工程、计算机视觉、文物数字化等领域中，由于点云的不完整、旋转错位、平移错位等，使得要得到完整点云就需要对多个局部点云进行配准。为了得到被测物体的完整数据模型,需要确定一个合适的坐标变换，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以方便地进行可视化等操作，这就是点云数据的配准

2.2 点云配准方法

点云配准步骤上可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两个阶段

粗配准是指在点云相对位姿完全未知的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。常见粗配准算法
- 基于特征匹配(PFH)的配准算法
  - SAC-IA 采样一致性初始配准算法（Sample - Consensus Initial Alignment）PCL库已实现，基于FPFH
- 基于穷举搜索的配准算法
  - 4PCS 四点一致集配准算法（4-Point Congruent Set）
  - Super4PCS
精配准是指在粗配准的基础上，让点云之间的空间位置差异最小化，得到一个更加精准的旋转平移变换矩阵。该算法的运行速度以及向全局最优化的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。所以需要各种粗配准技术为ICP算法提供较好的位置，在迭代过程中确立正确对应点集能避免迭代陷入局部极值，决定了算法的收敛速度和最终的配准精度。最常见的精配准算法是ICP及其变种。
- ICP 迭代最近点算法（Iterative Cloest Point）
  - GICP
  - NICP
  - MBICP
- NDT 正态分布变换算法（Normal Distributions Transform）

其他配准

依赖平台设备：将被测物体放在平台上，利用控制器对平台进行控制，使之按照指定角度转动，通过多次测量可以得到不同视角下的点云，由于提前获知了距离及角度信息，则可以直接对所有点云进行配准
辅助标志点：通过在被测物体表面粘贴标签，将这些标签作为标志点，对多次测量得到的点云数据进行配准时，对这些有显著特征的标签进行识别配准，代替了对整体点云的配准，提高效率，精确度

2.3 自动配准技术

通常所说的点云配准就是指自动配准，点云自动配准技术是通过一定的算法或者统计学规律，利用计算机计算两块点云之间的错位，从而达到把两片点云自动配准的效果。本质上就是把不同坐标系中测量得到的数据点云进行坐标变换，从而得到整体的数据模型
即求得坐标变换参数 R（旋转矩阵）和 T（平移向量）,使得两视角下测得的三维数据经坐标变换后的距离最小，配准算法按照实现过程可以分为整体配准和局部配准

2.4 PCL 实现的配准算法

PCL 提供的配准库算法是通过在给定的输入数据集中找到正确的点对应关系，并将每个单独的数据集转换为一致的全局坐标系的刚性变换。理想情况下，如果在输入数据集中完全知道点对应关系，则该配准范式可以轻松解决

2.4.1 两两配准

称一对点云数据集的配准问题为两两配准（pairwise registration）。通常，通过应用一个估算得到的表示平移和旋转的 4 × 4 刚体变换矩阵来使一个点云数据集精确地与另一个点云数据集(目标数据集)进行完美配准

在这里插入图片描述

具体实现步骤如下
1. 首先从两个数据集中按照同样的关键点选取标准，提取关键点
2. 对选择的所有关键点分别计算其特征描述子
3. 结合特征描述子在两个数据集中的坐标的位置，以两者之间特征和位置的相似度为基础，估算它们的对应关系，初步估计对应点对
4. 假定数据是有噪声的，除去对配准有影响的错误的对应点对
5. 利用剩余的正确对应关系来估算刚体变换，完成配准
整个配准过程最重要的是关键点的提取以及关键点的特征描述，以确保对应估计的准确性和效率，这样才能保证后续流程中的刚体变换矩阵估计的无误性。接下来对单次迭代的每一步进行解读：
- （1）关键点提取
  - 关键点是在场景中具有“特殊属性”的兴趣点，例如书的一角或书上写有“ PCL”的字母“ P”。 PCL中有许多不同的关键点提取技术，如NARF，SIFT和FAST。另外，您也可以将每个点或子集作为关键点。如果不进行关键点提取，直接“将两个kinect数据集执行对应估计”会产生的问题是：每帧中有300k点，因此可以有300k^2个对应关系，这个数量太庞大不利于计算
- （2）特征描述符
  - 基于找到的关键点，我们必须提取特征，在此我们封装解析点云数据并生成向量以相互比较。同样，有许多特征描述符提取技术可供选择，例如NARF，FPFH，BRIEF或SIFT。
- （3）对应关系估计
  - 假设已经得到由两次扫描的点云数据获得的两组特征向量，在此基础上，我们必须找到相似特征再确定数据的重叠部分才能进行配准。根据特征的类型，PCL 使用不同方法来搜索特征之间的对应关系
  - 进行点匹配时（使用点的 xyz 三维坐标作为特征值），针对有序点云数据和无序点云数据有不同的处理策略
    - 穷举配准（brute force matching）简称BFMatching，或称野蛮匹配
    - kd-tree最近邻查询，Fast Library for Approximate Nearest Neighbors.( FLANN )
    - 在有序点云数据的图像空间中查找
    - 在无序点云数据的索引空间中查找
  - 进行特征匹配时，有以下几种方法(不使用点的坐标，而是某些由查询点邻域确定的特征，如法向量、局部或全局形状直方图等)
    - 穷举配准( brute force matching）简称BFMatching，或称野蛮匹配
    - kd-tree最近邻查询，Fast Library for Approximate Nearest Neighbors.( FLANN )
  - 除了查询之外，对应估计也区分了两种类型
    - 直接对应估计(默认)：为点云 A 中的每一个点搜索点云 B 中的对应点,确认最终对应点对
    - “相互”( Reciprocal )对应估计：首先为点云 A 中的点到点云 B 搜索对应点，然后又从点云 B 到点云 A 搜索对应点，最后只取交集作为对应点对
- （4）对应关系去除
  - 由于噪声的影响，通常并不是所有估计的对应关系都是正确的。由于错误的对应关系对于最终的刚体变换矩阵的估算会产生负面的影响，所以必须去除它们，于是我们使用随机采样一致性( Random Sample Consensus , RANSAC )估算或者其他方法剔除错误对应关系，最终只保留一定比例的对应关系，这样即能提高变换矩阵的估算精度也可以提高配准速度。遇到有一对多对应关系的特例情况，即目标模型中的一个点在源中有若干个点与之对应。可以通过只取与其距离最近的对应点或者检查附近的其他匹配的滤波方法过滤掉其他伪对应关系。同样地针对对应关系的去除，PCL 有单独设计类与之对应。
- （5）变换矩阵估算
- 最后一步就是计算实际的转换关系
  - 根据对应关系评估误差值
  - 估算相机位姿之间的刚性变换，并最小化误差指标
  - 优化点的结构
  - 使用刚性变换将源旋转/变换到目标上
  - 迭代此过程直到满足预定的收敛标准

3. 点云配准示例

3.1 迭代最近点（ICP）

iterative_closest_point.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int main(int argc, char **argv) {
    // 定义输入和输出点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

    // 随机填充无序点云
    cloud_in->width = 5;
    cloud_in->height = 1;
    cloud_in->is_dense = false;
    cloud_in->points.resize(cloud_in->width * cloud_in->height);
    for (size_t i = 0; i < cloud_in->points.size(); ++i) {
        cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    std::cout << "Saved " << cloud_in->points.size() << " data points to input:" << std::endl;
    for (size_t i = 0; i < cloud_in->points.size(); ++i) {
        std::cout << "    " << cloud_in->points[i].x 
                  << " " << cloud_in->points[i].y
                  << " " << cloud_in->points[i].z << std::endl;
    }
    *cloud_out = *cloud_in;
    std::cout << "size:" << cloud_out->points.size() << std::endl;

    // 在点云上执行简单的刚性变换，将 cloud_out 中的 x 平移 0.7m，然后再次输出数据值
    for (size_t i = 0; i < cloud_in->points.size(); ++i) {
        cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
    }
    // 打印这些点
    std::cout << "Transformed " << cloud_in->points.size() << " data points:" << std::endl;
    for (size_t i = 0; i < cloud_out->points.size(); ++i) {
        std::cout << "    " << cloud_out->points[i].x 
                  << " " << cloud_out->points[i].y
                  << " " << cloud_out->points[i].z << std::endl;
    }
    
    // 创建 IterativeClosestPoint 的实例
        // setInputSource 将 cloud_in 作为输入点云
        // setInputTarget 将平移后的 cloud_out 作为目标点云
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);

    // 创建一个 pcl::PointCloud<pcl::PointXYZ>实例 Final 对象，存储配准变换后的源点云
    // 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集，如果这两个点云匹配正确的话
    // （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云），那么 icp.hasConverged()= 1 (true)
    // 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);
    std::cout << "has converged: " << icp.hasConverged() << std::cout 
              << "score: " << icp.getFitnessScore() << std::endl;
                           // 输入点云，目标点云，计算精度
    const pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix = icp.getFinalTransformation();
    std::cout << matrix << std::endl;
    return (0);
}

配置文件 CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(iterative_closest_point)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (iterative_closest_point iterative_closest_point.cpp)
target_link_libraries (iterative_closest_point ${PCL_LIBRARIES})

编译并执行

$ mkdir build
$ cd build
$ cmake ..
$ make

$ ./iterative_closest_point

# 结果输出
Saved 5 data points to input:
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762
size:5
Transformed 5 data points:
    1.05222 -0.151883 -0.106395
    0.302594 -0.473106 0.292602
    -0.0318983 0.667105 0.441304
    -0.0347655 0.854581 -0.0361733
    0.2393 -0.277468 -0.916762
has converged: 1
score: 1.1956e-13
           1 -2.30968e-07 -2.98023e-08          0.7
 -2.1793e-07            1 -7.82311e-08  -1.2368e-07
 7.45058e-08  4.09782e-08            1   3.8743e-08
           0            0            0            1

3.2 正态分布变换（NDT）

用正态分布变换算法来确定两个大型点云（都超过 100 000个点）之间的刚体变换。正态分布变换算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快

normal_distributions_transform.cpp

#include <iostream>
#include <thread>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>    // ndt 配准头文件
#include <pcl/filters/approximate_voxel_grid.h>    // 滤波头文件
#include <pcl/visualization/pcl_visualizer.h>    // 可视化头文件

using namespace std::chrono_literals;

int main(int argc, char **argv) {
    // 加载第一个房间点云扫描
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("./data/room_scan1.pcd", *target_cloud) == -1) {
        PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << target_cloud->size() << " data points from room_scan1.pcd" << std::endl;

    // 从新的视角加载第二个房间点云扫描
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("./data/room_scan2.pcd", *input_cloud) == -1) {
        PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << input_cloud->size() << " data points from room_scan2.pcd" << std::endl;

    // 过滤 输入的点云扫描 至原始尺寸的 10% 左右 以提高配准速度
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    // 体素是在三维空间中划分的小立方体单元，这里的体素大小决定了滤波后点云的稠密程度
    approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);  // 设置其体素大小为 (0.2, 0.2, 0.2)
    approximate_voxel_filter.setInputCloud(input_cloud);
    approximate_voxel_filter.filter(*filtered_cloud);
    std::cout << "Filtered cloud contains " << filtered_cloud->size()
              << " data points from room_scan2.pcd" << std::endl;

    // NDT 初始化
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    // 设置变换矩阵收敛条件的最小欧几里得距离
    // 即当两次迭代之间的变换矩阵的欧几里得距离小于该值时，认为已经收敛并停止迭代
    ndt.setTransformationEpsilon(0.01);
    // 设置匹配过程中采样点的步长，即在搜索空间中沿着某个方向移动的距离
    ndt.setStepSize(0.1);
    // 设置匹配过程中体素格子的大小，即将点云离散化成体素格子，并在每个格子中计算一个高斯分布模型
    ndt.setResolution(1.0);
    // 设置最大迭代次数，即在没有达到收敛条件前最多允许迭代的次数
    ndt.setMaximumIterations(35);
    // 设置输入点云（为滤波后的点云）
    ndt.setInputSource(filtered_cloud);
    // 设置目标点云
    ndt.setInputTarget(target_cloud);

    /*
    创建一个初始变换矩阵 init_guess，作为 NDT 算法的初始猜测值，以帮助 NDT 算法尽快收敛加速配准过程
        1. 首先，通过 Eigen::AngleAxisf 类创建一个表示绕 Z 轴旋转 0.6931弧度（逆时针 40度）的旋转矩阵
        2. 然后，通过 Eigen::Translation3f 类创建一个表示在 X 轴上平移 1.79387 个单位的平移矩阵
        3. 最后，使用矩阵乘法将旋转矩阵和平移矩阵组合成一个 4x4 的初始变换矩阵 init_guess
                                            （其中 .matrix() 将结果转换为 Eigen 的矩阵类型）
    */
    Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translation(1.79387, 0, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

    // 使用 NDT 算法将输入点云与目标点云对齐，并将结果存储在 output_cloud 对象中
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align(*output_cloud, init_guess);
    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged() // 判断算法是否收敛
              << " score: " << ndt.getFitnessScore() << std::endl; // 将输入点云与目标点云对齐后的匹配度评分

    // 对输入点云对象进行刚体变换，并将结果存储到输出点云对象中
        // 要进行变换的输入点云对象，存储变换结果的输出点云对象，所采用的刚体变换矩阵
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());

    // 保存转换后的输入点云
    pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud);

    // 点云可视化器 初始化
    pcl::visualization::PCLVisualizer::Ptr
            viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer_final->setBackgroundColor(0, 0, 0);

    // 着色并可视化 目标点云 (红色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
    // 着色并可视化 转换后的输入点云 (绿色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");
    
    viewer_final->addCoordinateSystem(1.0, "global");
    viewer_final->initCameraParameters();

    while (!viewer_final->wasStopped()) {
        viewer_final->spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }
    return (0);
}

配置文件 CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(normal_distributions_transform)

set(CMAKE_CXX_STANDARD 14) # std::chrono_literals

find_package(PCL 1.5 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(normal_distributions_transform normal_distributions_transform.cpp)
target_link_libraries (normal_distributions_transform ${PCL_LIBRARIES})

编译并执行

$ mkdir build
$ cd build
$ cmake ..
$ make

$ ./normal_distributions_transform

# 输出结果
Loaded 112586 data points from room_scan1.pcd
Loaded 112624 data points from room_scan2.pcd
Filtered cloud contains 12433 data points from room_scan2.pcd
Normal Distributions Transform has converged: 1
score: 0.641618

在这里插入图片描述

3.3 交互式 ICP

加载点云并对其施加刚性变换。之后，ICP 算法会将变换后的点云与原始对齐。每次用户按下“空格”，都会进行一次 ICP 迭代，并刷新查看器

interactive_icp.cpp

#include <string>

#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool next_iteration = false;

void print4x4Matrix(const Eigen::Matrix4d &matrix) {
    // 按行打印旋转矩阵的三个行向量。每个行向量由三个浮点数表示，使用 %6.3f 格式进行格式化以保留小数点后三位
    printf("Rotation matrix :\n");
    printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
    printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
    printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
    // 打印平移向量。平移向量包含三个浮点数，分别代表 x、y 和 z 轴的平移量
    printf("Translation vector :\n");
    printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}

// 回调函数，用于处理 PCL 可视化窗口中的键盘事件（只要按任意键，就会调用此函数）
// 参数 void *nothing 实际上没有被使用，这里仅是占位符，因为该回调函数不需要传递任何数据
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *nothing) {
    // 函数还检查事件是否为按下事件（而不是释放事件），以避免重复触发事件
    if (event.getKeySym() == "space" && event.keyDown()) {
        next_iteration = true;    // 当用户按下空格键时，程序执行下一次迭代
    }
}

int main(int argc, char *argv[]) {
    PointCloudT::Ptr cloud_in(new PointCloudT);    // 原始输入点云
    PointCloudT::Ptr cloud_tr(new PointCloudT);    // 转换后的点云
    PointCloudT::Ptr cloud_icp(new PointCloudT);    // ICP 输出点云

    // 解析命令行参数
        // 若命令行参数的数量大于等于 2，则将第二个参数（即 argv[1]）解析为文件名
        // 并将第三个参数（如果存在）解析为 ICP 算法的迭代次数
    if (argc < 2) {
        printf ("Usage :\n");
        printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);
        PCL_ERROR ("Provide one ply file.\n");
        return (-1);
    }
    int iterations = 1;    // 初始化 ICP 迭代次数
    if (argc > 2) {
        iterations = atoi (argv[2]);
        if (iterations < 1) {
            PCL_ERROR ("Number of initial iterations must be >= 1\n");
            return (-1);
        }
    }

    // 读取 .PLY 文件并计算加载时间
    pcl::console::TicToc time;
    time.tic();  // 开始计时
    if (pcl::io::loadPLYFile(argv[1], *cloud_in) < 0) {  // 加载文件，并将第一个命令行参数（即argv[1]）作为文件名
        PCL_ERROR("Error loading cloud %s.\n", argv[1]);
        return (-1);
    }
    // 输出已加载文件的名称以及其中包含的点数，还有读取该文件所需的时间
    std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size() << " points) in "
              << time.toc() << " ms\n" << std::endl;    // 调用 toc() 方法来计算时间差
    
    // 定义旋转矩阵和平移向量
    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
        // 定义旋转矩阵
    double theta = M_PI / 8;
    transformation_matrix(0, 0) = std::cos(theta);
    transformation_matrix(0, 1) = -sin(theta);
    transformation_matrix(1, 0) = sin(theta);
    transformation_matrix(1, 1) = std::cos(theta);
        // 定义平移向量（绕 z 轴）
    transformation_matrix(2, 3) = 0.4;
    // 在终端显示转换矩阵
    std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
    print4x4Matrix(transformation_matrix);
    // 执行点云转换
    pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);
    *cloud_tr = *cloud_icp;    // 将 cloud_icp 备份到 cloud_tr 以供以后使用

    // ICP 算法部分
    time.tic();
    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setMaximumIterations(iterations);  // 设置 ICP 算法最大迭代次数，这个参数控制了 ICP 算法的收敛速度
    icp.setInputSource(cloud_icp);  // 输入点云
    icp.setInputTarget(cloud_in);   // 目标点云
    icp.align(*cloud_icp);
    icp.setMaximumIterations(1);  // 将最大迭代次数设置为 1，避免重复执行 ICP 操作
    std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;

    if (icp.hasConverged()) {
        std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
        std::cout << "\nICP transformation " << iterations << " : cloud_icp-> cloud_in" << std::endl;
        // 获取最终的变换矩阵，并将它转换成双精度类型，最后调用 print4x4Matrix() 函数打印变换矩阵
        transformation_matrix = icp.getFinalTransformation().cast<double>();  
        print4x4Matrix(transformation_matrix);
    } else {
        PCL_ERROR ("\nICP has not converged.\n");
        return (-1);
    }

    // 可视化操作
    pcl::visualization::PCLVisualizer viewer("ICP demo");
    // 创建 左右 两个窗口分别显示点云：左侧显示原始点云，右侧显示配准后的点云，以便进行比较和评估
    int v1(0);
    int v2(1);
    // 前两个参数 0.0, 0.0 表示视口的左下角坐标（相对于整个窗口）
    // 后两个参数 0.5, 1.0 表示视口的宽度和高度占整个窗口的比例
    viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);  // 一个宽度为窗口一半、高度为整个窗口的视口，位于窗口的左半部分
    viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);  // 一个宽度为窗口一半、高度为整个窗口的视口，位于窗口的右半部分

    float bckgr_gray_level = 0.0;
    float txt_gray_lvl = 1.0 - bckgr_gray_level;

    // 设置原始点云颜色（白色）
    pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, (int)255 * txt_gray_lvl, 
                                                                             (int)255 * txt_gray_lvl, 
                                                                             (int)255 * txt_gray_lvl);
    // 左右两个窗口均显示
    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

    // 设置变换后的点云颜色（绿色）
    pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 20, 180, 20);
    // 只在左边窗口显示
    viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

    // 设置 ICP 配准的点云颜色（红色）
    pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 180, 20, 20);
    // 只在右边窗口显示
    viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

    // 在每个窗口添加文字描述
    viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
    viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

    std::stringstream ss;
    ss << iterations;  // 将 iterations 的值写入到字符串流中，从而将其转换成字符串形式
    // 将变量值转换成字符串形式，便于输出和显示
        // 使用 ss.str() 获取字符串流中的（iterations）字符串表示，再将其与 "ICP iterations = " 字符串拼接
        // 得到一个新的字符串 iterations_cnt，用于输出到可视化窗口（右边窗口）中
    std::string iterations_cnt = "ICP iterations = " + ss.str();
    // iterations_cnt 是要显示的文本内容，10 和 60 是文本框左上角的 x 和 y 坐标，16 是文本字体大小
    // txt_gray_lvl 是文本颜色的 RGB 值，"iterations_cnt" 是文本框的名称，v2 是显示的窗口编号
    viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

    viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
    viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
    // 设置相机位置和方位
    viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
    viewer.setSize(1280, 1024);  // 可视化窗口大小
    // 通过注册键盘事件回调函数，视图对象就可以监听用户的键盘操作，并根据用户的输入做出相应的响应
        // 1. &keyboardEventOccurred：表示要注册的键盘事件回调函数的指针，这个函数会在用户按下或释放键盘上的某个按键时被调用
        // 2. (void*)NULL：传递给回调函数，可以是任何类型的指针。这里设置为 NULL，表示不需要传递任何其他信息给回调函数
    viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

    while (!viewer.wasStopped()) {
        viewer.spinOnce();
        // 如果用户按下键盘上的任意键，则会调用 keyboardEventOccurred 函数。此功能检查键是否为 “空格”
        // 如果是，则全局布尔值 next_iteration 设置为 true，从而允许查看器循环输入代码的下一部分：调用 ICP 对象以进行对齐
        if (next_iteration) {    
            time.tic();
            icp.align(*cloud_icp);
            std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;
            // 如果 ICP 已经收敛，则将变换矩阵（transformation_matrix）更新为 ICP 的最终变换矩阵
            // 并在屏幕上输出 ICP 迭代次数和变换矩阵。同时，将更新后的点云显示在可视化视图中
            if (icp.hasConverged()) {
                printf ("\033[11A");  // 在终端增加 11 行以覆盖显示的最后一个矩阵
                printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
                // 输出当前 ICP 迭代的变换矩阵，并且将迭代次数加 1
                std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
                // 将最终的 ICP 变换矩阵乘到之前的变换矩阵中并显示
                transformation_matrix *= icp.getFinalTransformation().cast<double>();
                print4x4Matrix(transformation_matrix);

                ss.str("");  // 清空 stringstream 字符串流
                ss << iterations;  // 将当前迭代次数转化为字符串形式
                std::string iterations_cnt = "ICP iterations = " + ss.str();
                // 更新视窗中的文本信息，将 "iterations_cnt" 这个 id 指向新的字符串
                viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
                viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");  // 更新视窗中（右边）显示的点云数据
            } else {
                PCL_ERROR ("\nICP has not converged.\n");
                return (-1);
            }
        }
        next_iteration = false;
    }
    return 0;
}

配置文件 CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(interactive_icp)

set(CMAKE_CXX_STANDARD 14) # std::chrono_literals

find_package(PCL 1.5 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(interactive_icp interactive_icp.cpp)
target_link_libraries (interactive_icp ${PCL_LIBRARIES})

编译并执行

$ mkdir build
$ cd build
$ cmake ..
$ make

$ ./interactive_icp ../monkey.ply 1

# 输出结果：通过按 “空格” 进行多次迭代
# 如果 ICP 表现出色，则两个矩阵的值应完全相同，并且 ICP 找到的矩阵的对角线外的符号应相反
    |  0.924 -0.383  0.000 |
R = |  0.383  0.924  0.000 |
    |  0.000  0.000  1.000 |
Translation vector :
t = <  0.000,  0.000,  0.400 >

    |  0.924  0.383  0.000 |
R = | -0.383  0.924  0.000 |
    |  0.000  0.000  1.000 |
Translation vector :
t = <  0.000,  0.000, -0.400 >

在这里插入图片描述

3.4 刚体鲁棒姿态估计

展示如何在具有杂波和遮挡的场景中找到刚体的对齐姿势

alignment_prerejective.cpp

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

// 首先从定义类型开始，以免使代码混乱
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

// Align a rigid object to a scene with clutter and occlusions
int main (int argc, char **argv) {
    // 然后，实例化必要的数据容器，检查输入参数并加载对象和场景点云
    PointCloudT::Ptr object(new PointCloudT);
    PointCloudT::Ptr object_aligned(new PointCloudT);
    PointCloudT::Ptr scene(new PointCloudT);
    FeatureCloudT::Ptr object_features(new FeatureCloudT);
    FeatureCloudT::Ptr scene_features(new FeatureCloudT);
    
    // Get input object and scene
    if (argc != 3) {
        pcl::console::print_error("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
        return (1);
    }
    
    // Load object and scene
    pcl::console::print_highlight("Loading point clouds...\n");
    if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
        pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0) {
        pcl::console::print_error("Error loading object/scene file!\n");
        return (1);
    }
    
    // 为加快处理速度，使用 pcl::VoxelGrid 类将对象和场景点云的采样率下采样至 5 mm
    pcl::console::print_highlight("Downsampling...\n");
    pcl::VoxelGrid<PointNT> grid;
    const float leaf = 0.005f;
    grid.setLeafSize(leaf, leaf, leaf);
    grid.setInputCloud(object);
    grid.filter(*object);
    grid.setInputCloud(scene);
    grid.filter(*scene);
    
    // Estimate normals for scene
    pcl::console::print_highlight("Estimating scene normals...\n");
    pcl::NormalEstimationOMP<PointNT,PointNT> nest;
    nest.setRadiusSearch(0.01);
    nest.setInputCloud(scene);
    nest.compute(*scene);
    
    // Estimate features
    // 对于下采样点云中的每个点，我们现在使用PCL的pcl::FPFHEstimationOMP<>类来计算用于对齐过程中用于匹配的快速点特征直方图（FPFH）描述符。
    pcl::console::print_highlight("Estimating features...\n");
    FeatureEstimationT fest;
    fest.setRadiusSearch(0.025);
    fest.setInputCloud(object);
    fest.setInputNormals(object);
    fest.compute(*object_features);
    fest.setInputCloud(scene);
    fest.setInputNormals(scene);
    fest.compute(*scene_features);
    
    // Perform alignment
    // SampleConsensusPrerejective 实现了有效的 RANSAC 姿势估计循环
    pcl::console::print_highlight("Starting alignment...\n");
    pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
    align.setInputSource(object);
    align.setSourceFeatures(object_features);
    align.setInputTarget(scene);
    align.setTargetFeatures(scene_features);
    align.setMaximumIterations(50000); // Number of RANSAC iterations
    align.setNumberOfSamples(3); // Number of points to sample for generating/prerejecting a pose
    align.setCorrespondenceRandomness(5); // Number of nearest features to use
    align.setSimilarityThreshold(0.9f); // Polygonal edge length similarity threshold
    align.setMaxCorrespondenceDistance(2.5f * leaf); // Inlier threshold
    align.setInlierFraction(0.25f); // Required inlier fraction for accepting a pose hypothesis
    {
        pcl::ScopeTime t("Alignment");
        align.align(*object_aligned);
    }
    
    if (align.hasConverged()) {
    // Print results
    printf("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation();
    pcl::console::print_info("    | %6.3f %6.3f %6.3f | \n", transformation(0,0), transformation(0,1), transformation(0,2));
    pcl::console::print_info("R = | %6.3f %6.3f %6.3f | \n", transformation(1,0), transformation(1,1), transformation(1,2));
    pcl::console::print_info("    | %6.3f %6.3f %6.3f | \n", transformation(2,0), transformation(2,1), transformation(2,2));
    pcl::console::print_info("\n");
    pcl::console::print_info("t = < %0.3f, %0.3f, %0.3f >\n", transformation(0,3), transformation(1,3), transformation(2,3));
    pcl::console::print_info("\n");
    pcl::console::print_info("Inliers: %i/%i\n", align.getInliers().size(), object->size());
    
    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment");
    visu.addPointCloud(scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
    visu.addPointCloud(object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
    visu.spin();
    } else {
        pcl::console::print_error("Alignment failed!\n");
        return (1);
    }
    return (0);
}

配置文件 CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(alignment_prerejective)

find_package(PCL 1.7 REQUIRED COMPONENTS io registration segmentation visualization)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(alignment_prerejective alignment_prerejective.cpp)
target_link_libraries(alignment_prerejective ${PCL_LIBRARIES})

编译并执行

$ mkdir build
$ cd build
$ cmake ..
$ make

$ ./alignment_prerejective chef.pcd rs1.pcd

# 输出结果
Alignment took 352ms.
    |  0.040 -0.929 -0.369 |
R = | -0.999 -0.035 -0.020 |
    |  0.006  0.369 -0.929 |
t = < -0.120, 0.055, 0.076 >
Inliers: 1422/3432