三維點(diǎn)云配準(zhǔn)算法，包括4PCS、K-4PCS、SAC-IA、ICP、PCA、深度學(xué)習(xí)方法等

1 什么是點(diǎn)云配準(zhǔn)

點(diǎn)云配準(zhǔn)指的是輸入兩幅點(diǎn)云 Ps (source) 和 Pt (target)，輸出一個(gè)變換矩陣T（即旋轉(zhuǎn)R和平移t）使得 T(Ps)和Pt的重合程度盡可能高。我們可以把點(diǎn)云想象成由無(wú)數(shù)個(gè)三維點(diǎn)組成的云彩，而點(diǎn)云配準(zhǔn)就是要把這些云彩按照它們實(shí)際的位置和姿態(tài)拼接在一起，就像把多個(gè)拼圖拼接在一起，最終形成一個(gè)完整的三維模型。粗配準(zhǔn)（Coarse Registration）在兩幅點(diǎn)云之間的變換完全未知的情況下進(jìn)行較為粗糙的配準(zhǔn)，目的主要是為精配準(zhǔn)提供較好的變換初值。精配準(zhǔn)（Fine Registration）精配準(zhǔn)是給定一個(gè)初始變換，進(jìn)一步優(yōu)化得到更精確的變換。粗配準(zhǔn)和精配準(zhǔn)流程如下圖所示：

2 常見的配準(zhǔn)算法

2.1 4PCS配準(zhǔn)

1 原理

并非全共線的共面四點(diǎn)a，b，c，d，定義了兩個(gè)獨(dú)立的比率r1和r2，其在仿射變化中是不變且唯一的。現(xiàn)在給定一個(gè)具有n個(gè)點(diǎn)的點(diǎn)集Q，以及兩個(gè)由點(diǎn)P得到的仿射不變的比率r1，r2，對(duì)每一對(duì)點(diǎn)q1，q2? Q，計(jì)算他們的中間點(diǎn):若任意兩對(duì)這樣的點(diǎn)，一對(duì)由 r1計(jì)算得到的中間點(diǎn)和另一對(duì)由 r2計(jì)算得到的中間點(diǎn)在允許范圍內(nèi)一致，那么可以認(rèn)為這兩對(duì)點(diǎn)可能是 P中基礎(chǔ)點(diǎn)的仿射對(duì)應(yīng)點(diǎn)。將四點(diǎn)轉(zhuǎn)化應(yīng)用到全局點(diǎn)云轉(zhuǎn)化,計(jì)算點(diǎn)云的匹配重疊度，若達(dá)到設(shè)置的閾值，則完成點(diǎn)云粗配準(zhǔn)。2 核心代碼

pcl::FPCSInitialAlignmentfpcs;
fpcs.setInputSource(source_cloud);//源點(diǎn)云
fpcs.setInputTarget(target_cloud);//目標(biāo)點(diǎn)云
fpcs.setApproxOverlap(0.7);//設(shè)置源和目標(biāo)之間的近似重疊度。
fpcs.setDelta(0.01);//設(shè)置常數(shù)因子delta，用于對(duì)內(nèi)部計(jì)算的參數(shù)進(jìn)行加權(quán)。
fpcs.setNumberOfSamples(100);//設(shè)置驗(yàn)證配準(zhǔn)效果時(shí)要使用的采樣點(diǎn)數(shù)量

2.2 K-4PCS配準(zhǔn)

1 步驟

K-4PCS方法主要分為兩個(gè)步驟:

(1)利用VoxelGrid濾波器對(duì)點(diǎn)云Q進(jìn)行下采樣，然后使用標(biāo)準(zhǔn)方法進(jìn)行3D關(guān)鍵點(diǎn)檢測(cè)。

(2)通過(guò)4PCS算法使用關(guān)鍵點(diǎn)集合而非原始點(diǎn)云進(jìn)行數(shù)據(jù)的匹配，降低了搜索點(diǎn)集的規(guī)模，提高了運(yùn)算效率。

