本發(fā)明涉及一種3d點(diǎn)云的數(shù)據(jù)處理方法，特別涉及一種基于立體柵格自動分析3d點(diǎn)云配準(zhǔn)誤差的方法，可用在對配準(zhǔn)的精度要求較高的激光雷達(dá)三維測繪和結(jié)構(gòu)測量領(lǐng)域。

背景技術(shù)：

三維激光掃描技術(shù)可快速高效的獲取被測對象表面的三維坐標(biāo)，目前被廣泛的應(yīng)用于測繪工程，結(jié)構(gòu)測量，文物保護(hù)等領(lǐng)域。但由于三維激光雷達(dá)掃描范圍的局限性，有時需要多測站多角度掃描，或者多個激光雷達(dá)同時掃描。因此，需要通過3d點(diǎn)云數(shù)據(jù)融合和配準(zhǔn)技術(shù)，來將多個點(diǎn)云配準(zhǔn)到同一個坐標(biāo)系下，獲得物體表面完整的信息，實(shí)現(xiàn)對三維場景真實(shí)立體的再現(xiàn)。

針對點(diǎn)云配準(zhǔn)，目前行業(yè)內(nèi)的標(biāo)準(zhǔn)做法是在測量期間放置標(biāo)志物。但是由于手動放置標(biāo)志物該過程非常費(fèi)時，并且當(dāng)這些標(biāo)志物不可避免地遮擋了一些對象和場景或者分布在邊界不清的點(diǎn)云集合中，都會導(dǎo)致配準(zhǔn)的失敗。基于以上原因，越來越多的研究者投入激光雷達(dá)點(diǎn)云自動配準(zhǔn)的研究中來。其中icp算法無需標(biāo)記，且產(chǎn)生了很多高效的變體，應(yīng)用廣泛。

但是由于激光雷達(dá)采樣物體表面產(chǎn)生的點(diǎn)云常常是稀疏不均的，并且雷達(dá)采樣的誤差以及移動的物體，植物等反射表面產(chǎn)生的噪聲都會造成配準(zhǔn)精度下降。多站配準(zhǔn)還會引起誤差傳遞，更重要的是，針對不同的激光雷達(dá)采集的不同場景，配準(zhǔn)算法的效果都有明顯差異。在激光雷達(dá)三維測繪和結(jié)構(gòu)測量領(lǐng)域，比如對空間位置的測量，器件面積和體積的測量，都要求配準(zhǔn)算法具有高精度的特性。王忠武等(圖像融合中配準(zhǔn)誤差的影響【j】，測繪科學(xué)，王忠武，趙忠明，2010(02))實(shí)例表明,配準(zhǔn)誤差對融合質(zhì)量的影響較大,在遙感圖像融合處理中,配準(zhǔn)誤差越小越好。因此，在配準(zhǔn)算法變體繁多的情況下，針對不同的場景選擇最合適的配準(zhǔn)算法就必須研究配準(zhǔn)算法的準(zhǔn)確性。

除此之外，點(diǎn)云數(shù)目大小也會對配準(zhǔn)算法產(chǎn)生影響：

其一是對算法效率的影響：點(diǎn)數(shù)越多，計算效率低。但是點(diǎn)數(shù)越少，并不一定能提高算法效率；

其二是對配準(zhǔn)準(zhǔn)確率的影響，點(diǎn)數(shù)少，則一些包含關(guān)鍵信息和特征的點(diǎn)可能被濾除，從而導(dǎo)致配準(zhǔn)時出現(xiàn)錯誤對應(yīng)點(diǎn)對增多的情況。

目前計算配準(zhǔn)算法誤差的方法大部分都只是在自身配準(zhǔn)方法的基礎(chǔ)上計算，因此不具有通用性和可比較性。

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

在激光雷達(dá)三維測繪和結(jié)構(gòu)測量領(lǐng)域，比如對空間位置的測量，器件面積和體積的測量，都要求配準(zhǔn)算法具有高精度的特性，因此，為了在配準(zhǔn)算法變體繁多的情況下，選擇精度最高的算法構(gòu)建3d場景，本發(fā)明提供一種基于立體柵格自動分析3d點(diǎn)云配準(zhǔn)誤差的方法，利用全局特征子尋找對應(yīng)點(diǎn)，再加權(quán)平均得到最終的誤差。該方法可適應(yīng)稀疏密度不均的點(diǎn)云，并且能夠自動確定鄰域大小，保留足夠多的特征信息，與理論誤差的相關(guān)性也得到了驗(yàn)證；還可以幫助配準(zhǔn)算法在實(shí)際應(yīng)用中確定合適的點(diǎn)云規(guī)模；可以用于但不限于激光雷達(dá)三維測繪和結(jié)構(gòu)測量領(lǐng)域。

