本發(fā)明涉及三維點(diǎn)云數(shù)據(jù)重建，具體涉及一種慣性定位與點(diǎn)云配準(zhǔn)耦合互補(bǔ)的實(shí)時三維重建方法。

背景技術(shù)：

三維掃描及三維重建技術(shù)的出現(xiàn)為計算機(jī)圖形學(xué)、機(jī)器視覺、工業(yè)設(shè)計、醫(yī)療診斷、虛擬實(shí)境、增強(qiáng)現(xiàn)實(shí)、逆向工程等一系列研究和實(shí)際應(yīng)用提供了可能。該相關(guān)領(lǐng)域內(nèi)的內(nèi)容核心是如何讓三維掃描技術(shù)獲得的數(shù)據(jù)得到高效及精確的配準(zhǔn)。

所謂三維點(diǎn)云數(shù)據(jù)配準(zhǔn)，指的是對于從不同位置角度對同一物體得到的不同的點(diǎn)云，找出它們之間的實(shí)際空間關(guān)系。一般地，主流的配準(zhǔn)方法使用粗配準(zhǔn)到精配準(zhǔn)的流程，從不同位置的傳感器得到的同一物體的點(diǎn)云數(shù)據(jù)中提取信息，通過RANSAC等粗配準(zhǔn)算法，找到對應(yīng)程度較高的兩片點(diǎn)云，通過ICP算法迭代得到這兩片點(diǎn)云的剛體變換關(guān)系，從而實(shí)現(xiàn)配準(zhǔn)。在這個算法過程中，由點(diǎn)云自身提供的信息來引導(dǎo)點(diǎn)云配準(zhǔn)迭代過程的收斂，往往導(dǎo)致龐大的計算量，從而影響實(shí)際配準(zhǔn)過程的速度，難以在保持精度的前提下達(dá)到一定的實(shí)時性。

慣性器件直接獲得加速度信息，在定位的應(yīng)用上，通過積分來確定速度和位移會造成誤差不斷積累，最終導(dǎo)致結(jié)果發(fā)散，單方面利用慣性器件難以有效為三維重建提供幫助。如果采用零速校正等方案補(bǔ)償累積誤差的方案，又會要求傳感器的運(yùn)動過程受到過多限制，極大削弱實(shí)時性和便利性。

專利號為CN105261060A的專利公布了一種基于點(diǎn)云壓縮和慣性導(dǎo)航的移動場景實(shí)時三維重構(gòu)方法，該方法分為三個階段：第1階段，設(shè)計一種基于PFH的點(diǎn)云壓縮算法；第2階段，設(shè)計了一種基于慣性導(dǎo)航的點(diǎn)云配準(zhǔn)算法；第3階段，完成點(diǎn)云融合；該方法只是將慣性器件獲得的加速度值進(jìn)行一次積分和二次積分得到的結(jié)果分別作為慣性導(dǎo)航的速度和位移數(shù)據(jù)，并以此來確定RGB-D傳感器的實(shí)際位置姿態(tài)。采用這樣的方法，會導(dǎo)致隨著三維重構(gòu)過程的進(jìn)行，對RGB-D傳感器的位置和姿態(tài)的估計結(jié)果出現(xiàn)嚴(yán)重的發(fā)散，使得估計結(jié)果與實(shí)際結(jié)果出現(xiàn)較大偏差，進(jìn)而使慣性定位數(shù)據(jù)失去對點(diǎn)云配準(zhǔn)計算的約束作用。

技術(shù)實(shí)現(xiàn)要素：

針對慣性定位發(fā)散與三維點(diǎn)云數(shù)據(jù)配準(zhǔn)的效率問題，本發(fā)明提出了一種慣性定位與點(diǎn)云配準(zhǔn)耦合互補(bǔ)的實(shí)時三維重建方法，本發(fā)明對慣性定位與點(diǎn)云配準(zhǔn)方法的優(yōu)點(diǎn)進(jìn)行耦合，利用互補(bǔ)反饋機(jī)制，既保證了三維重建精度，又提升了整個三維重建過程的實(shí)時性。