2 核心代碼

pcl::KFPCSInitialAlignmentkfpcs;
kfpcs.setInputSource(source);//源點(diǎn)云
kfpcs.setInputTarget(target);//目標(biāo)點(diǎn)云
kfpcs.setApproxOverlap(0.7);//源和目標(biāo)之間的近似重疊。
kfpcs.setLambda(0.5);//平移矩陣的加權(quán)系數(shù)。
kfpcs.setDelta(0.002,false);//配準(zhǔn)后源點(diǎn)云和目標(biāo)點(diǎn)云之間的距離
kfpcs.setNumberOfThreads(6);//OpenMP多線程加速的線程數(shù)
kfpcs.setNumberOfSamples(200);//配準(zhǔn)時(shí)要使用的隨機(jī)采樣點(diǎn)數(shù)量
pcl::PointCloud::Ptrkpcs(newpcl::PointCloud);
kfpcs.align(*kpcs);

2.3 SAC-IA配準(zhǔn)

1 步驟SAC-IA配準(zhǔn)的實(shí)現(xiàn)流程：

①分別計(jì)算源點(diǎn)云和目標(biāo)點(diǎn)云的FPFH特征描述子；

②基于FPFH特征描述子對(duì)兩個(gè)點(diǎn)云中的點(diǎn)進(jìn)行匹配；

③隨機(jī)選擇 n (n >= 3) 對(duì)匹配點(diǎn)；

④求解該匹配情況下的旋轉(zhuǎn)與平移矩陣；

⑤計(jì)算此時(shí)對(duì)應(yīng)的誤差；重復(fù)步驟３－５，直到滿足條件，將最小誤差對(duì)應(yīng)的旋轉(zhuǎn)和位移作為最終結(jié)果。

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);//設(shè)置樣本之間的最小距離
sac_ia.setCorrespondenceRandomness(6);//在選擇隨機(jī)特征對(duì)應(yīng)時(shí)，設(shè)置要使用的鄰居的數(shù)量;
pointcloud::Ptralign(newpointcloud);
sac_ia.align(*align);

2.4 主成分分析法（PCA）配準(zhǔn)

1 原理

主要利用點(diǎn)云數(shù)據(jù)的主軸方向進(jìn)行配準(zhǔn)。首先計(jì)算兩組點(diǎn)云的協(xié)方差矩陣，根據(jù)協(xié)方差矩陣計(jì)算主要的特征分量，即點(diǎn)云數(shù)據(jù)的主軸方向，然后再通過(guò)主軸方向求出旋轉(zhuǎn)矩陣，計(jì)算兩組點(diǎn)云中心坐標(biāo)的便移直接求出平移向量。

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配準(zhǔn)

1 原理

ICP算法的核心是最小化一個(gè)目標(biāo)函數(shù),實(shí)際上就是所有對(duì)應(yīng)點(diǎn)之間的歐式距離的平方和。2 步驟

①尋找對(duì)應(yīng)點(diǎn)：我們?cè)谟谐踔档那闆r下，假設(shè)用初始的變換矩陣對(duì)source cloud進(jìn)行變換，將變換后的點(diǎn)云與target cloud進(jìn)行比較，只要兩個(gè)點(diǎn)云距離小于一定閾值，我們就認(rèn)為這兩個(gè)點(diǎn)就是對(duì)應(yīng)點(diǎn)。

②R、T優(yōu)化：有了對(duì)應(yīng)點(diǎn)之后，我們就可以用對(duì)應(yīng)點(diǎn)對(duì)旋轉(zhuǎn)R與平移T進(jìn)行估計(jì)。這里R和T中只有6個(gè)自由度，而我們的對(duì)應(yīng)點(diǎn)數(shù)量是龐大的。因此，我們可以采用最小二乘等方法求解最優(yōu)的旋轉(zhuǎn)平移矩陣，一個(gè)數(shù)值優(yōu)化問(wèn)題。

