三维点云处理之点云配准_点云匹配

1. ICP

（1）定义

​ 对于两个点云source和target，求source经位姿变换后得到的点云与target尽可能重合的旋转$R$与平移$t$。由于实际中source和target两个点云中的点不一定完全对应，因此取二者能够相互关联的那些点（公共点）来参与位姿计算：
X = { x 1 , x 2 , ⋯   , x N x }            Y = { y 1 , y 2 , ⋯   , y N y }        N x = N y X = \{ x_1,x_2,\cdots,x_{N_x} \} \ \ \ \ \ \ \ \ \ \ Y = \{ y_1, y_2, \cdots, y_{N_y} \} \ \ \ \ \ \ N_x=N_y X={x1​,x2​,⋯,xNx​​}          Y={y1​,y2​,⋯,yNy​​}      Nx​=Ny​
目标：通过最小化损失函数计算旋转矩阵$R$和平移向量$t$
$$minE(R,t)=min\frac{1}{N_y}\sum _{i=1}^{N_y}||x_i-R{y}_i-t|{|}^2$$
流程

​ 获取两个点云之间的关联点一般是以二者距离最近的两个点为相互关联点，但若两初始点云相距较远，此时通过距离最近找到的关联点不一定是真正的关联点。在具有较好的初始位姿前提下，可通过迭代的方式，将当前迭代中变换后的点作为下一迭代的起始点，直到两关联点云达到预期的尽可能重合状态（较好的初始位姿是为了使经过迭代后两点云相互靠近，否则即使迭代多次后两点云仍不会达到预期重合状态）。其流程如下所示：

（2）ICP公式推导

​ 根据定义可知，目标函数如下：
$$\begin{gathered} E(R,t)=\frac{1}{N_y}\sum _{i=1}^{N_y}||x_i-R{y}_i-t-u_x+R{u}_y+u_x-R{u}_y|{|}^2 \\ =\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)+(u_x-R{u}_y-t)|{|}^2) \\ =\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)|{|}^2+||u_x-R{u}_y-t|{|}^2 \\ +2(x_i-u_x-R(y_i-u_y){)}^T(u_x-R{u}_y-t)) \\ =\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)|{|}^2+||u_x-R{u}_y-t|{|}^2) \end{gathered}$$
​ 其中，$u_x$和$u_y$分别是点云X和Y的质心，即：
$$u_x=\frac{1}{N_x}\sum _{i=1}^{N_x}{x}_i{u}_y=\frac{1}{N_y}\sum _{i=1}^{N_y}{y}_i$$
​ 上述推导中去掉两点云的中心，其目的是： 中心到中心之间的距离就是两点云之间的平移，去中心之后两点云之间的约束只剩下旋转，这样可将平移和旋转解耦开。

​ 令$$E_1(R,t)=\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)|{|}^2$$$$E_2(R,t)=\frac{1}{N_y}\sum _{i=1}^{N_y}||u_x-R{u}_y-t|{|}^2$$

那么，$E(R,t)=E_1(R,t)+E_2(R,t)$。
在求解$minE(R,t)$的过程中，由于第一项$E_1(R,t)$只和$R$有关，因此只要根据$min{E}_1(R,t)$求出$R$，令第二项$E_2(R,t)$取最小值0，即可求出平移$t$。

求解旋转矩阵$R$

• 化简：将原始点云转换为去质心点云

​ 记两点云的去质心点云分别为：$$x_i^{,}=x_i-u_x$$$$y_i^{,}=y_i-u_y$$则
$$\begin{gathered} E_1(R,t)=\frac{1}{N_y}\sum _{i=1}^{N_y}||x_i-u_x-R(y_i-u_y)|{|}^2=\frac{1}{N_y}\sum _{i=1}^{N_y}||x_i^{,}-R{y}_i^{,}|{|}^2 \\ =\frac{1}{N_y}\sum _{i=1}^{N_y}(x_i^{,T}{x}_i^{,}+y_i^{,T}{R}^{T}R{y}_i^{,}-2x_i^{,T}R{y}_i^{,})=\frac{1}{N_y}\sum _{i=1}^{N_y}(x_i^{,T}{x}_i^{,}+y_i^{,T}{y}_i^{,}-2x_i^{,T}R{y}_i^{,}) \end{gathered}$$
​ 其中，只有第三项与$R$有关。
令$E_1^{,}(R,t)={\sum }_{i=1}^{N_y}{x}_i^{,T}R{y}_i^{,}$则$min{E}_1(R,t)$即$max{E}_1^{,}(R,t)$

• 求解

$$E_1^{,}(R,t)=\sum _{i=1}^{N_y}{x}_i^{,T}R{y}_i^{,}=\sum _{i=1}^{N_y}Trace(x_i^{,T}R{y}_i^{,})=\sum _{i=1}^{N_y}Trace(R{y}_i^{,}{x}_i^{,T})=Trace(RH)$$