一種慣性定位與點(diǎn)云配準(zhǔn)耦合互補(bǔ)的實(shí)時三維重建方法，具體步驟如下：

(1)采集初始慣性定位數(shù)據(jù)及初始點(diǎn)云數(shù)據(jù)；

(2)對初始點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理；

(3)將初始慣性定位數(shù)據(jù)記為原始加速度和原始四元數(shù)；

(4)采集相機(jī)的當(dāng)前加速度、當(dāng)前四元數(shù)及當(dāng)前時刻的點(diǎn)云數(shù)據(jù)；

(5)若在初始循環(huán)中，則將當(dāng)前加速度、當(dāng)前四元數(shù)、原始加速度以及原始四元數(shù)一起處理，得到零時先驗(yàn)平移變換和零時先驗(yàn)轉(zhuǎn)動變換矩陣，然后將零時先驗(yàn)平移變換和轉(zhuǎn)動變換矩陣作為系統(tǒng)的先驗(yàn)變換矩陣；若不在初始循環(huán)中，則將上一時刻的步驟(13)中的后續(xù)先驗(yàn)變換矩陣作為系統(tǒng)的先驗(yàn)變換矩陣；

(6)根據(jù)先驗(yàn)變換矩陣將待配準(zhǔn)點(diǎn)云逆變換為新的待配準(zhǔn)點(diǎn)云；根據(jù)慣性數(shù)據(jù)約束，在新的待配準(zhǔn)點(diǎn)云與參考點(diǎn)云中，將原始三維物體表面不重合的部分去掉，剩下的為有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云；

(7)判斷有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云之間的重疊區(qū)域是否大于閾值，若否，進(jìn)入步驟(8)，若是，進(jìn)入步驟(9′)；

(8)使用有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云，進(jìn)行點(diǎn)云數(shù)據(jù)粗配準(zhǔn)，得到剛體變換矩陣；

(9)將剛體變換矩陣轉(zhuǎn)換的位移和四元數(shù)作為觀測量進(jìn)行卡爾曼濾波，得到位移和四元數(shù)后驗(yàn)估計值，并轉(zhuǎn)換為后驗(yàn)變換矩陣；將后驗(yàn)變換矩陣作為約束矩陣，進(jìn)入步驟(10)；

(9′)直接將先驗(yàn)變換矩陣作為約束矩陣，然后進(jìn)入步驟(10)；

(10)將約束矩陣作為ICP算法的初始值開始迭代計算，進(jìn)行點(diǎn)云數(shù)據(jù)精配準(zhǔn)；得到迭代收斂變換矩陣；

(11)待配準(zhǔn)點(diǎn)云數(shù)據(jù)根據(jù)迭代收斂變換矩陣進(jìn)行逆變換，得到優(yōu)化的點(diǎn)云數(shù)據(jù)，并進(jìn)行三維重建；

(12)根據(jù)實(shí)時三維重建反饋結(jié)果，判斷三維重建是否完成，即用戶是否觸發(fā)終止命令，如果是，終止計算，如果否，繼續(xù)步驟(13)；

(13)將迭代收斂變換矩陣轉(zhuǎn)換的位移和四元數(shù)作為觀測量進(jìn)行卡爾曼濾波，得到位移優(yōu)化后驗(yàn)估計值和四元數(shù)優(yōu)化后驗(yàn)估計值，由位移和四元數(shù)的優(yōu)化后驗(yàn)估計值進(jìn)行時間更新，預(yù)測下一時間步的位移后續(xù)先驗(yàn)估計值和四元數(shù)后續(xù)先驗(yàn)估計值；

(14)將位移和四元數(shù)的后續(xù)先驗(yàn)估計值轉(zhuǎn)換為后續(xù)先驗(yàn)變換矩陣；

(15)循環(huán)步驟(4)～步驟(12)。

在步驟(2)中，將初始點(diǎn)云數(shù)據(jù)通過基于體素柵格的采樣方式變換為TSDF(truncated signed distance field)數(shù)據(jù)。

在步驟(3)中，記原始加速度為a_ini＝(a_x,ini a_y,ini a_z,ini)^T，記原始四元數(shù)為q_ini＝(q_0,ini q_1,ini q_2,ini q_3,ini)^T。

