三维点云配准算法，包括4PCS、K-4PCS、SAC-IA、ICP、PCA、深度学习方法等-电子发烧友网

1 什么是点云配准

点云配准指的是输入两幅点云 Ps (source) 和 Pt (target)，输出一个变换矩阵T（即旋转R和平移t）使得 T(Ps)和Pt的重合程度尽可能高。我们可以把点云想象成由无数个三维点组成的云彩，而点云配准就是要把这些云彩按照它们实际的位置和姿态拼接在一起，就像把多个拼图拼接在一起，最终形成一个完整的三维模型。粗配准（Coarse Registration）在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值。精配准（Fine Registration）精配准是给定一个初始变换，进一步优化得到更精确的变换。粗配准和精配准流程如下图所示：

2 常见的配准算法

2.1 4PCS配准

1 原理

并非全共线的共面四点a，b，c，d，定义了两个独立的比率r1和r2，其在仿射变化中是不变且唯一的。现在给定一个具有n个点的点集Q，以及两个由点P得到的仿射不变的比率r1，r2，对每一对点q1，q2⊂ Q，计算他们的中间点:若任意两对这样的点，一对由 r1计算得到的中间点和另一对由 r2计算得到的中间点在允许范围内一致，那么可以认为这两对点可能是 P中基础点的仿射对应点。将四点转化应用到全局点云转化,计算点云的匹配重叠度，若达到设置的阈值，则完成点云粗配准。2 核心代码

pcl::FPCSInitialAlignmentfpcs;
fpcs.setInputSource(source_cloud);//源点云
fpcs.setInputTarget(target_cloud);//目标点云
fpcs.setApproxOverlap(0.7);//设置源和目标之间的近似重叠度。
fpcs.setDelta(0.01);//设置常数因子delta，用于对内部计算的参数进行加权。
fpcs.setNumberOfSamples(100);//设置验证配准效果时要使用的采样点数量

2.2 K-4PCS配准

1 步骤

K-4PCS方法主要分为两个步骤:

(1)利用VoxelGrid滤波器对点云Q进行下采样，然后使用标准方法进行3D关键点检测。

(2)通过4PCS算法使用关键点集合而非原始点云进行数据的匹配，降低了搜索点集的规模，提高了运算效率。

2 核心代码

pcl::KFPCSInitialAlignmentkfpcs;
kfpcs.setInputSource(source);//源点云
kfpcs.setInputTarget(target);//目标点云
kfpcs.setApproxOverlap(0.7);//源和目标之间的近似重叠。
kfpcs.setLambda(0.5);//平移矩阵的加权系数。
kfpcs.setDelta(0.002,false);//配准后源点云和目标点云之间的距离
kfpcs.setNumberOfThreads(6);//OpenMP多线程加速的线程数
kfpcs.setNumberOfSamples(200);//配准时要使用的随机采样点数量
pcl::PointCloud::Ptrkpcs(newpcl::PointCloud);
kfpcs.align(*kpcs);

2.3 SAC-IA配准

1 步骤SAC-IA配准的实现流程：

①分别计算源点云和目标点云的FPFH特征描述子；

②基于FPFH特征描述子对两个点云中的点进行匹配；

③随机选择 n (n >= 3) 对匹配点；

④求解该匹配情况下的旋转与平移矩阵；

⑤计算此时对应的误差；重复步骤３－５，直到满足条件，将最小误差对应的旋转和位移作为最终结果。

2 核心代码

pcl::SampleConsensusInitialAlignmentsac_ia;
sac_ia.setInputSource(source);
sac_ia.setSourceFeatures(source_fpfh);
sac_ia.setInputTarget(target);
sac_ia.setTargetFeatures(target_fpfh);
sac_ia.setMinSampleDistance(0.1);//设置样本之间的最小距离
sac_ia.setCorrespondenceRandomness(6);//在选择随机特征对应时，设置要使用的邻居的数量;
pointcloud::Ptralign(newpointcloud);
sac_ia.align(*align);

2.4 主成分分析法（PCA）配准

1 原理

主要利用点云数据的主轴方向进行配准。首先计算两组点云的协方差矩阵，根据协方差矩阵计算主要的特征分量，即点云数据的主轴方向，然后再通过主轴方向求出旋转矩阵，计算两组点云中心坐标的便移直接求出平移向量。

2 核心代码

voidComputeEigenVectorPCA(constpcl::PointCloud::Ptr&cloud,Eigen::Vector4f&pcaCentroid,Eigen::Matrix3f&eigenVectorsPCA)
{
pcl::compute3DCentroid(*cloud,pcaCentroid);
Eigen::Matrix3fcovariance;
pcl::computeCovarianceMatrixNormalized(*cloud,pcaCentroid,covariance);
Eigen::SelfAdjointEigenSolvereigen_solver(covariance,Eigen::ComputeEigenvectors);
eigenVectorsPCA=eigen_solver.eigenvectors();
}

2.5 ICP配准

1 原理

ICP算法的核心是最小化一个目标函数,实际上就是所有对应点之间的欧式距离的平方和。2 步骤

①寻找对应点：我们在有初值的情况下，假设用初始的变换矩阵对source cloud进行变换，将变换后的点云与target cloud进行比较，只要两个点云距离小于一定阈值，我们就认为这两个点就是对应点。

②R、T优化：有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。这里R和T中只有6个自由度，而我们的对应点数量是庞大的。因此，我们可以采用最小二乘等方法求解最优的旋转平移矩阵，一个数值优化问题。

③迭代：我们优化得到了一个新的R与T，导致了一些点转换后的位置发生变化，一些对应点也相应的发生了变化。因此，我们又回到了步骤②中的寻找对应点方法。②③步骤不停迭代进行，直到满足一些迭代终止条件，如R、T的变化量小于一定值，或者上述目标函数的变化小于一定值，或者对应点不再变化等。

3 核心代码

icp.setInputSource(source);//源点云
icp.setInputTarget(target);//目标点云
icp.setTransformationEpsilon(1e-10);//为终止条件设置最小转换差异
icp.setMaxCorrespondenceDistance(1);//设置对应点对之间的最大距离（此值对配准结果影响较大）。
icp.setEuclideanFitnessEpsilon(0.05);//设置收敛条件是均方误差和小于阈值，停止迭代；
icp.setMaximumIterations(35);//最大迭代次数
pcl::PointCloud::Ptricp_cloud(newpcl::PointCloud);
icp.align(*icp_cloud);