• • 对于等式（1），由于$x_i^{,}$和$y_i^{,}$均为点的三维坐标，故均为$(3,1)$的列向量，相应地$x_i^{,T}$为$(1,3)$的行向量。又$R$为$(3,3)$的旋转矩阵，因此$x_i^{,T}R{y}_i^{,}$为$(1,1)$的常数，常数的迹等于它自身。
  • 对于等式（2）$$tr(AB)=\sum _{i=1}^m\sum _{j=1}^n{a}_{ij}{b}_{ji}=\sum _{i=1}^m\sum _{j=1}^n{b}_{ji}{a}_{ij}=tr(BA)$$
  • 对于等式（3），令$$H=\sum _{i=1}^{N_y}{y}_i^{,}{x}_i^{,T}$$

​ 因此，问题转换为寻找合适的$R$，使得$Trace(RH)$取得最大值

定理：若有正定矩阵$A{A}^T$，则对于任意正交矩阵$B$，有$$Trace(A{A}^T)\ge Trace(BA{A}^T)$$

意义：若能找到$R$，把$Trace(RH)$转换成$Trace(A{A}^T)$的形式，按照上述定理有：
$$Trace(RH)=Trace(A{A}^T)\ge Trace(BA{A}^T)$$
​ 此时$Trace(RH)$取得最大值，故该$R$就是我们要找的旋转矩阵

证明：
$$tr(BA{A}^T)=tr(A^{T}BA)=\sum _i{a}_i^T(B{a}_i)其中,a_i为A的列向量$$
​ 根据柯西-施瓦茨不等式（两向量乘积 小于等于 两向量模的乘积）
$$a_i^T(B{a}_i)\le \sqrt{(a_i^T{a}_i)(a_i^T{B}^{T}B{a}_i)}=a_i^T{a}_i$$
​ 因此，$$tr(BA{A}^T)=\sum _i{a}_i^T(B{a}_i)\le \sum _i{a}_i^T{a}_i=tr(A{A}^T)$$

问题转换：要寻找合适的$R$，使得$Trace(RH)$取得最大值。即要寻找合适的$R$，可将$Trace(RH)$转换成$Trace(A{A}^T)$的形式

具体求解：

​ 对$H$进行SVD分解，有$H=U\Sigma V^T$$。令$$R=V{U}^T$$，则有：
$$RH=V{U}^{T}U\Sigma V^T=V\Sigma V^T=V{\Sigma }^{\frac{1}{2}}{\Sigma }^{\frac{1}{2}}{V}^T=V{\Sigma }^{\frac{1}{2}}(V{\Sigma }^{\frac{1}{2}}{)}^T$$
​ 在确定旋转矩阵$R$后，将其代入目标函数第二项$E_2^{,}(R,t)$即可求出平移：$$t=u_x-R{u}_y$$

思考1：$E(R,t)$公式的第一步中对点云去质心的目的

​ 两点云之间的位姿变换包括旋转+平移，而平移就是两点云之间的质心距离。因此，对点云进行去质心操作后，剩下的就是旋转。也就是说，通过去质心的方式将旋转和平移解耦开了。

思考2：在求解$R$时，为什么要引入$Trace$

​ 主要是为了应用柯西-施瓦茨不等式，使得目标函数第一项能取得最小值，此时的$R$便为所要求的旋转矩阵。

（3）ICP的改进

数据点选取

• 随机选取点进行ICP操作，而不是对所有点都进行ICP操作，加快计算速度
• 使用Voxel Grid进行降采样，对降采样后的点进行ICP操作，同样是为了加快计算速度
• 对点云进行特征点提取，对提取到的特征点进行ICP操作，这样也可加快计算速度

数据关联

• 通过kd-tree/octree的最近邻查找，获取离当前点最近的点
• 通过特征描述子进行匹配，获取离当前点最近的点

剔除外点

• 对于某一匹配对pair，若其中的两点距离过大，则认为匹配的点是外点，需要去除
• 直接按照比例去除距离较大的匹配点

损失函数

• Point-to-Point：通过点到点之间的距离计算损失函数
• Point-to-Line：通过点到线之间的距离计算损失函数
• Point-to-Plane：通过点到面之间的距离计算损失函数

2. NDT

（1）定义

​ 对于两个点云source和target，将点云进行栅格划分。若已知target中栅格内点云的分布（$\mu ,\Sigma$），求两个点云之间的变换矩阵，使得将source中对应栅格内的点进行变换后的点的分布与target中栅格内点的分布近似。同ICP一样，初始的source与target可能相距较远，需要通过多次迭代才能实现转换后点的分布一致。

（2）求解过程

• 目标

2D模型下，需要求解沿x, y方向的平移和绕z方向的旋转，即 p = p 3 = [ t x t y ϕ z ] T p=p_3=\left[

$$\begin{array}{cc} t_x & t_y & \phi_z\end{array}$$ \right]^T p=p3​=[tx​​ty​​ϕz​​]T

3D模型下，需要求解沿x, y, z方向的平移和绕x, y, z方向的旋转，即 p = p 6 = [ t x t y t z ϕ x ϕ y ϕ z ] T p=p_6=\left[

$$\begin{array}{cc} t_x & t_y & t_z & \phi_x & \phi_y & \phi_z\end{array}$$ \right]^T p=p6​=[tx​​ty​​tz​​ϕx​​ϕy​​ϕz​​]T

• 求解target栅格内点的分布