在步驟(4)中，記當(dāng)前加速度為a_cur＝(a_x,cur a_y,cur a_z,cur)^T，記當(dāng)前四元數(shù)為q_cur＝(q_0,cur q_1,cur q_2,cur q_3,cur)^T。

在步驟(5)中，若在初始循環(huán)中，則將步驟(3)和步驟(4)中的數(shù)據(jù)進(jìn)行處理，得到零時先驗(yàn)平移變換和先驗(yàn)轉(zhuǎn)動變換，具體方法為：

步驟(3)和步驟(4)中的原始四元數(shù)和當(dāng)前四元數(shù)所對應(yīng)的轉(zhuǎn)動變換矩陣的逆矩陣分別為：

世界坐標(biāo)系下的原始加速度和當(dāng)前加速度分別為：

a_ini^W＝R_ini^-1a_ini

a_cur^W＝R_cur^-1a_cur

零時先驗(yàn)平移變換為其中△t為第一步時間間隔；

零時先驗(yàn)轉(zhuǎn)動變換為R_pr(q_ini,q_cur)＝R_curR_ini^-1；

先驗(yàn)變換矩陣為[T_pr(a_ini,a_cur)|R_pr(q_ini,q_cur)]。

在步驟(5)中，先驗(yàn)變換矩陣為傳感器位置變化后待配準(zhǔn)點(diǎn)云提供了其所在坐標(biāo)系的位置姿態(tài)約束條件。

在步驟(7)中，判斷有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云間重疊區(qū)域大小的具體方法是：在通過先驗(yàn)變換矩陣的待配準(zhǔn)點(diǎn)云中隨機(jī)取點(diǎn)，根據(jù)該點(diǎn)與參考點(diǎn)云下采樣組成的平面是相交、在前或是在后取不同的采樣值，根據(jù)采樣值求平均取極限來估計重疊區(qū)域占比；判斷閾值取20％～80％，判斷閾值可以根據(jù)具體設(shè)備參數(shù)和配準(zhǔn)結(jié)果精度可做調(diào)整。

在步驟(8)中，使用經(jīng)過先驗(yàn)變換矩陣確定的有效待配準(zhǔn)點(diǎn)云進(jìn)行點(diǎn)云數(shù)據(jù)粗配準(zhǔn)，相當(dāng)于在粗配準(zhǔn)中引入了來自于先驗(yàn)變換矩陣的位置姿態(tài)約束條件。

在步驟(8)中，當(dāng)采取慣性數(shù)據(jù)約束下的基于RANSAC的方法進(jìn)行粗配準(zhǔn)時，具體步驟為：

(8-1)在有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云中的點(diǎn)按照法向量特征分層，如果某個點(diǎn)的法向量在整個點(diǎn)云的法向量空間中出現(xiàn)的頻率越低，就將該點(diǎn)分配到優(yōu)先級越高的層；

(8-2)在有效待配準(zhǔn)點(diǎn)云中取三點(diǎn)為一組，尋找在準(zhǔn)重合參考點(diǎn)云中的對應(yīng)點(diǎn)組，選取點(diǎn)按照(8-1)建立的分層機(jī)制，從優(yōu)先級高的層逐漸往優(yōu)先級低的層進(jìn)行；

(8-3)找到對應(yīng)點(diǎn)組之后，以對應(yīng)點(diǎn)之間距離平方和最小為原則，采用SVD法計算出當(dāng)前點(diǎn)組所對應(yīng)的剛體變換及點(diǎn)間距離平方和；

(8-4)當(dāng)點(diǎn)間距離平方和小于閾值時，基于RANSAC的配準(zhǔn)結(jié)束；閾值初始設(shè)定為10l，l為RGB-D傳感器物理極限精度，后續(xù)根據(jù)實(shí)驗(yàn)反饋可調(diào)整閾值的取值。

在步驟(8)中，當(dāng)采取慣性數(shù)據(jù)約束下的基于PCA的方法進(jìn)行粗配準(zhǔn)時，具體步驟為：

(8-1′)將有效待配準(zhǔn)點(diǎn)云P′和有效參考點(diǎn)云P的點(diǎn)坐標(biāo)分別記為：

