1. 程式人生 > >Kinect-Fusion ICP演算法構建帶法向量點雲金字塔

Kinect-Fusion ICP演算法構建帶法向量點雲金字塔

主要步驟:

1、構建帶法向量的點雲金字塔,ICP執行時先對金字塔高層的點雲進行匹配,再往下層走,即從粗匹配到精匹配的過程,加快收斂速度;

2、找兩點雲的匹配點;

3、構造目標函式,並且最小化目標函式獲得轉換矩陣;

4、判斷目標函式的大小是否小於所設定的閾值,或者迭代次數是否超過某個設定的最大值,如果前面的判斷是F,則返回到第二步,直至前面的判斷是T;

第一步:

金字塔第一層:

使用雙邊濾波器對原始的深度圖進行濾波,再通過以下兩個公式進行計算出每個畫素對應的相機座標系下的座標點和法向量;

 u為畫素點座標,D為深度圖,K為相機的內部引數可以通過張正友校正法得到;

v[]為歸一化函式;

金字塔第n層:

先對n-1層的深度圖進行均值濾波,然後再進行下采樣獲得第n層的深度圖,也可以使用opencv中的pyrDown函式(這個是經過高斯濾波之後進行下采樣)。