​ 均值：$$\mu =\frac{1}{N_x}\sum _{i=1}^{N_x}{x}_i$$协方差：$$\Sigma =\frac{1}{N_x-1}\sum _{i=1}^{N_x}(x_i-\mu )(x_i-\mu {)}^T$$

• 根据预测的位姿，对点进行旋转和平移：$$y_i^{,}=T(p,y_i)=R{y}_i+t$$

• 计算二者的概率分布相似性

​ 旋转和平移后的点与目标点集中的点在同一坐标系下，此时**将变换后的点代入到目标点的分布中，求出它有多大程度满足目标点的分布。**若求出的概率越大，则表示二者越接近
$$f(X,y_i^{,})=\frac{1}{\sqrt{2\pi }|\Sigma |}exp(\frac{(y_i^{,}-\mu {)}^T{\Sigma }^{-1}(y_i^{,}-\mu )}{2})$$
​ 比较变换后的点与目标点之间的接近程度，另一种方法是：求出变换后的点的分布，然后在分布上比较二者之间的相似性。但这种方法的缺点是：每次迭代都需要求出变换后的点的分布，导致计算量较大。上面方法是直接将变换后的点代入目标点的分布中，以计算出变换后的点有多大程度符合目标点的分布。

对于所有点而言，将每个点代入目标点分布中进行相乘，得到对应的联合概率为：
$$\begin{gathered} \Psi =\prod _{i=1}^{N_y}f(X,T(p,y_i))=\prod _{i=1}^{N_y}\frac{1}{\sqrt{2\pi }|\Sigma |}exp(-\frac{(y_i^{,}-\mu {)}^T{\Sigma }^{-1}(y_i^{,}-\mu )}{2}) \\ \Rightarrow \\ ln\Psi =\sum _{i=1}^{N_y}(-\frac{(y_i^{,}-\mu {)}^T{\Sigma }^{-1}(y_i^{,}-\mu )}{2}+ln\frac{1}{\sqrt{2\pi }|\Sigma |}) \end{gathered}$$

• 目标函数
  $$\begin{gathered} max\Psi =maxln\Psi =min\sum _{i=1}^{N_y}(y_i^{,}-\mu {)}^T{\Sigma }^{-1}(y_i^{,}-\mu ) \\ {y}_i^{,}=T(p,y_i)=R{y}_i+t待求参数:R,t \end{gathered}$$
  经过以上分析，NDT问题可转化为目标函数的优化问题。将目标函数进行化简：

令：$$e_i(p)=y_i^{,}-\mu$$$$F_i(p)=e_i^T(p){\Sigma }^{-1}{e}_i(p)=(y_i^{,}-\mu {)}^T{\Sigma }^{-1}(y_i^{,}-\mu )$$则有：
$$min\sum _{i=1}^{N_y}(y_i^{,}-\mu {)}^T{\Sigma }^{-1}(y_i^{,}-\mu )=min\sum _{i=1}^{N_y}{F}_i(p)$$
​ 迭代优化过程为：在迭代中通过不断调整$$p\to p+\Delta p$$找到$\Delta p$使得目标函数达到最小值
$$min\sum _{i=1}^{N_y}{F}_i(p)=\sum _{i=1}^{N_y}{e}_i^T(p+\Delta p){\Sigma }^{-1}{e}_i(p+\Delta p)$$
​ 泰勒展开：
$$\begin{gathered} e_i(p+\Delta p)\approx e_i(p)+\frac{d{e}_i}{dp}\Delta p=e_i+J_i\Delta p \\ {F}_i(p+\Delta p)=e_i^T(p+\Delta p){\Sigma }^{-1}{e}_i(p+\Delta p)\approx (e_i+J_i\Delta p{)}^T{\Sigma }^{-1}(e_i+J_i\Delta p) \\ =e_i^T{\Sigma }^{-1}{e}_i+2e_i^T{\Sigma }^{-1}{J}_i\Delta p+\Delta p^T{J}_i^T{\Sigma }^{-1}{J}_i\Delta p=F_i(p)+2b_i^T\Delta p+\Delta p^T{H}_i\Delta p \\ 其中,b_i^T=e_i^T{\Sigma }^{-1}{J}_i{H}_i=J_i^T{\Sigma }^{-1}{J}_i \end{gathered}$$
目标函数随自变量的变化为：
$$\Delta F_i(p)=F_i(p+\Delta p)-F_i(p)=2b_i^T\Delta p+\Delta p^T\Delta H_i\Delta p$$
由此，上述优化问题可转化为：找到$\Delta p$使得$\Delta F_i(p)$​取得极小值。令其导数为零，则有：
$$\frac{d\Delta F_i(p)}{d\Delta p}=2b_i+2H_i\Delta p=0==>H_i\Delta p=-b_i$$
根据定义：$b_i^T=e_i^T{\Sigma }^{-1}{J}_i$，$H_i=J_i^T{\Sigma }^{-1}{J}_i$。因此，只需得到$J_i=\frac{d{e}_i}{dp}$，即可求出$\Delta p$
雅可比矩阵求解

• 2D场景