③迭代：我們優(yōu)化得到了一個(gè)新的R與T，導(dǎo)致了一些點(diǎn)轉(zhuǎn)換后的位置發(fā)生變化，一些對(duì)應(yīng)點(diǎn)也相應(yīng)的發(fā)生了變化。因此，我們又回到了步驟②中的尋找對(duì)應(yīng)點(diǎn)方法。②③步驟不停迭代進(jìn)行，直到滿足一些迭代終止條件，如R、T的變化量小于一定值，或者上述目標(biāo)函數(shù)的變化小于一定值，或者對(duì)應(yīng)點(diǎn)不再變化等。

3 核心代碼

icp.setInputSource(source);//源點(diǎn)云
icp.setInputTarget(target);//目標(biāo)點(diǎn)云
icp.setTransformationEpsilon(1e-10);//為終止條件設(shè)置最小轉(zhuǎn)換差異
icp.setMaxCorrespondenceDistance(1);//設(shè)置對(duì)應(yīng)點(diǎn)對(duì)之間的最大距離（此值對(duì)配準(zhǔn)結(jié)果影響較大）。
icp.setEuclideanFitnessEpsilon(0.05);//設(shè)置收斂條件是均方誤差和小于閾值，停止迭代；
icp.setMaximumIterations(35);//最大迭代次數(shù)
pcl::PointCloud::Ptricp_cloud(newpcl::PointCloud);
icp.align(*icp_cloud);

3 深度學(xué)習(xí)的配準(zhǔn)算法

①PointNetLK (Deep ICP)是基于 PointNet的改進(jìn)版ICP算法。PointNet被用來(lái)提取點(diǎn)云的全局特征，然后使用牛頓法迭代近似相似性變換參數(shù)，并且使用這個(gè)過(guò)程中估計(jì)的點(diǎn)對(duì)映射來(lái)更新權(quán)重。

②Deep Closest Point (DCP)基于深度神經(jīng)網(wǎng)絡(luò)的點(diǎn)云配準(zhǔn)算法，它先通過(guò)PointNet提取特征，然后計(jì)算每個(gè)點(diǎn)在目標(biāo)點(diǎn)云中的最近鄰點(diǎn)，并計(jì)算這兩個(gè)點(diǎn)之間的距離。之后，它將這些信息傳遞到一個(gè)形狀編碼器來(lái)學(xué)習(xí)在兩個(gè)點(diǎn)云之間尋找最優(yōu)配準(zhǔn)關(guān)系，并輸出變換矩陣使得兩個(gè)點(diǎn)云重合。

③PRNetPRNet是基于 PointNet++ 的點(diǎn)云配準(zhǔn)算法。它的主要思想是將兩個(gè)點(diǎn)云投射到一個(gè)球面上，然后計(jì)算在這個(gè)球面上的卷積特征。卷積完成后，PRNet使用粗配準(zhǔn)階段進(jìn)行初始配準(zhǔn)，再使用 RANSAC 進(jìn)行細(xì)配準(zhǔn)，最終輸出配準(zhǔn)矩陣。

④PPFNetPPFNet是基于局部點(diǎn)對(duì)特征（PPF）的點(diǎn)云配準(zhǔn)算法，使用神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)點(diǎn)對(duì)之間的相對(duì)變換，并輸出變換矩陣使得兩個(gè)點(diǎn)云對(duì)齊。這個(gè)算法使用卷積神經(jīng)網(wǎng)絡(luò)對(duì)點(diǎn)云進(jìn)行編碼，并學(xué)習(xí) PPF 匹配關(guān)系的特征，并使用訓(xùn)練過(guò)的網(wǎng)絡(luò)對(duì)新的點(diǎn)云對(duì)進(jìn)行配準(zhǔn)。