本發(fā)明為解決上述技術(shù)問題采用以下技術(shù)方案：

本發(fā)明提供一種基于立體柵格自動分析3d點(diǎn)云配準(zhǔn)誤差的方法，包括如下步驟：

步驟1，通過激光雷達(dá)獲得源點(diǎn)云；

步驟2，獲得配準(zhǔn)完成時的變換矩陣；

步驟3，對步驟1中的源點(diǎn)云進(jìn)行下采樣；

步驟4，對步驟3中下采樣后的源點(diǎn)云進(jìn)行濾波；

步驟5，將濾波后源點(diǎn)云中的每一點(diǎn)乘上變換矩陣，獲得目標(biāo)點(diǎn)云；

步驟6，計算步驟4中濾波后的源點(diǎn)云和步驟5中獲得的目標(biāo)點(diǎn)云中的每一點(diǎn)的曲率，并根據(jù)曲率的大小提取特征點(diǎn)，得到源點(diǎn)云特征點(diǎn)集合以及目標(biāo)點(diǎn)云特征點(diǎn)集合；

步驟7，計算步驟6中提取的源點(diǎn)云和目標(biāo)點(diǎn)云特征點(diǎn)的點(diǎn)特征直方圖特征向量；

步驟8，尋找步驟6中提取的源點(diǎn)云特征點(diǎn)和目標(biāo)點(diǎn)云特征點(diǎn)的對應(yīng)點(diǎn)，基于隨機(jī)采樣一致性算法ransac對尋找到的所有對應(yīng)點(diǎn)對集合進(jìn)行篩選，得到對應(yīng)點(diǎn)對集合，其中隨機(jī)采樣一致性算法ransac的拒絕閾值為激光雷達(dá)的分辨率；

步驟9，計算步驟8中對應(yīng)點(diǎn)對集合中每對對應(yīng)點(diǎn)兩點(diǎn)之間的距離的平均值；

步驟10，若當(dāng)前對應(yīng)點(diǎn)對集合中的對應(yīng)點(diǎn)對數(shù)少于第一次尋找得到的對應(yīng)點(diǎn)對數(shù)的40％，則進(jìn)入步驟11，否則將當(dāng)前隨機(jī)采樣一致性算法ransac的拒絕閾值減去激光雷達(dá)分辨率的1/50作為新的拒絕閾值，返回步驟8；

步驟11，將各次迭代時步驟9得到的距離平均值進(jìn)行加權(quán)平均，即得到最終激光雷達(dá)3d點(diǎn)云配準(zhǔn)的誤差。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，步驟3中對源點(diǎn)云進(jìn)行下采樣，具體為：

3.1，構(gòu)建一個能夠包圍源點(diǎn)云數(shù)據(jù)的最小的三維長方體空間，將該長方體空間均勻劃分為若干個三維體素柵格；

3.2，對每個柵格內(nèi)的每個點(diǎn)，利用利用kd-tree搜索其k近鄰，計算鄰域法向量，并求出該點(diǎn)和其鄰域法向量之間的夾角，其中，k的取值為柵格內(nèi)的點(diǎn)數(shù)；

3.3，對每個柵格，計算該柵格內(nèi)所有點(diǎn)和其鄰域法向量之間夾角的平均值，若平均值大于50°則該柵格的邊長減小為原邊長的一半，否則該柵格的邊長增大為原邊長的兩倍；

3.4，根據(jù)3.3中的柵格邊長，重新劃分3.1中的長方體空間，重復(fù)3.2至3.3，直至所有柵格的邊長保持不變；

3.5，選取每個柵格內(nèi)的質(zhì)心作為關(guān)鍵點(diǎn)來代替該柵格內(nèi)的其他點(diǎn)，從而形成關(guān)鍵點(diǎn)集合，完成源點(diǎn)云的下采樣。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，步驟4中對下采樣后的源點(diǎn)云進(jìn)行濾波，具體為：

4.1，對每個關(guān)鍵點(diǎn)利用kd-tree搜索其k近鄰，計算該關(guān)鍵點(diǎn)與其所有近鄰點(diǎn)的距離的平均值，得到關(guān)鍵點(diǎn)集合對應(yīng)的一個平均值集合；