​ 令 p = [ t x t y ϕ z ] T p=\left[

$$\begin{array}{cc} t_x & t_y & \phi_z\end{array}$$ \right]^T p=[tx​​ty​​ϕz​​]T ，则：
$$\begin{gathered} y_i^{,}=T(p,y_i)=\left[\begin{array}{cc}cos{\varphi }_z& -sin{\varphi }_z\\ sin{\varphi }_z& cos{\varphi }_z\end{array}\right]y_i+\left[\begin{array}{c}{t}_x\\ t_y\end{array}\right]=\left[\begin{array}{cc}cos{\varphi }_z& -sin{\varphi }_z\\ sin{\varphi }_z& cos{\varphi }_z\end{array}\right]\left[\begin{array}{c}{y}_{i1}\\ y_{i2}\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\end{array}\right] \\ =\left[\begin{array}{c}cos{\varphi }_z{y}_{i1}-sin{\varphi }_z{y}_{i2}+t_x\\ sin{\varphi }_z{y}_{i1}+cos{\varphi }_z{y}_{i2}+t_y\end{array}\right] \\ {e}_i=y_i^{,}-\mu =\left[\begin{array}{c}cos{\varphi }_z{y}_{i1}-sin{\varphi }_z{y}_{i2}+t_x-{\mu }_x\\ sin{\varphi }_z{y}_{i1}+cos{\varphi }_z{y}_{i2}+t_y-{\mu }_y\end{array}\right] \\ {J}_i=\left[\begin{array}{ccc}\frac{\partial e_{i1}}{\partial t_x}& \frac{\partial e_{i1}}{\partial t_y}& \frac{\partial e_{i1}}{\partial {\varphi }_z}\\ \frac{\partial e_{i2}}{\partial t_x}& \frac{\partial e_{i2}}{\partial t_y}& \frac{\partial e_{i2}}{\partial {\varphi }_z}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -y_{i1}sin{\varphi }_z-y_{i2}cos{\varphi }_z\\ 0& 1& y_{i1}cos{\varphi }_z-y_{i2}sin{\varphi }_z\end{array}\right] \end{gathered}$$

• 3D场景

​ p = [ t x t y t z ϕ x ϕ y ϕ z ] T p=\left[

$$\begin{array}{cc} t_x & t_y & t_z & \phi_x & \phi_y & \phi_z\end{array}$$ \right]^T p=[tx​​ty​​tz​​ϕx​​ϕy​​ϕz​​]T ，$e_i=y_i^{,}-\mu$。其中，$y_i^{,}$表达如下：
$$y_i^{,}=T(p,y_i)=R_x{R}_y{R}_z{y}_i+t=\left[\begin{array}{ccc}{c}_y{c}_z& -c_y{s}_z& s_y\\ c_x{s}_z+s_x{s}_y{c}_z& c_x{c}_z-s_x{s}_y{s}_z& -s_x{c}_y\\ s_x{s}_z-c_x{s}_y{c}_z& c_x{s}_y{s}_z+s_x{c}_z& c_x{c}_y\end{array}\right]y_i+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$$
​ 雅可比矩阵：$J_i=\left[\begin{array}{cccccc}1& 0& 0& 0& c& f\\ 0& 1& 0& a& d& g\\ 0& 0& 1& b& e& h\end{array}\right]$，其中参数表达如下：
$$\begin{gathered} a=y_{ix}(-s_x{s}_z+c_x{s}_y{c}_z)+y_{iy}(-s_x{c}_z-c_x{s}_y{s}_z)+y_{iz}(-c_x{c}_y) \\ b=y_{ix}(c_x{s}_z+s_x{s}_y{c}_z)+y_{iy}(-s_x{s}_y{c}_z)+y_{iy}(-s_x{s}_y{s}_z+c_x{c}_z)+y_{iz}(-s_x{c}_y) \\ c=y_{ix}(-s_y{c}_z)+y_{iy}(s_y{s}_z)+y_{iz}(c_y) \\ d=y_{ix}(s_x{c}_y{c}_z)+y_{iy}(-s_x{c}_y{s}_z)+y_{iz}(s_x{s}_y) \\ e=y_{ix}(-c_x{c}_y{c}_z)+y_{iy}(c_x{c}_y{s}_z)+y_{iz}(-c_x{s}_y) \\ f=y_{ix}(-c_y{s}_z)+y_{iy}(-c_y{c}_z) \\ g=y_{ix}(c_x{c}_z-s_x{s}_y{s}_z)+y_{iy}(-c_x{s}_z-s_x{s}_y{c}_z) \\ h=y_{ix}(s_x{c}_z+c_x{s}_y{s}_z)+y_{iy}(c_x{s}_y{s}_z-s_x{s}_z) \end{gathered}$$
（3）NDT与ICP的比较

​  ICP是要求两点云经转换后尽可能接近，NDT是要求两点云经转换后点的分布尽可能接近，终止条件相较于ICP而言没那么严格，因此在一定程度上NDT的鲁棒性要好过于ICP。另外，NDT的效率高于ICP，主要体现在：NDT中只用计算一次target点云的分布（均值和方差），后续迭代中会不断复用这一分布，以此作为衡量两点云匹配的吻合程度；而ICP需要在每一次迭代中使用上一位姿更新点云位置，因此效率低于NDT。

3. 实践：点云配准

（1）整体思路