p′_i＝(p′_ix p′_iy p′_iz)^T

p_i＝(p_ix p_iy p_iz)^T

其中i表示第i個點(diǎn)；

求有效待配準(zhǔn)點(diǎn)云P′和有效參考點(diǎn)云P的重心坐標(biāo)分別為：

其中N_P′和N_P分別為有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云的點(diǎn)個數(shù)；

(8-2′)以v表示采集點(diǎn)云數(shù)據(jù)的傳感器的視角矢量，以n′_i和n_i分別表示有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云中第i個點(diǎn)的法向量，求有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云中的每個點(diǎn)的權(quán)重分別為：

求和得：

計入權(quán)重的重心為：

(8-3′)計算協(xié)方差矩陣如下：

將協(xié)方差矩陣進(jìn)行SVD奇異值分解：

K′＝U′D′U′^T

K＝UDU^T

其中D′和D是對角矩陣；U′和U為3階歸一化特征向量矩陣。

則轉(zhuǎn)動剛體變換R′和平移剛體變換T′分別為：

R′＝U′U^-1

在步驟(9)中，將當(dāng)前加速度作為加速度觀測量，將由剛體變換矩陣轉(zhuǎn)換的位移作為位移觀測量，將由剛體變換矩陣轉(zhuǎn)換的轉(zhuǎn)動四元數(shù)作為四元數(shù)觀測量，進(jìn)行卡爾曼濾波，得到位移后驗(yàn)估計值與四元數(shù)后驗(yàn)估計值，并將位移后驗(yàn)估計值與四元數(shù)后驗(yàn)估計值轉(zhuǎn)換為后驗(yàn)變換矩陣。

在步驟(9)中，為了抑制慣性導(dǎo)航的誤差累積，將由剛體變換矩陣轉(zhuǎn)換的位移與轉(zhuǎn)動觀測量進(jìn)行卡爾曼濾波，即將先驗(yàn)變換矩陣通過點(diǎn)云粗配準(zhǔn)的約束轉(zhuǎn)化為較為精確的后驗(yàn)變換矩陣。

在步驟(9)中，卡爾曼濾波公式為：

x_k+1＝A_kx_k+Bu_k+w_k

z_k＝H_kx_k+v_k

其中，x是狀態(tài)矢量，A是狀態(tài)轉(zhuǎn)移矩陣，B是控制矩陣，u是系統(tǒng)輸入矢量，w是協(xié)方差陣為Q的過程噪聲矢量；z是觀測矢量，H是系統(tǒng)觀測矩陣，v是協(xié)方差陣為W的量測噪聲矢量；k表示該矢量或矩陣，是濾波過程的第k步對應(yīng)的矢量或矩陣。

在步驟(10)中，將約束矩陣作為約束條件，將使ICP迭代計算能更快收斂，結(jié)果更為精確。

在步驟(11)中，待配準(zhǔn)點(diǎn)云數(shù)據(jù)根據(jù)迭代收斂變換矩陣轉(zhuǎn)換到世界坐標(biāo)系下，并進(jìn)行三維重建過程。

在步驟(13)中，將迭代收斂變換矩陣對應(yīng)的位移和四元數(shù)作為觀測量，使用卡爾曼濾波對相機(jī)此時相對于初始時刻的位移與四元數(shù)的優(yōu)化后驗(yàn)估計值做出估計，然后根據(jù)此優(yōu)化后驗(yàn)估計值預(yù)測下一時刻的位移與四元數(shù)的后續(xù)先驗(yàn)估計值。

在步驟(13)中，反饋的觀測量代替單純的慣性數(shù)據(jù)的積分來作為估計的依據(jù)，通過卡爾曼濾波器估計當(dāng)前時刻位移和四元數(shù)的優(yōu)化后驗(yàn)估計值，可以有效抑制誤差累積導(dǎo)致的發(fā)散。

在步驟(13)中，依據(jù)卡爾曼濾波原理得出位移后續(xù)先驗(yàn)估計值與四元數(shù)后續(xù)先驗(yàn)估計值的具體方法為：