4.2，計算4.1中平均值集合的標(biāo)準(zhǔn)差以及平均值，其中，該平均值集合的平均值即為全局距離平均值；

4.3，對任一關(guān)鍵點(diǎn)，若該關(guān)鍵點(diǎn)與其所有近鄰點(diǎn)的距離的平均值超出全局距離平均值一倍標(biāo)準(zhǔn)差，則剔除該關(guān)鍵點(diǎn)，得到一個新的關(guān)鍵點(diǎn)集合，從而完成對下采樣后的源點(diǎn)云的濾波。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，步驟6中計算濾波后的源點(diǎn)云和目標(biāo)點(diǎn)云中的每一點(diǎn)的曲率，并根據(jù)曲率的大小提取特征點(diǎn)，具體為：

6.1，分別計算濾波后的源點(diǎn)云中所有點(diǎn)曲率的平均值以及目標(biāo)點(diǎn)云中所有點(diǎn)曲率的平均值；

6.2，選取濾波后的源點(diǎn)云中曲率大于平均值的點(diǎn)作為特征點(diǎn)，得到源點(diǎn)云特征點(diǎn)集合；

6.3，選取目標(biāo)點(diǎn)云中曲率大于平均值的點(diǎn)作為特征點(diǎn)，得到目標(biāo)點(diǎn)云特征點(diǎn)集合。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，步驟8中尋找源點(diǎn)云特征點(diǎn)和目標(biāo)點(diǎn)云特征點(diǎn)的對應(yīng)點(diǎn)，具體為：

8.1，遍歷源點(diǎn)云特征點(diǎn)集合，計算每個源點(diǎn)云特征點(diǎn)與目標(biāo)點(diǎn)云特征點(diǎn)集合中每個點(diǎn)對應(yīng)的特征向量的距離，并計算所有源點(diǎn)云特征點(diǎn)與目標(biāo)點(diǎn)云特征點(diǎn)集合中每個點(diǎn)對應(yīng)的特征向量的距離的平均值；

8.2，遍歷目標(biāo)點(diǎn)云特征點(diǎn)集合，計算每個目標(biāo)點(diǎn)云特征點(diǎn)與源點(diǎn)云特征點(diǎn)集合中每個點(diǎn)對應(yīng)的特征向量的距離，并計算所有目標(biāo)點(diǎn)云特征點(diǎn)與源點(diǎn)云特征點(diǎn)集合中每個點(diǎn)對應(yīng)的特征向量的距離的平均值；

8.3，若lij＜20％lp且nji＜20％lq，其中，lij表示目標(biāo)點(diǎn)云特征點(diǎn)集合中第i個特征點(diǎn)mi與源點(diǎn)云特征點(diǎn)集合中第j個特征點(diǎn)qj對應(yīng)的特征向量的距離，lp表示所有目標(biāo)點(diǎn)云特征點(diǎn)與源點(diǎn)云特征點(diǎn)集合中每個點(diǎn)對應(yīng)的特征向量的距離的平均值，nji表示源點(diǎn)云特征點(diǎn)集合中第j個特征點(diǎn)qj與目標(biāo)點(diǎn)云特征點(diǎn)集合中第i個特征點(diǎn)mi對應(yīng)的特征向量的距離，lq表示所有源點(diǎn)云特征點(diǎn)與目標(biāo)點(diǎn)云特征點(diǎn)集合中每個點(diǎn)對應(yīng)的特征向量的距離的平均值，i∈{1,2,…,mn}，mn表示目標(biāo)點(diǎn)云特征點(diǎn)集合m中的特征點(diǎn)數(shù)，j∈{1,2,…,qn}，qn表示源點(diǎn)云特征點(diǎn)集合q中的特征點(diǎn)數(shù)，則點(diǎn)mi與qj為對應(yīng)點(diǎn)；

8.4，基于隨機(jī)采樣一致性算法ransac對8.3中得到的所有對應(yīng)點(diǎn)對集合進(jìn)行篩選，去除錯誤的對應(yīng)關(guān)系，得到對應(yīng)點(diǎn)對集合。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，步驟11中將各次迭代時步驟9得到的距離平均值進(jìn)行加權(quán)平均，即得到最終激光雷達(dá)3d點(diǎn)云配準(zhǔn)的誤差，具體為：

11.1，各次迭代時步驟9得到的距離平均值按照迭代的先后順序進(jìn)行倒序排序；

11.2，按照11.1的排序依次分配權(quán)值：1/2、1/4、1/8、……、1/2ⁿ，其中，n表示迭代次數(shù)；