• 数据预处理：降采样、移除噪点。
• 获取初始位姿：不管是ICP还是NDT，都需要一个较好的初始位姿。在实际中，初始位姿往往是未知的。可使用特征点提取 + 特征点描述 + 特征点匹配 + RANSAC的方式获取初始位姿。
• ICP/NDT精细配准

（2）实现步骤

• 读取数据
• 对点云进行降采样
• 使用ISS算法进行关键点提取
• 使用FPFH算法进行关键点描述
• 使用RANSAC进行粗配准，得到初始旋转平移矩阵
  • 在特征空间建立关键点的correspondences
  • 随机选择correspondences进行模型拟合
  • 迭代，选择出矩阵T作为ICP的初始矩阵
• 使用ICP对初始旋转平移矩阵进行迭代，优化配准
  • 建立所有点的correspondences，移除outlier pairs
  • 计算$R$和$t$
  • 迭代，直至算法收敛

（3）代码实现

• RANSAC

import os
import argparse
from tkinter.messagebox import NO
import progressbar
import collections
import numpy as np
import open3d as o3d
import copy
import concurrent.futures
from scipy.spatial.distance import pdist
from scipy.spatial.transform import Rotation
import matplotlib.pyplot as plt

# from ICP import  icp_exact_match
# 3对点即可求解
def solve_procrustes_transf(P,Q):    # 求解平移旋转矩阵 solve_procrustes_transformation
    up = P.mean(axis=0)
    uq = Q.mean(axis=0)

    # move to center:
    P_centered = P - up
    Q_centered = Q - uq

    U, s, V = np.linalg.svd(np.dot(Q_centered.T, P_centered), full_matrices=True, compute_uv=True)
    R = np.dot(U, V)
    t = uq - np.dot(R, up)

    # format as transform:
    T = np.zeros((4, 4))
    T[0:3, 0:3] = R
    T[0:3, 3] = t
    T[3, 3] = 1.0

    return T

def get_init_matches(feature_source, feature_target):
    ##对 feature target fpfh 建立 kd—tree
    feature_source = feature_source.transpose()
    feature_target = feature_target.transpose()
    fpfh_search_tree = o3d.geometry.KDTreeFlann(feature_target)
    ##建立 pairs
    _, N = feature_source.shape
    matches = []
    # 对于每一个source keypoints中的点，在target keypoints中找到fpfh descriptors空间上找到与之最近的点索引idx_nn_target，二者构成一对pair
    for i in range(N):
        query = feature_source[:, i]
        _, idx_nn_target, _ = fpfh_search_tree.search_knn_vector_xd(query, 1)   # source -> target
        matches.append([i,idx_nn_target[0]])    # 通过knn 寻找唯一 的 nearest points 一一配对 构建pair

    matches = np.asarray(matches)
    return  matches


def iter_match(keypoints_source, keypoints_target, sample_matches,):
    # 获取sample点的索引
    sample_source_idx, sample_target_idx = sample_matches[:, 0], sample_matches[:, 1]
    # 获取sample点
    sample_source_points = np.array(keypoints_source)[sample_source_idx]
    sample_target_points = np.array(keypoints_target)[sample_target_idx]

    # 对sample点，通过procrustes_transf计算其变换矩阵T
    T = solve_procrustes_transf(sample_source_points, sample_target_points)

    return T


def ransac_match(keypoints_source, keypoints_target, fpfh_source, fpfh_target,iterators):
    # 在fpfh空间上，建立source与target之间的初始匹配
    matches = get_init_matches(fpfh_source, fpfh_target)

    N, _ = matches.shape
    idx_matches = np.arange(N)   #　对每对 pair 打标签

    poses = []
    corresponces = []
    keypoints_source_matches_idx, keypoints_target_matches_idx = matches[:, 0], matches[:, 1]
    keypoints_source_matches_points = np.array(keypoints_source)[keypoints_source_matches_idx]
    keypoints_target_matches_points = np.array(keypoints_target)[keypoints_target_matches_idx]
    for iter in range(iterators):
        # 每次迭代随机选取3对pair
        sample_matches = matches[np.random.choice(idx_matches, 3, replace=False)]
        # 通过随机选取的3对pair，获取其对应的变换矩阵T
        T = iter_match(keypoints_source, keypoints_target, sample_matches)
        R, t = T[0:3, 0:3], T[0:3, 3]
        poses.append(T)
        keypoints_source_matches_points = np.dot(keypoints_source_matches_points, R.T) + t
        # 获取3对pair得到的变换矩阵产生的corresponce
        corresponce = np.linalg.norm(keypoints_target_matches_points - keypoints_source_matches_points,axis = 1)
        corresponces.append(np.sum(corresponce))
    
    min_idx = np.argmin(corresponces)  # 将corresponce最小的变换矩阵作为RANSAC提取到的最佳位姿pose
    return poses[min_idx]

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93

• ISS

import open3d as o3d
import os
import numpy as np
from pyntcloud import PyntCloud
import matplotlib.pyplot as plt
from sklearn.neighbors import KDTree # KDTree 进行搜索
import random
from pandas import DataFrame
import pandas as pd
from scipy import spatial
import heapq