其中為先驗(yàn)估計值建立的狀態(tài)矢量，為后驗(yàn)估計值建立的狀態(tài)矢量。A是狀態(tài)轉(zhuǎn)移矩陣，B是控制矩陣，u是系統(tǒng)輸入矢量；k表示該矢量或矩陣是當(dāng)前時刻對應(yīng)的矢量或矩陣，k+1表示該矢量或矩陣是下一時刻對應(yīng)的矢量或矩陣。

在步驟(14)中，將步驟(13)得到的位移和四元數(shù)的后續(xù)先驗(yàn)估計值轉(zhuǎn)換為后續(xù)先驗(yàn)變換矩陣，此時的后續(xù)先驗(yàn)變換矩陣是基于上一時位移和四元數(shù)的優(yōu)化后驗(yàn)估計值得到的；上一時刻的位移和四元數(shù)的優(yōu)化后驗(yàn)估計值是將迭代收斂變換矩陣對應(yīng)的位移和四元數(shù)值作為觀測量進(jìn)行卡爾曼濾波得到的。

本發(fā)明將傳感器獲取的目標(biāo)三維點(diǎn)云數(shù)據(jù)進(jìn)行粗配準(zhǔn)和精配準(zhǔn)的過程，與慣性傳感器獲取的輔助定位信息進(jìn)行耦合互補(bǔ)，既能夠很好地抑制誤差，又能提高三維重建的精度與速度。

在整個方法流程中，輔助慣性定位數(shù)據(jù)為配準(zhǔn)提供優(yōu)化的約束條件，配準(zhǔn)結(jié)果為慣性定位提供收斂的觀測值，二者合理地耦合互補(bǔ)，使整個三維重建過程實(shí)時跟蹤傳感器位置姿態(tài)，并給出精確配準(zhǔn)過的目標(biāo)三維點(diǎn)云數(shù)據(jù)。

本發(fā)明的方法能夠有效地抑制慣性定位的發(fā)散，并提升三維點(diǎn)云數(shù)據(jù)配準(zhǔn)的效率，減少配準(zhǔn)過程中的運(yùn)算，使三維重建過程達(dá)到較高的實(shí)時性和準(zhǔn)確性。

附圖說明

圖1為本發(fā)明實(shí)時三維重建方法的流程圖；

圖2為TSDF算法融合三維點(diǎn)云原始數(shù)據(jù)的示意圖；

圖3為本發(fā)明實(shí)時三維重建方法的過程示意圖。

具體實(shí)施方式

為了更為具體地描述本發(fā)明，下面結(jié)合附圖及具體實(shí)施方式對本發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說明。

如圖1所示，本發(fā)明慣性定位與點(diǎn)云配準(zhǔn)耦合互補(bǔ)的實(shí)時三維重建方法的具體步驟如下：

(1)陀螺儀和加計組成的慣性單元獲取初始時刻相機(jī)加速度a_ini＝(a_x,ini a_y,inia_z,ini)^T、四元數(shù)q_ini＝(q_0,ini q_1,ini q_2,ini q_3,ini)^T；RGB-D傳感器獲得初始時刻點(diǎn)云數(shù)據(jù)；

對點(diǎn)云數(shù)據(jù)進(jìn)行處理，將點(diǎn)云數(shù)據(jù)通過基于體素柵格的采樣方式變換為truncated signed distance field(TSDF)數(shù)據(jù)，該方式原理示意圖如圖2所示：

記原有的柵格距離值為D_i(x)，權(quán)重為W_i(x)，新加入的柵格距離值為d_i+1(x)，權(quán)重為w_i+1(x)，則新的TSDF數(shù)據(jù)按下式融合：

W_i+1(x)＝W_i(x)+w_i+1(x)；

其中，D_i+1(x)為當(dāng)前點(diǎn)云對應(yīng)的柵格距離值，W_i+1(x)為當(dāng)前點(diǎn)云的權(quán)重。

(2)采集當(dāng)前時刻相機(jī)的加速度a_cur、四元數(shù)q_cur及傳感器的當(dāng)前點(diǎn)云數(shù)據(jù)。