11.3，根據(jù)11.2分配的權(quán)值對距離平均值進(jìn)行加權(quán)平均，獲得最終激光雷達(dá)3d點(diǎn)云配準(zhǔn)的誤差。

本發(fā)明采用以上技術(shù)方案與現(xiàn)有技術(shù)相比，具有以下技術(shù)效果：

本發(fā)明提供的一種基于立體柵格自動分析3d點(diǎn)云配準(zhǔn)誤差的方法，采用可變大小的立體柵格進(jìn)行下采樣，保留了更多的特征信息；在立體柵格內(nèi)進(jìn)行濾波，提取特征點(diǎn)，計算點(diǎn)特征直方圖特征向量實(shí)現(xiàn)了自動確定鄰域大小，避免鄰域過小不能反映局部特征，鄰域過大細(xì)節(jié)特征扭曲而造成的對應(yīng)點(diǎn)提取不準(zhǔn)確；同時通過不斷循環(huán)嚴(yán)格化尋找對應(yīng)點(diǎn)的控制變量以及最后階段加權(quán)平均都使該方法能更好的應(yīng)用于實(shí)際環(huán)境中，幫助在激光雷達(dá)三維測繪和結(jié)構(gòu)測量領(lǐng)域提高對物體建模的精度。

附圖說明

圖1為本發(fā)明的方法流程圖；

圖2為velodyne-16雷達(dá)在室內(nèi)長廊采集的一幀源點(diǎn)云和目標(biāo)點(diǎn)云；

圖3為需要進(jìn)行下采樣的源點(diǎn)云；

圖4為圖3中的源點(diǎn)云用本發(fā)明進(jìn)行下采樣的結(jié)果；

圖5為圖4中的源點(diǎn)云用本發(fā)明濾除離群點(diǎn)的結(jié)果；

圖6為稀疏點(diǎn)云對計算配準(zhǔn)誤差的影響(圓形點(diǎn)：采樣點(diǎn)。三角形：雷達(dá)。虛線和實(shí)線表示雷達(dá)在不同位置。圓圈是雷達(dá)的掃描軌跡。由于稀疏點(diǎn)云點(diǎn)云采樣點(diǎn)之間距離大，所以配準(zhǔn)之后采樣點(diǎn)本身存在距離，其中后面長方條的采樣點(diǎn)僅表示隨著物體離雷達(dá)中心距離越遠(yuǎn)，點(diǎn)云更加稀疏)；

圖7為計算理論誤差時構(gòu)造的三種點(diǎn)云；

圖8為理論誤差與本發(fā)明計算出的誤差對比結(jié)果；

圖9為50幀點(diǎn)云理論誤差與本發(fā)明計算出的誤差的相關(guān)關(guān)系；

圖10為本發(fā)明提出的方法分別計算出配準(zhǔn)失敗與成功的誤差的對比圖；

圖11為針對velodyne-16雷達(dá)，icpn與icpf算法誤差的比較；

圖12為針對velodyne-16雷達(dá)，icpf算法在點(diǎn)云數(shù)目不同時配準(zhǔn)的誤差；

圖13為針對velodyne-16雷達(dá)，icpf算法在點(diǎn)云數(shù)目不同時配準(zhǔn)消耗的總時間。

具體實(shí)施方式

下面結(jié)合附圖對本發(fā)明的技術(shù)方案做進(jìn)一步的詳細(xì)說明：

本發(fā)明一種基于立體柵格自動分析3d點(diǎn)云配準(zhǔn)誤差的方法，如圖1所示，包括如下步驟：

步驟1，通過激光雷達(dá)獲得源點(diǎn)云；

步驟2，獲得配準(zhǔn)完成時的變換矩陣；

步驟3，對步驟1中的源點(diǎn)云進(jìn)行下采樣；

步驟4，對步驟3中下采樣后的源點(diǎn)云進(jìn)行濾波；

步驟5，將濾波后源點(diǎn)云中的每一點(diǎn)乘上變換矩陣，獲得目標(biāo)點(diǎn)云；

步驟6，計算步驟4中濾波后的源點(diǎn)云和步驟5中獲得的目標(biāo)點(diǎn)云中的每一點(diǎn)的曲率，并根據(jù)曲率的大小提取特征點(diǎn)，得到源點(diǎn)云特征點(diǎn)集合以及目標(biāo)點(diǎn)云特征點(diǎn)集合；

步驟7，計算步驟6中提取的源點(diǎn)云和目標(biāo)點(diǎn)云特征點(diǎn)的點(diǎn)特征直方圖特征向量；