# 获取提取到的特征点及其对应于原点集中的索引
def iss(data, gamma12=0.7, gamma23=0.7, leaf_size=4, iss_radius=0.15, max_nms_points=1000, nms_radius=0.15):
    n, _ = data.shape
    tree = KDTree(data, leaf_size)  # 以leafsize为最小节点，对源点集data构建kdtree
    nearest_idx = tree.query_radius(data, iss_radius)  # 通过kdtree的rnn搜索，快速查找所有点的iss_radius邻域点，查找到的索引存储在nearest_idx中
    iss_points_before_nms = []
    min_values = []
    for i in range(45, n):
        # print(i)
        current_point_neighbor_idx = nearest_idx[i]  # 当前点i的邻域点
        # 若当前点没有iss_radius邻域点，则对下一点进行处理
        if len(current_point_neighbor_idx) == 0:
            continue
        i_idx = np.argwhere(current_point_neighbor_idx == i)  # 查找current_point_neighbor_idx数组中值为当前点i的索引
        current_point_neighbor_idx = np.delete(current_point_neighbor_idx, i_idx)  # 去掉当前点i
        # current_point_neighbor_idx在删掉当前点索引i后，有可能为空
        if len(current_point_neighbor_idx) == 0:
            continue
        Wi = np.linalg.norm(data[i] - data[current_point_neighbor_idx], axis=1)  # 按照公式，计算当前点i到其所有邻域点之间的距离pi-pj
        Wi = 1.0 / Wi

        cov_tmp = np.zeros((3, 3))
        for j in range(len(current_point_neighbor_idx)):
            pij = (data[i] - data[current_point_neighbor_idx[j]])[:, np.newaxis]  # shape:(3, )  ==>  (3, 1)  3行1列
            cov_tmp += Wi[j] * (pij.dot(pij.transpose()))
        w_sum = np.sum(Wi)
        cov_pi = cov_tmp / np.sum(Wi)  # 当前点pi的协方差矩阵

        sigma = np.linalg.svd(cov_pi, compute_uv=False)  # svd分解求协方差矩阵的特征值
        if (sigma[1] / sigma[0] < gamma12) and (sigma[2] / sigma[1] < gamma23):
            iss_points_before_nms.append(data[i])
            min_values.append(sigma[2])
    
    iss_points_after_NMS = []
    iss_points_after_NMS_idx = []
    iss_points_before_nms_idx = [i for i in range(len(iss_points_before_nms))]
    nms_kdtree = spatial.KDTree(iss_points_before_nms, leaf_size)  # 以iss_points_before_nms为源点集，构建kdtree
    for k in range(max_nms_points):
        max_index = min_values.index(max(min_values))  # 获取最小特征值里面当前迭代的最大值
        max_point = iss_points_before_nms[max_index]
        
        delete_idx = nms_kdtree.query_ball_point(max_point, nms_radius)  # 处于当前最大点４邻域内的所有点的索引
        for idx in delete_idx:
            # 如果当前点max_point的nms_radius邻域点iss后的点集中，则将这些邻域点删除掉
            if idx in iss_points_before_nms_idx:  
                del min_values[iss_points_before_nms_idx.index(idx)]
                del iss_points_before_nms[iss_points_before_nms_idx.index(idx)]
                del iss_points_before_nms_idx[iss_points_before_nms_idx.index(idx)]

        iss_points_after_NMS.append(max_point)  # 获取NMS后的点
        idx = np.where((data == max_point).all(axis=1))  
        iss_points_after_NMS_idx.append(idx)  # 获取NMS后的点在源点云data中的索引

        # nms后的最大点数可能超过iss后的点数
        if len(min_values) == 0:
            break
    
    return iss_points_after_NMS, iss_points_after_NMS_idx

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

• FPFH

from ctypes import pointer
import imp
import os
import matplotlib.pyplot as plt
import numpy as np
import open3d as o3d
import math
from pyntcloud import PyntCloud
from pandas import DataFrame
from scipy import spatial
from sklearn.neighbors import KDTree
from mpl_toolkits.mplot3d import Axes3D