(3)確定先驗(yàn)變換矩陣先驗(yàn)變換矩陣根據(jù)當(dāng)前循環(huán)是否處于初始循環(huán)代表不同意義：若當(dāng)前循環(huán)為初始時刻的循環(huán)，則將當(dāng)前時刻的慣性定位數(shù)據(jù)與原始加速度和原始四元數(shù)一起處理，得到零時先驗(yàn)平移變換和零時先驗(yàn)轉(zhuǎn)動變換矩陣，然后將零時先驗(yàn)平移變換和轉(zhuǎn)動變換矩陣作為系統(tǒng)的先驗(yàn)變換矩陣若當(dāng)前循環(huán)不是初始時刻的循環(huán)，先驗(yàn)變換矩陣取上一時刻的步驟(12)中的后續(xù)先驗(yàn)變換矩陣

求零時先驗(yàn)變換矩陣的具體操作為：將當(dāng)前時刻的加速度a_cur和四元

數(shù)數(shù)據(jù)q_cur與原始加速度a_ini和原始四元數(shù)數(shù)據(jù)q_ini一起處理，轉(zhuǎn)換成零時先驗(yàn)變換矩陣其中R_pr(q_ini,q_cur)為零時先驗(yàn)轉(zhuǎn)動變換矩陣，是原始四元數(shù)q_ini和當(dāng)前轉(zhuǎn)四元數(shù)q_cur的矩陣，由原始轉(zhuǎn)動矩陣R_ini和當(dāng)前轉(zhuǎn)動矩陣R_cur求得，T_pr(a_ini,a_cur)為零時先驗(yàn)平移變換矩陣，是世界坐標(biāo)系下的原始加速度a_ini^W和世界坐標(biāo)系下的當(dāng)前加速度a_cur^W的矩陣。

其中，原始轉(zhuǎn)動矩陣和當(dāng)前轉(zhuǎn)動矩陣由下式得到：

兩個矩陣的逆矩陣直接通過矩陣轉(zhuǎn)置求得：

R_ini^-1＝R_ini^T

R_cur^-1＝R_cur^T

先驗(yàn)轉(zhuǎn)動變換矩陣按照下式得到：

R_pr(q_ini,q_cur)＝R_curR_ini^-1

世界坐標(biāo)系下的原始加速度和當(dāng)前加速度由下式得到：

a_ini^W＝R_ini^-1a_ini

a_cur^W＝R_cur^-1a_cur

平移變換矩陣T_pr由是世界坐標(biāo)系下的原始加速度a_ini^W和世界坐標(biāo)系下的當(dāng)前加速度a_cur^W按照下式得到：

其中△t為第一步時間間隔；

先驗(yàn)變換矩陣為[T_pr(a_ini,a_cur)|R_pr(q_ini,q_cur)]。

(4)根據(jù)先驗(yàn)變換矩陣將待配準(zhǔn)點(diǎn)云逆變換為新的待配準(zhǔn)點(diǎn)云；根據(jù)慣性數(shù)據(jù)約束，在新的待配準(zhǔn)點(diǎn)云與參考點(diǎn)云中，將原始三維物體表面不重合的部分去掉，剩下有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云N_P′即為點(diǎn)云P′中點(diǎn)的個數(shù)，N_P即為點(diǎn)云P中點(diǎn)的個數(shù)。

(5)判斷有效待配準(zhǔn)點(diǎn)云和有效參考點(diǎn)云之間的重疊區(qū)域是否大于閾值，若否，進(jìn)入步驟(6)，若是，進(jìn)入步驟(8*)。

(6)根據(jù)有效參考點(diǎn)云P、有效待配準(zhǔn)點(diǎn)云P′以及先驗(yàn)變換矩陣進(jìn)行點(diǎn)云數(shù)據(jù)粗配準(zhǔn)，得到剛體變換矩陣[R′|T′]；

剛體變換矩陣[R′|T′]中的R′通過將矩陣中的元素進(jìn)行運(yùn)算可得到四元數(shù)q′的值。

剛體變換矩陣[R′|T′]中的T′是位移S′的矩陣。