步驟8，尋找步驟6中提取的源點(diǎn)云特征點(diǎn)和目標(biāo)點(diǎn)云特征點(diǎn)的對應(yīng)點(diǎn)，基于ransac(randomsampleconsensus,隨機(jī)采樣一致性算法)對尋找到的所有對應(yīng)點(diǎn)對集合進(jìn)行篩選，得到對應(yīng)點(diǎn)對集合，其中隨機(jī)采樣一致性算法ransac的拒絕閾值為激光雷達(dá)的分辨率；

步驟9，計算步驟8中對應(yīng)點(diǎn)對集合中每對對應(yīng)點(diǎn)兩點(diǎn)之間的距離的平均值；

步驟10，若當(dāng)前對應(yīng)點(diǎn)對集合中的對應(yīng)點(diǎn)對數(shù)少于第一次尋找得到的對應(yīng)點(diǎn)對數(shù)的40％，則進(jìn)入步驟11，否則將當(dāng)前隨機(jī)采樣一致性算法ransac的拒絕閾值減去激光雷達(dá)分辨率的1/50作為新的拒絕閾值，返回步驟8；

步驟11，將各次迭代時步驟9得到的距離平均值進(jìn)行加權(quán)平均，即得到最終激光雷達(dá)3d點(diǎn)云配準(zhǔn)的誤差。

下面通過具體實(shí)施例對本發(fā)明的技術(shù)方案做進(jìn)一步闡述：

本實(shí)例中，采用的是velodyne公司生產(chǎn)的最小型的16線3維激光雷達(dá)，雷達(dá)在不同的地點(diǎn)和不同的角度采集15幀點(diǎn)云，得到的其中一幀源點(diǎn)云和目標(biāo)點(diǎn)云如圖2所示。

上述步驟3中，由于激光雷達(dá)采樣物體表面產(chǎn)生的點(diǎn)云常常是稀疏不均的：對距離而言，在離雷達(dá)中心越遠(yuǎn)的地方點(diǎn)云越稀疏，而離雷達(dá)中心較近的地方點(diǎn)云又較稠密。對角度而言，不同角度掃描得到的點(diǎn)云數(shù)據(jù)在各自重疊區(qū)域內(nèi)點(diǎn)云密度存在差異，這都會造成算法在搜索鄰域時的半徑大小不具有通用性。因此，在盡可能多的保留點(diǎn)云特征的前提下，通過對點(diǎn)云進(jìn)行下采樣，減少密度的差異性。

如圖3所示的源點(diǎn)云(方位角：90°、仰角：45°)進(jìn)行下采樣具體方法為：首先，求出能夠包圍輸入的三維點(diǎn)云數(shù)據(jù)最小的三維長方體空間，將該長方體空間均勻劃分為一個個三維體素柵格；然后，在每一個柵格內(nèi)，利用kd-tree搜索每個點(diǎn)的k近鄰(k的取值為柵格內(nèi)的點(diǎn)數(shù))，進(jìn)行法向量估計。該點(diǎn)和其鄰域法向量之間夾角的平均值可以表示該點(diǎn)處法向量的變化程度。變化程度大的點(diǎn)表示該區(qū)域起伏變化明顯，所含信息更多，故應(yīng)減少立體柵格的邊長，這樣有更多的柵格來保留較多的數(shù)據(jù)點(diǎn)，若變化程度小，則說明該區(qū)域較為平坦，故應(yīng)增大立體柵格的邊長，該區(qū)域內(nèi)柵格數(shù)目減少僅保留少部分?jǐn)?shù)據(jù)點(diǎn)。體素柵格尺寸的變化除了可以保留更多的特征信息外，對點(diǎn)云密度的平均也有明顯效果。然后，在每一個柵格內(nèi)選取質(zhì)心來代替體素內(nèi)其它點(diǎn)，在這里選取質(zhì)心而不是更常用的體素中心，是因?yàn)橘|(zhì)心可以更好的保留原始空間點(diǎn)的布局。最后，根據(jù)關(guān)鍵點(diǎn)所在柵格的大小更新每個關(guān)鍵點(diǎn)的k鄰域大小，實(shí)現(xiàn)了在后續(xù)操作中自動確定鄰域大小的目的，避免了鄰域過小不能反映局部特征，鄰域過大細(xì)節(jié)特征扭曲而造成的對應(yīng)點(diǎn)提取不準(zhǔn)確的現(xiàn)象。圖4是對圖3所示的源點(diǎn)云進(jìn)行下采樣的結(jié)果(方位角：90°、仰角：45°)。