# 功能: 计算当前点的spfh描述
# 输入: 
#      raw_points: 原始点云的所有三维点  Nx3 (N个点的x、y、z坐标)
#      raw_points_normal: 原始点云的所有三维点对应的法向量  Nx3 (N个点各自法向量的３个方向的分量)
#      near_points_idx: 原始点云中所有点的指定邻域大小的邻域点索引  NxM (N个点，每个点有M个邻域点，每个点的M不一定相等)
#      B: 每个描述子直方图中bin的个数
# 输出: 
#      signature: [alpha, phi, theta]三元组的直方图组合，用以表示当前点的spfh描述
def SPFH(raw_points, raw_points_normal, key_point_idx, near_points_idx, B):
    key_near_idx = near_points_idx[key_point_idx]  # 当前key_point的邻域点索引(包含自身)
    key_near_idx_list = list(key_near_idx)
    key_near_idx_list.remove(key_point_idx)     # 当前key_point的邻域点索引中，去掉自身索引

    n1 = raw_points_normal[key_point_idx]
    n2 = raw_points_normal[key_near_idx_list]

    # 坐标系
    u = n1
    v = np.cross(u, (raw_points[key_near_idx_list] - raw_points[key_point_idx]) / np.linalg.norm(raw_points[key_near_idx_list] - raw_points[key_point_idx]))
    w = np.cross(u, v)

    # 三元组[alpha, phi, theta]
    alpha = np.sum(v * n2, axis=1)
    phi = np.sum(u*(raw_points[key_near_idx_list] - raw_points[key_point_idx]) / 
                               np.linalg.norm(raw_points[key_near_idx_list] - 
                                              raw_points[key_point_idx]), axis=1)  
    theta2 =  np.arctan(w*n2, u*n2)
    theta = np.sum(np.arctan(w*n2, u*n2), axis=1)

    # histogram
    alpha_histogram = np.histogram(alpha, B, range=(-1.0, 1.0))[0]
    sum_alpha_histogram = np.sum(alpha_histogram)
    alpha_histogram = alpha_histogram / sum_alpha_histogram

    phi_histogram = np.histogram(phi, B, range=(-1.0, 1.0))[0]
    sum_phi_histogram = np.sum(phi_histogram)
    phi_histogram = phi_histogram / sum_phi_histogram

    theta_histogram = np.histogram(theta, B, range=(-np.pi, np.pi))[0]
    sum_theta_histogram = np.sum(theta_histogram)
    theta_histogram = theta_histogram / np.sum(sum_theta_histogram)

    signature = np.hstack((alpha_histogram, phi_histogram, theta_histogram))

    return signature

# 功能: 计算当前点的fpfh描述  当前点的spsh描述 + 当前点的邻域点的spfh描述
# 输入: 
#      raw_points: 原始点云的所有三维点  Nx3 (N个点的x、y、z坐标)
#      raw_points_normal: 原始点云的所有三维点对应的法向量  Nx3 (N个点各自法向量的３个方向的分量)
#      near_points_idx: 原始点云中所有点的指定邻域大小的邻域点索引  NxM (N个点，每个点有M个邻域点，每个点的M不一定相等)
#      B: 每个描述子直方图中bin的个数
# 输出: 
#      fpfh: 当前点与其邻域点的spfh直方图描述的组合，用以表示当前点的fpfh描述
def fpfh(raw_points, raw_points_normal, key_point_idx, near_points_idx, B=5):
    # 当前key_point的spfh 3B
    spfh_pq = SPFH(raw_points, raw_points_normal, key_point_idx, near_points_idx, B)  

    # 当前key_point邻域点的spfh
    key_near_idx = near_points_idx[key_point_idx]  # 当前key_point的邻域点索引(包含自身)
    key_near_idx_list = list(key_near_idx)
    key_near_idx_list.remove(key_point_idx)     # 当前key_point的邻域点索引中，去掉自身索引
    
    # p1_near_num
    W = 1.0 / np.linalg.norm((raw_points[key_point_idx] - raw_points[key_near_idx_list]), axis=1)
    # [p1_near_num, 3B]  
    spfh_pk = np.asarray([SPFH(raw_points, raw_points_normal, point_k_idx, near_points_idx, B) for point_k_idx in key_near_idx_list])
    # np.dot() 矩阵相乘里面包含了np.sum()
    spfh_pk = np.dot(W, spfh_pk) / len(W)

    fpfh = spfh_pq + spfh_pk
    # 归一化
    fpfh = fpfh / np.linalg.norm(fpfh)
    return fpfh

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

• ICP

import open3d as o3d
import os
import numpy as np
from pyntcloud import PyntCloud
import matplotlib.pyplot as plt
from sklearn.neighbors import KDTree # KDTree 进行搜索
import random
from pandas import DataFrame
import pandas as pd
from scipy import spatial
import heapq




def icp(pcd_source, pcd_target, init_pose, iterators, threshold):
    source_points = np.array(pcd_source.points)      # (20000, 3)  三维空间点3个方向的坐标值
    target_points = np.array(pcd_target.points)      # (20000, 3)  三维空间点3个方向的坐标值
    init_R = init_pose[0:3, 0:3]
    init_t = init_pose[0:3, 3]
    source_points = np.dot(source_points, init_R.T) + init_t
    
    for iter in range(iterators):
        # 计算source与target的中心点
        center_source_point = np.mean(source_points, axis=0)
        center_target_point = np.mean(target_points, axis=0)
        # 计算source与target的各自去中心点
        del_center_source_points = source_points - center_source_point
        del_center_target_points = target_points - center_target_point

        # 计算矩阵W
        W = np.zeros((3, 3))
        for i in range(len(del_center_source_points)):
            W += del_center_source_points[i] * del_center_target_points[i].T
        # 对矩阵W进行SVD分解
        U, Sigma, VT = np.linalg.svd(W)
        R = U * VT
        t = center_target_point - np.dot(center_source_point, R.T)
        # 计算变换后，source与target之间的误差
        error = 0
        for i in range(len(source_points)):
            error += np.sum(np.abs(source_points[i] - target_points[i]))
        
        # 根据error判断是否可提前终止icp
        if error < threshold:
            break
        
        # 更新source_points
        source_points = np.dot(source_points, R.T) + t
    
    return R, t

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

• registration