體素柵格粗配準(zhǔn)采用基于RANSAC法的方法或優(yōu)化的PCA法，慣性單元提供的先驗(yàn)變換矩陣可以對所選的采樣點(diǎn)云數(shù)據(jù)提供范圍限制約束，從而更加快速準(zhǔn)確地得出剛體變換矩陣[R′|T′]。

(7)將剛體變換矩陣[R′|T′]轉(zhuǎn)換為當(dāng)前時刻相對初始時刻的位移S′和四元數(shù)q′；

記R′為：

記q′為：

q′＝(q′₀ q′₁ q′₂ q′₃)^T

則求取q′方法為：

先求絕對值：

確定的符號的方法為：

sign(q′₁)＝sign(q′₀)[sign(R′₃₂-R′₂₃)]

sign(q′₂)＝sign(q′₀)[sign(R′₁₃-R′₃₁)]

sign(q′₃)＝sign(q′₀)[sign(R′₂₁-R′₁₂)]

q′₀的符號可任取；

記T′為：

T′＝(T′_x T′_y T′_z)^T

位移S′有：

S′＝(S′_x S′_y S′_z)^T＝T′＝(T′_x T′_y T′_z)^T

(8)根據(jù)加速度a_cur、位移S′和四元數(shù)q′進(jìn)行卡爾曼濾波計算，得到位移后驗(yàn)估計值S_eva和四元數(shù)后驗(yàn)估計值q_eva；并將位移后驗(yàn)估計值S_eva和四元數(shù)后驗(yàn)估計值q_eva轉(zhuǎn)換為后驗(yàn)變換矩陣[R_eva(q_eva)|T_eva(S_eva)]；

其中，后驗(yàn)變換矩陣[R_eva(q_eva)|T_eva(S_eva)]中的R_eva(q_eva)為四元數(shù)后驗(yàn)估計值q_eva的矩陣，后驗(yàn)變換矩陣[R_eva(q_eva)|T_eva(S_eva)]中的T_eva(S_eva)為位移后驗(yàn)估計值S_eva的矩陣。

卡爾曼濾波公式：

x_k+1＝A_kx_k+Bu_k+w_k

z_k＝H_kx_k+v_k

其中，x是狀態(tài)矢量，A是狀態(tài)轉(zhuǎn)移矩陣，B是控制矩陣，u是系統(tǒng)輸入矢量，w是協(xié)方差陣為Q的過程噪聲矢量；z是觀測矢量，H是系統(tǒng)觀測矩陣，v是協(xié)方差陣為W的量測噪聲矢量；k表示該矢量或矩陣，是濾波過程的第k步對應(yīng)的矢量或矩陣。

得到后驗(yàn)變換矩陣[R_eva(q_eva)|T_eva(S_eva)]之后，將其作為約束矩陣，進(jìn)入步驟(9)的計算。

(8*)將先驗(yàn)變換矩陣直接作為約束矩陣，進(jìn)入步驟(9)的計算。

(9)以約束矩陣作為約束條件，通過ICP算法進(jìn)行點(diǎn)云數(shù)據(jù)精配準(zhǔn)，得到平移變換矩陣T″和轉(zhuǎn)動四元數(shù)q″，并將轉(zhuǎn)動四元數(shù)q″轉(zhuǎn)換為轉(zhuǎn)動變換矩陣R″，平移變換矩陣T″與轉(zhuǎn)動變換矩陣R″構(gòu)成迭代收斂變換矩陣[R″|T″]；

其中，轉(zhuǎn)動變換矩陣R″是元素四元數(shù)q″的矩陣，平移變換矩陣可以直接轉(zhuǎn)化為位移S″；

ICP算法在需配準(zhǔn)的兩片點(diǎn)云中選取對應(yīng)點(diǎn)，以兩片點(diǎn)云上所有對應(yīng)點(diǎn)之間平方和最小為標(biāo)準(zhǔn)，通過迭代得到兩片點(diǎn)云間精確的轉(zhuǎn)動變換矩陣R″；

對于點(diǎn)云數(shù)據(jù)P和點(diǎn)云數(shù)據(jù)P′，計算所有對應(yīng)點(diǎn)平方和的平均的公式如下(以角標(biāo)一樣表示對應(yīng)的點(diǎn))：