激光雷達(dá)掃描得到的點(diǎn)云密度不均勻，雷達(dá)采樣的誤差以及移動的物體，植物等反射表面產(chǎn)生的噪聲都會造成點(diǎn)云中存在一些稀疏的離群點(diǎn)。點(diǎn)云中這些點(diǎn)一般分布在雷達(dá)精度之外。例如：vlp雷達(dá)精度在3cm和100m之間。因?yàn)槔走_(dá)近處不能放置物體，故實(shí)例中采集的點(diǎn)云會產(chǎn)生一些極端大值。這些離群點(diǎn)在計算點(diǎn)特征描述子時，會導(dǎo)致計算值異常，因此需要對點(diǎn)云進(jìn)行離群點(diǎn)濾波。

上述步驟4)中對點(diǎn)云濾波的方法為：首先，對每一個關(guān)鍵點(diǎn)利用kd-tree搜索其k近鄰；然后，計算出該關(guān)鍵點(diǎn)與其所有鄰域點(diǎn)的距離和，再取平均得到平均距離；接著，計算出所有關(guān)鍵點(diǎn)與其鄰域點(diǎn)的平均距離，計算這組數(shù)據(jù)的標(biāo)準(zhǔn)差和平均值，這組數(shù)據(jù)的平均值稱為全局距離平均值；最后，當(dāng)某一關(guān)鍵點(diǎn)與其鄰域點(diǎn)的平均距離超出全局距離平均值1個標(biāo)準(zhǔn)差時，該關(guān)鍵點(diǎn)則被去除。圖5為去除離群點(diǎn)的結(jié)果(方位角：0°、仰角：90°)。

上述步驟6中提取特征點(diǎn)的方法為：分別計算濾波后的源點(diǎn)云中所有點(diǎn)曲率的平均值以及目標(biāo)點(diǎn)云中所有點(diǎn)曲率的平均值；選取濾波后的源點(diǎn)云中曲率大于平均值的點(diǎn)作為特征點(diǎn)，得到源點(diǎn)云特征點(diǎn)集合；選取目標(biāo)點(diǎn)云中曲率大于平均值的點(diǎn)作為特征點(diǎn)，得到源點(diǎn)云特征點(diǎn)集合。

對于在點(diǎn)云集合中的每個點(diǎn)，首先，利用kd-tree搜索其k近鄰，計算其鄰域內(nèi)k個點(diǎn)的質(zhì)心，如式(1)所示，其中，ps表示鄰域內(nèi)的第s個點(diǎn)的坐標(biāo)，表示質(zhì)心的坐標(biāo)；然后，計算該點(diǎn)與k鄰域的協(xié)方差矩陣和對應(yīng)的特征值，如式(2)所示，其中，cov表示該點(diǎn)與k鄰域的協(xié)方差矩陣，表示與該協(xié)方差矩陣對應(yīng)的第r個特征向量，λr表示與第r個特征向量對應(yīng)的第r個特征值，由于該點(diǎn)的坐標(biāo)為x、y、z三維坐標(biāo)，故特征向量和特征值只有三個，即r的取值為r∈{0,1,2}；再后，根據(jù)式(3)計算該點(diǎn)對應(yīng)的曲率；最后，求取所有點(diǎn)曲率的平均值，曲率大于平均值的點(diǎn)即選為關(guān)鍵點(diǎn)。

上述步驟8中尋找源點(diǎn)云特征點(diǎn)和目標(biāo)點(diǎn)云特征點(diǎn)的對應(yīng)點(diǎn)的方法為：已知目標(biāo)點(diǎn)云特征點(diǎn)集合m和源點(diǎn)云特征點(diǎn)集合q的點(diǎn)特征直方圖特征向量，首先，針對集合m中的第i個特征點(diǎn)mi，計算mi與集合q中第j個特征點(diǎn)qj對應(yīng)的特征向量的距離lij，i∈{1,2,…,mn}，mn表示目標(biāo)點(diǎn)云特征點(diǎn)集合m中的特征點(diǎn)數(shù)，j∈{1,2,…,qn}，qn表示源點(diǎn)云特征點(diǎn)集合q中的特征點(diǎn)數(shù)；然后，遍歷集合m中的每一個點(diǎn)，得到一組距離值，計算這組距離值的平均值lp；接著，針對集合q中的第j個特征點(diǎn)qj，計算qj與集合m中第i個特征點(diǎn)mi對應(yīng)的特征向量的距離nji；然后，遍歷集合q中的每一個點(diǎn)，得到一組距離值，計算這組距離值的平均值lq；最后，當(dāng)lij＜20％lp且nji＜20％lq，就將該點(diǎn)對(mi,qj)記做對應(yīng)點(diǎn)對。由于噪聲的影響，不是前述所有的對應(yīng)關(guān)系都是合理的，因此，采取基于ransac(randomsampleconsensus,隨機(jī)采樣一致性算法)來去除錯誤的對應(yīng)關(guān)系，其中，隨機(jī)采樣一致性算法ransac的拒絕閾值為激光雷達(dá)的分辨率。