from cmath import nan
import os
import argparse
import progressbar
import collections
import numpy as np
import open3d as o3d
from ISS import *
from FPFH import *
from filter import *
from RANSAC import *
from ICP import *
from data_process import *
from sklearn.neighbors import KDTree
import matplotlib.pyplot as plt


if __name__ == '__main__':
    radius = 0.5
    #step1 读取数据
    input_dir  = "registration_dataset"
    df_output = {
        'idx1': [],
        'idx2': [],
        't_x': [],
        't_y': [],
        't_z': [],
        'q_w': [],
        'q_x': [],
        'q_y': [],
        'q_z': []
    }
    registration_results = read_registration_results(os.path.join(input_dir,'reg_result.txt'))
    for i,r  in (list(registration_results.iterrows())):
        idx_target = int(r['idx1'])
        idx_source = int(r['idx2'])
        # idx_target = 0
        # idx_source = 456
        ##load point cloud
        pcd_source = read_point_cloud_bin(
            os.path.join(input_dir,'point_clouds',f'{idx_source}.bin')
        )
        pcd_target = read_point_cloud_bin(
            os.path.join(input_dir,'point_clouds',f'{idx_target}.bin')
        )

        pcd_source_points = np.array(pcd_source.points)     
        pcd_source_normals = np.array(pcd_source.normals)   
        pcd_target_points = np.array(pcd_target.points)      
        pcd_target_normals = np.array(pcd_target.normals)    
        
        # voxel grid进行降采样
        source_downsampled_points, source_downsampled_normals,
        source_downsampled_points_idx = voxel_grid_downsample(pcd_source_points,
                                                              pcd_source_normals, 0.5) 
        target_downsampled_points, target_downsampled_normals,
        target_downsampled_points_idx = voxel_grid_downsample(pcd_target_points,
                                                              pcd_target_normals, 0.5)  

    
        # iss提取特征点  keypoints_source_idx 是相对于source_downsampled_points中的索引
        keypoints_source, keypoints_source_idx = iss(source_downsampled_points, 
                                                     gamma12=0.6, gamma23=0.6, 
                                                     leaf_size=4, iss_radius=0.15, 	
                                                     max_nms_points=5000, 
                                                     nms_radius=0.05)
        keypoints_target, keypoints_target_idx = iss(target_downsampled_points, 
                                                     gamma12=0.6, gamma23=0.6, 
                                                     leaf_size=4, iss_radius=0.15, 
                                                     max_nms_points=5000, 
                                                     nms_radius=0.05)
        leaf_size = 4
        near_radius = 0.5
        # 获取所有点的r邻域点
        source_downsampled_points_kdtree = KDTree(source_downsampled_points, leaf_size)
        source_downsampled_near_points_idx = source_downsampled_points_kdtree.query_radius(source_downsampled_points, near_radius)
        target_downsampled_points_kdtree = KDTree(target_downsampled_points, leaf_size)
        target_downsampled_near_points_idx = target_downsampled_points_kdtree.query_radius(target_downsampled_points, near_radius)
        
        # fpfh: 对提取到的特征点进行fpfh描述
        fpfh_source_keypoints = []
        for key_point_source_idx in keypoints_source_idx:
            key_point_idx = key_point_source_idx[0][0]
            key_source_near_idx_list = 
                                  list(source_downsampled_near_points_idx[key_point_idx])
            # 提前避免fpfh中出现无效数据
            if len(key_source_near_idx_list) == 1:
                continue

            tmp_fpfh = fpfh(source_downsampled_points, 
                            source_downsampled_normals, 
                            key_point_source_idx[0][0],     
                            source_downsampled_near_points_idx, B=11)
            				fpfh_source_keypoints.append(tmp_fpfh)
        fpfh_source_keypoints = np.asarray(fpfh_source_keypoints)

        fpfh_target_keypoints = []
        for key_point_target_idx in keypoints_target_idx:
            key_point_idx = key_point_target_idx[0][0]
            key_target_near_idx_list =                   
                                  list(target_downsampled_near_points_idx[key_point_idx])
            # 提前避免fpfh中出现无效数据
            if len(key_target_near_idx_list) == 1:
                continue

            tmp_fpfh = fpfh(target_downsampled_points, 
                            target_downsampled_normals, 
                            key_point_target_idx[0][0], 
                            target_downsampled_near_points_idx, B=11)
            fpfh_target_keypoints.append(tmp_fpfh)
        fpfh_target_keypoints = np.asarray(fpfh_target_keypoints)

        # 在fpfh空间上，对提取到的特征点进行RANSAC操作，获取初始的旋转　+ 平移
        T = ransac_match(keypoints_source, keypoints_target, 
                         fpfh_source_keypoints, fpfh_target_keypoints, 10)
        # 在初始的旋转 + 平移下，对原始点云进行迭代ICP操作，获取最优的旋转 + 平移
        R, t = icp(pcd_source, pcd_target, T, 100, 100000)
        
        # 将旋转转换成四元数的形式，并写成DataFrame结构
        add_to_output(df_output, idx_target, idx_source, R, t)
        # 将结果写入groundtruths.txt中
        write_output(os.path.join(input_dir, 'groundtruths.txt'), df_output)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123

（4）配准效果