其中，R和T的初始輸入為上述約束條件[R_eva|T_eva]，N_p′為點(diǎn)云P′中的點(diǎn)的個數(shù)，若第k次迭代剛體變換與第k-1次迭代的點(diǎn)對距離平方和滿足關(guān)系||d_k-d_k-1||<ε，ε為設(shè)定的閾值(ε的范圍初始設(shè)置為l，l為RGB-D傳感器物理極限精度，后續(xù)根據(jù)實(shí)驗(yàn)反饋可調(diào)整ε取值)，則此時的剛體變換為精確的迭代收斂變換矩陣[R″|T″]。

(10)根據(jù)迭代收斂變換矩陣[R″|T″]計算點(diǎn)云數(shù)據(jù)P′在世界坐標(biāo)系下的對應(yīng)點(diǎn)云數(shù)據(jù)P″，并利用點(diǎn)云數(shù)據(jù)P″進(jìn)行三維重建。

(11)根據(jù)實(shí)時三維重建反饋結(jié)果，判斷三維重建是否完成，即用戶是否出發(fā)終止命令，如果是，終止計算，如果否，繼續(xù)步驟(12)；

(12)將迭代收斂變換矩陣對應(yīng)的位移S″和四元數(shù)q″作為觀測量，通過卡爾曼濾波，對RGB-D傳感器相對于初始時刻的位移優(yōu)化后驗(yàn)估計值和四元數(shù)優(yōu)化后驗(yàn)估計值進(jìn)行估計，然后以優(yōu)化后驗(yàn)估計值和對下一時刻的位移和四元數(shù)的先驗(yàn)估計值做出預(yù)測，得到相對于初始時刻的位移后續(xù)先驗(yàn)估計值與四元數(shù)后續(xù)先驗(yàn)估計值

在步驟(12)中，通過卡爾曼濾波估計優(yōu)化后驗(yàn)估計值和時，將迭代收斂變換矩陣對應(yīng)的位移S″和轉(zhuǎn)動四元數(shù)q″作為觀測矢量代入如步驟(8)所述的卡爾曼濾波公式。

在步驟(12)中，依據(jù)卡爾曼濾波原理得出位移后續(xù)先驗(yàn)估計值與四元數(shù)后續(xù)先驗(yàn)估計值的具體方法為：

其中為先驗(yàn)估計值建立的狀態(tài)矢量，為后驗(yàn)估計值建立的狀態(tài)矢量。A是狀態(tài)轉(zhuǎn)移矩陣，B是控制矩陣，u是系統(tǒng)輸入矢量；k表示該矢量或矩陣是當(dāng)前時刻對應(yīng)的矢量或矩陣，k+1表示該矢量或矩陣是下一時刻對應(yīng)的矢量或矩陣。

該步驟的重要性在于，反饋的觀測量代替單純的慣性數(shù)據(jù)的積分來估計傳感器的速度和位移的優(yōu)化后驗(yàn)估計值，抑制誤差累積導(dǎo)致的發(fā)散，使得對下一時刻的位移和四元數(shù)的先驗(yàn)估計結(jié)果依然可靠，適用于作為配準(zhǔn)的參照值。

(13)將位移后續(xù)先驗(yàn)估計值與四元數(shù)后續(xù)先驗(yàn)估計值轉(zhuǎn)換為后續(xù)先驗(yàn)變換矩陣

其中轉(zhuǎn)動變換矩陣是四元數(shù)后續(xù)先驗(yàn)估計值的矩陣，平移變換矩陣是位移后續(xù)先驗(yàn)估計值的矩陣。

(14)循環(huán)步驟(2)～步驟(11)。

在進(jìn)行一個相對完整的三維重建過程中，本算法的效率相比skanect等市場上主流的三維掃描軟件提高了一倍以上。

以上所述的具體實(shí)施方式對本發(fā)明的技術(shù)方案和有益效果進(jìn)行了詳細(xì)說明，應(yīng)理解的是以上所述僅為本發(fā)明的最優(yōu)選實(shí)施例，并不用于限制本發(fā)明，凡在本發(fā)明的原則范圍內(nèi)所做的任何修改、補(bǔ)充和等同替換等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。