上述步驟11得到最終激光雷達(dá)3d點(diǎn)云配準(zhǔn)的誤差的方法為：首先，將各次迭代時步驟9得到的距離平均值按照迭代的先后順序進(jìn)行倒序排序。因?yàn)殡S著拒絕閾值的嚴(yán)格化，尋找的對應(yīng)點(diǎn)的準(zhǔn)確性也不斷提高。所以根據(jù)排序依次分配權(quán)值為：1/2、1/4、1/8、……、1/2ⁿ，其中，n表示迭代次數(shù)。最后，根據(jù)分配的權(quán)值對距離平均值進(jìn)行加權(quán)平均，獲得最終激光雷達(dá)3d點(diǎn)云配準(zhǔn)的誤差。

在立體柵格內(nèi)進(jìn)行濾波，提取特征點(diǎn)，計算點(diǎn)特征直方圖特征向量實(shí)現(xiàn)了自動確定鄰域大小，避免了鄰域過小不能反映局部特征，鄰域過大細(xì)節(jié)特征扭曲而造成的對應(yīng)點(diǎn)提取不準(zhǔn)確的現(xiàn)象。

以下是本實(shí)例得出的結(jié)果：

由于激光雷達(dá)采集到的兩幀之間的點(diǎn)，本身就存在幾厘米的距離，如圖6所示，其中，圓形點(diǎn)為采樣點(diǎn)，三角形為雷達(dá)，虛線和實(shí)線表示雷達(dá)在不同位置，圓圈是雷達(dá)的掃描軌跡。由于稀疏點(diǎn)云點(diǎn)云采樣點(diǎn)之間距離大，所以配準(zhǔn)之后采樣點(diǎn)本身存在距離，其中，后面長方條的采樣點(diǎn)僅表示隨著物體離雷達(dá)中心距離越遠(yuǎn)，點(diǎn)云更加稀疏。同時，隨著目標(biāo)物體離雷達(dá)的距離越遠(yuǎn)，相差的距離也越大。已知vlp16雷達(dá)激光發(fā)射器每次旋轉(zhuǎn)的角度為0.2°，在一個30m的長廊，相鄰兩幀采樣點(diǎn)之間距離最大可達(dá)30*0.2/180*pi＝10.47cm(這只是最壞的假設(shè)，實(shí)際發(fā)生的概率非常小)。故實(shí)際算法的誤差是比使用本發(fā)明提出的方法計算出的誤差要小的，但是由于激光雷達(dá)點(diǎn)云稀疏是采樣時不可避免的因素，故本發(fā)明提出的方法只能作為一個相對尺度，衡量算法之間的誤差，不能作為一個絕對尺度衡量源點(diǎn)云和配準(zhǔn)后的點(diǎn)云之間的距離。

為了比較配準(zhǔn)算法之間的準(zhǔn)確性，現(xiàn)將本發(fā)明的方法與其他兩種現(xiàn)有方法進(jìn)行比較。第一種配準(zhǔn)方法為icpf，是張建華等人提出的基于3d點(diǎn)云點(diǎn)特征直方圖特征實(shí)時三維空間定位方法(申請?zhí)枺篶n201610659484.x)，將該方法改為用于兩幀點(diǎn)云的配準(zhǔn)，其中，特征描述部分用pca方法代替iss算法，并加入下采樣和濾波操作。第二種配準(zhǔn)方法是為icpn，即將第一種方法的計算點(diǎn)特征直方圖特征向量部分改為計算法向量特征。

