點雲（剛性）配準 icp

二、**解讀

提到配準演算法，icp認第二，沒哪種演算法敢認第一，可見，icp這道坎是繞不過去的，在本文中，會重點介紹icp的原理與實現方案，同時，也會結合pcl中的**進行詳細介紹。

從本質上講，icp類演算法的基本原理是：對於兩組點雲p和q，計算旋轉矩陣r與旋轉矩陣t，使目標函式e最小：

e = 1

2\frac

21 ∑

\sum∑∣∣

q−r∗

p−t∣

∣2

||q - r * p - t ||^2

∣∣q−r∗

p−t∣

∣2其中，q∈q

q \in q

q∈q， p∈p

p \in p

p∈p。

icpiterated closest point中文翻譯未迭代最近點演算法，其核心在迭代，通過不斷迭代使點雲資料達到收斂：首先，需要計算q與p中的對應點對，計算對應點對的方法比較多，按照原理可以分為多種變體，有point to point，point to plane等，同時，為了提高對應點計算的速度，還可以合理的使用kd tree，octree等資料結構；然後，根據計算的點對關係計算轉換矩陣（旋轉與平移）。轉換矩陣的求解大體分為兩種：利用線性代數的求解（svd奇異值分解），及利用非線性優化方式的方式求解（類似於ba,bundle adjustment），其中奇異值分解的方法因為準確、穩定以及高效的特點被廣泛運用（在eigen中已經提供svd分解方法，本文不做過多研究）。

實現icp的主要步驟有：

步驟1：點雲歸一化即將兩個點雲的某一特徵點（通常會使用質心，也可以選取重心或其他合理的點）平移至共同座標原點，其目的是為了方便計算對應點對；

步驟2：確定對應點對即在關係模糊的兩個點雲中，更合理的預判對應關係，經典icp中是根據point to point的歐式距離進行判斷，假設兩片點雲中歐式距離最近的點作為對應點。該方法具有一定的普適性，在後續優化中，也出現了以point to plane作為對應點的計算規則，該對應關係的查詢占用大量資源，故pcl中利用kd_tree的資料結構實現查詢的加速，當然，也可以通過cuda實現加速。

步驟3：矩陣計算對步驟2中提取的有序點對進行矩陣計算，主要方法有svd、ba的方法（具體方法參考1.4），獲得r和t;

步驟4：變換點雲：將計算獲取的r和t作用於正片點雲；

步驟5：計算目標函式以點對間的歐式距離和作為目標函式；

步驟6：判斷目標函式與閾值的關係，決定是否需要進行下一輪的迭代。

首先計算兩組點雲q和p的質心，分別表示為μ

q\mu^q

μq與μ

p\mu^p

μp，計算公式為：

\mu^q =\frac \sum_i q_i , q_i \in q \\ \mu^p =\frac \sum_i p_i , p_i \in p \\ \end

q_i = q_i - \mu^q \\ p_i = p_i - \mu^p \end

^p_iq_i^t

c=i=1∑

npi

qit

最後，對矩陣c進行svd分解：

c =u

∑v

tc = u \sum v^t

c=u∑vt

得到r和t:

r = vu^t \\ t = \mu^q - r \mu^p \end

// call umeyama directly from eigen (pcl patched version until eigen is released)

//計算cloud_src與cloud_tat的旋轉矩陣

transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);下列原始碼為採用svd演算法計算對應點對關係：

source_it.reset (); target_it.reset ();
// is the source dataset
transformation_matrix.setidentity ();
eigen::matrixcentroid_src, centroid_tgt;
// estimate the centroids of source, target
compute3dcentroid (source_it, centroid_src);
compute3dcentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();
// subtract the centroids from source, target
eigen::matrixcloud_src_demean, cloud_tgt_demean;
demeanpointcloud (source_it, centroid_src, cloud_src_demean);
demeanpointcloud (target_it, centroid_tgt, cloud_tgt_demean);
gettransformationfromcorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);

上述講解了如何計算對應點對的轉換矩陣，現在講解下如何計算對應點對，經典icp指的的point to point的方法，即以點與點之間的某種關係作為乙個度量值，pcl中的icp用的就是這種方法，其基本思路為：計算目標點的最近歐式距離，若小於某個閾值，則認為是對應點，若大於某個閾值，則認為是無效點，下面展示point to point的原始碼片段：

//目錄\pcl\registration\impl\correspondence_estimation
// iterate over the input set of source indices
for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
{tree_->nearestksearch ((*input_)[*idx], 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;
corr.index_query = *idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;

點雲（剛性）配準 icp

ICP點雲配準原理及優化

ITK elastix 剛性配準

點集配準與ICP演算法一

點雲（剛性）配準 icp

ICP點雲配準原理及優化

ITK elastix 剛性 配準

點集配準與ICP演算法 一

相關推薦

ITK elastix 剛性配準

點集配準與ICP演算法一