為驗(yàn)證該方法的合理性，實(shí)例構(gòu)造了50種理想矩陣(包含旋轉(zhuǎn)和平移)和三種點(diǎn)云，如圖7所示，將理想變換矩陣和實(shí)際變換矩陣(第一種配準(zhǔn)方法得到的變換矩陣)相減，然后將相減得到的矩陣各個元素相加，最后結(jié)果即為理論誤差。實(shí)際配準(zhǔn)中，由于不能事先獲得理想變換矩陣，故不能利用矩陣計算誤差；所以用同樣的數(shù)據(jù)利用本發(fā)明提出的方法測量誤差，結(jié)果如圖8所示。對兩組數(shù)據(jù)進(jìn)行相關(guān)分析，如圖9所示，其相關(guān)系數(shù)達(dá)到了0.8226，屬于顯著相關(guān)，所以該方法可以衡量vlp16激光雷達(dá)實(shí)際應(yīng)用中配準(zhǔn)的誤差。

本實(shí)施例發(fā)現(xiàn)該方法分析誤差的結(jié)果可以分為三個等級衡量配準(zhǔn)情況，相對尺度小于0.001，配準(zhǔn)實(shí)際誤差小于10^-5，屬于配準(zhǔn)結(jié)果理想的等級i。相對尺度小于0.25，配準(zhǔn)實(shí)際誤差小于0.01，屬于配準(zhǔn)結(jié)果一般的等級ii。而配準(zhǔn)失敗時，相對尺度在0.6以上。從圖10可以看出，配準(zhǔn)失敗利用該相對尺度和理想誤差計算出的數(shù)值與配準(zhǔn)成功的數(shù)據(jù)有明顯的區(qū)分度。

實(shí)例采用vlp16雷達(dá)獲得兩幀連續(xù)的點(diǎn)云，兩點(diǎn)云平移距離較小(5cm)，在icpf和icpn都配準(zhǔn)成功的基礎(chǔ)下，采用本發(fā)明提出的方法比較了兩算法的準(zhǔn)確率。在準(zhǔn)確率方面，如圖11所示，兩算法基本相當(dāng)。主要原因是在輸入點(diǎn)云情況較好的情況下，算法準(zhǔn)確率主要是由細(xì)配準(zhǔn)算法來決定，而兩配準(zhǔn)算法的細(xì)配準(zhǔn)方法都為icp方法。

本發(fā)明除了可以針對激光雷達(dá)選擇最優(yōu)的配準(zhǔn)算法構(gòu)建3d場景模型外，還可以優(yōu)化配準(zhǔn)算法，尋找最合適的點(diǎn)云規(guī)模。

算法的時間復(fù)雜度是衡量算法效率的重要指標(biāo)，點(diǎn)云的規(guī)模決定了算法中基本操作重復(fù)執(zhí)行的次數(shù)。然而點(diǎn)云數(shù)目過少，計算特征描述子時易出錯，從而導(dǎo)致配準(zhǔn)失??；

針對第一種配準(zhǔn)方法，實(shí)例將15組不同的源點(diǎn)云，分別采用網(wǎng)格化體素濾波器得到下采樣后的3組點(diǎn)云，點(diǎn)數(shù)分別為10000點(diǎn)，7000點(diǎn)和5000點(diǎn)。比較它們與沒有下采樣的源點(diǎn)云(20000點(diǎn))各自耗費(fèi)的時間以及準(zhǔn)確率。

其中與原點(diǎn)云的比較結(jié)果如圖12所示，可以看出，點(diǎn)云進(jìn)行下采樣可以非常明顯的提高配準(zhǔn)算法的效率。然而，這種明顯的影響效果在點(diǎn)云數(shù)目減少到7000點(diǎn)之后逐漸消失；原因在于點(diǎn)數(shù)減少時，特征描述子估算的時間上升。

點(diǎn)云規(guī)模除了影響算法的效率之外，對配準(zhǔn)的準(zhǔn)確性也有影響。實(shí)例采用本發(fā)明的方法分別計算了點(diǎn)數(shù)為10000點(diǎn)，7000點(diǎn)以及5000點(diǎn)點(diǎn)云配準(zhǔn)的準(zhǔn)確性。從圖13可以看出點(diǎn)云數(shù)目為5000點(diǎn)，配準(zhǔn)準(zhǔn)確率最差。

因此，針對vlp16雷達(dá)實(shí)際應(yīng)用中，采用7000點(diǎn)是綜合效率和配準(zhǔn)準(zhǔn)確率的最佳方案。

以上所述，僅為本發(fā)明中的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉該技術(shù)的人在本發(fā)明所揭露的技術(shù)范圍內(nèi)，可理解想到的變換或替換，都應(yīng)涵蓋在本發(fā)明的包含范圍之內(nèi)，因此，本發(fā)明的保護(hù)范圍應(yīng)該以權(quán)利要求書的保護(hù)范圍為準(zhǔn)。