《點(diǎn)云庫(kù)PCL學(xué)習(xí)教程》第6章 八叉樹

第6章八叉樹建立空間索引在點(diǎn)云數(shù)據(jù)處理中已被廣泛應(yīng)用，常見空間索引一般是自頂向下逐級(jí)劃分空間的各種空間索引結(jié)構(gòu)，比較有代表性的包括BSP樹、KD樹、KDB樹、R樹、R+樹、CELL樹、四叉樹和八叉樹等索引結(jié)構(gòu)，而在這些結(jié)構(gòu)中KD樹和八叉樹在3D點(diǎn)云數(shù)據(jù)組織中應(yīng)用較為廣泛。PCL對(duì)八叉樹的數(shù)據(jù)結(jié)構(gòu)建立和索引方法進(jìn)行了實(shí)現(xiàn)，以方便在此基礎(chǔ)上的其他點(diǎn)云處理操作。本章首先對(duì)常用的點(diǎn)云空間索引方法octree概念進(jìn)行介紹，然后對(duì)PCL的octree相關(guān)模塊及類進(jìn)行簡(jiǎn)單說明，最后通過應(yīng)用實(shí)例來展示如何對(duì)PCL中octree模塊進(jìn)行靈活運(yùn)用。本章各小節(jié)目錄6.1octree概述及相關(guān)算法簡(jiǎn)介6.2PCL中octree模塊及類介紹6.3應(yīng)用實(shí)例解析6.1octree概述及相關(guān)算法簡(jiǎn)介八叉樹結(jié)構(gòu)是由Hunter博士于1978年首次提出的一種數(shù)據(jù)模型。八叉樹結(jié)構(gòu)通過對(duì)三維空間的幾何實(shí)體進(jìn)行體元剖分，每個(gè)體元具有相同的時(shí)間和空間復(fù)雜度，通過循環(huán)遞歸的劃分方法對(duì)大小為2n×2n×2n的三維空間的幾何對(duì)象進(jìn)行剖分，從而構(gòu)成一個(gè)具有根節(jié)點(diǎn)的方向圖。在八叉樹結(jié)構(gòu)中如果被劃分的體元具有相同的屬性，則該體元構(gòu)成一個(gè)葉節(jié)點(diǎn)；否則繼續(xù)對(duì)該體元剖分成8個(gè)子立方體，依次遞歸剖分，對(duì)于2n×2n×2n大小的空間對(duì)象，最多剖分n次，如圖6-1所示。

6.2PCL中octree模塊及類介紹PCL中octree庫(kù)提供了octree數(shù)據(jù)結(jié)構(gòu)，利用FLANN進(jìn)行快速鄰域檢索。鄰域檢索在匹配、特征描述子計(jì)算、領(lǐng)域特征提取中是非?；A(chǔ)的核心操作。octree模塊利用十幾個(gè)類實(shí)現(xiàn)了利用octree數(shù)據(jù)結(jié)構(gòu)對(duì)點(diǎn)云的高效管理和檢索，以及相應(yīng)的一些空間處理算法，例如壓縮、空間變化檢測(cè)，其依賴于pcl_common模塊。octree模塊中類說明目前PCL中octree模塊中目前共有16個(gè)類，以后有可能增加。1.classpcl::octree::Octree2BufBase<DataT,LeafT>類Octree2BufBase實(shí)現(xiàn)了同時(shí)存儲(chǔ)管理兩個(gè)八叉樹于內(nèi)存中，如此，可以非常高效地實(shí)現(xiàn)八叉樹的建立管理等操作，并且該類實(shí)現(xiàn)對(duì)臨近樹節(jié)點(diǎn)結(jié)構(gòu)的變化探測(cè)，對(duì)應(yīng)到空間點(diǎn)云，其就可以對(duì)空間曲面的動(dòng)態(tài)變化進(jìn)行探測(cè)，在進(jìn)行空間動(dòng)態(tài)變化探測(cè)中非常有用，例如目前基于kinect設(shè)備的體感交互應(yīng)用。類Octree2BufBase關(guān)鍵成員函數(shù)：voidsetMaxVoxelIndex(unsignedintmaxVoxelIndex_arg)設(shè)置在各個(gè)維度上最大的體素個(gè)數(shù)。voidsetTreeDepth(unsignedintdepth_arg)設(shè)置八叉樹的深度，需要在初始化八叉樹時(shí)設(shè)置。voidadd(unsignedintidxX_arg,unsignedidxY_arg,unsignedintidxZ_arg,constDataT&data_arg)在idxX、idxY、idxZ對(duì)應(yīng)的葉子節(jié)點(diǎn)上填充DataT的數(shù)據(jù)，其中idxX、idxY、idxZ為在三個(gè)維度上的整型索引值。boolexistLeaf(unsignedintidxX_arg,unsignedintidxY_arg,unsignedintidxZ_arg)const判斷在idxX、idxY、idxZ對(duì)應(yīng)的葉子節(jié)點(diǎn)是否存在，如果存在返回true，否則返回false。unsignedintgetLeafCount()const返回在該八叉樹中的葉子數(shù)目。unsignedintgetBranchCount()const返回在該八叉樹中的分支數(shù)目。voiddeleteTree(boolfreeMemory_arg=false)刪除八叉樹的結(jié)構(gòu)及其葉子節(jié)點(diǎn)。voiddeletePreviousBuffer()刪除另一個(gè)緩沖區(qū)中對(duì)應(yīng)八叉樹的結(jié)構(gòu)及其葉子節(jié)點(diǎn)。voiddeleteCurrentBuffer()刪除當(dāng)前緩沖區(qū)中對(duì)應(yīng)八叉樹的結(jié)構(gòu)和其葉子節(jié)點(diǎn)。voidswitchBuffers()交換緩沖區(qū)，并重設(shè)八叉樹結(jié)構(gòu)。voidserializeTree(std::vector<char>&binaryTreeOut_arg,booldoXOREncoding_arg=false)串行化輸出八叉樹結(jié)構(gòu)到binaryTreeOut_arg向量，doXOREncoding_arg設(shè)置輸出時(shí)是否將當(dāng)前緩沖區(qū)與后臺(tái)緩沖區(qū)中數(shù)據(jù)進(jìn)行異或操作后再輸出，異或操作是兩個(gè)八叉樹結(jié)構(gòu)之間差異數(shù)據(jù)的輸出。voidserializeTree(std::vector<char>&binaryTreeOut_arg,std::vector<DataT>&dataVector_arg,booldoXOREncoding_arg=false)串行化重載函數(shù)，其中參數(shù)dataVector_arg存儲(chǔ)八叉樹中葉子節(jié)點(diǎn)上的數(shù)據(jù)，其他兩個(gè)參數(shù)同上。voidserializeLeafs(std::vector<DataT>&dataVector_arg)參數(shù)dataVector_arg存儲(chǔ)八叉樹中葉子節(jié)點(diǎn)上的數(shù)據(jù)，該函數(shù)只串行化八叉樹中的數(shù)據(jù)。voidserializeNewLeafs(std::vector<DataT>&dataVector_arg,constintminPointsPerLeaf_arg=0)串行化當(dāng)前緩沖區(qū)八叉樹中存在但在后臺(tái)緩沖區(qū)八叉樹中不存在的節(jié)點(diǎn)數(shù)據(jù)，其中，minPointsPerLeaf_arg為需要串行化的節(jié)點(diǎn)中點(diǎn)的最小個(gè)數(shù)，如果點(diǎn)個(gè)數(shù)小于該值則不串行化此新節(jié)點(diǎn)。voiddeserializeTree(std::vector<char>&binaryTreeIn_arg,booldoXORDecoding_arg=false)反串行化，參數(shù)意義上同上面對(duì)應(yīng)的串行化函數(shù)。2.classpcl::octree::OctreeBase<DataT,LeafT,OctreeBranchT>類OctreeBase為八叉樹基類，其關(guān)鍵成員函數(shù)參考其他類介紹。3.classpcl::octree::OctreeBaseWithState<DataT,LeafT>類OctreeBaseWithState為帶有狀態(tài)的八叉樹基類，其中額外存儲(chǔ)的狀態(tài)多用于可見性估計(jì)，同樣其關(guān)鍵成員函數(shù)參考其他類的介紹。4.classpcl::octree::OctreeIteratorBase<DataT,LeafT,OctreeT>類OctreeIteratorBase為八叉樹迭代器的基類，用于深度優(yōu)先或廣度優(yōu)先遍歷八叉樹時(shí)使用。類OctreeIteratorBase關(guān)鍵成員函數(shù)：voidreset()初始化迭代器。constOctreeKey&getCurrentOctreeKey()const獲取當(dāng)前八叉樹節(jié)點(diǎn)對(duì)應(yīng)迭代器的鍵值。unsignedintgetCurrentOctreeDepth()const獲取當(dāng)前八叉樹迭代器對(duì)應(yīng)節(jié)點(diǎn)所在的深度值。constOctreeNode*getCurrentOctreeNode()const獲取當(dāng)前八叉樹節(jié)點(diǎn)。boolisBranchNode()const判斷當(dāng)前節(jié)點(diǎn)是否為分支節(jié)點(diǎn)，是返回true，否則返回false。boolisLeafNode()const判斷當(dāng)前節(jié)點(diǎn)是否為葉子節(jié)點(diǎn)，是返回true，否則返回false。chargetNodeConfiguration()const獲取當(dāng)前節(jié)點(diǎn)的設(shè)置對(duì)應(yīng)的比特位值。virtualvoidgetData(constDataT*&data_arg)const獲取當(dāng)前節(jié)點(diǎn)包含數(shù)據(jù)的首個(gè)元素。virtualvoidgetData(std::vector<DataT>&dataVector_arg)const獲取當(dāng)前節(jié)點(diǎn)對(duì)應(yīng)的向量數(shù)據(jù)。virtualunsignedlonggetNodeID()const獲取當(dāng)前節(jié)點(diǎn)對(duì)應(yīng)的整數(shù)ID。5.Classpcl::octree::OctreeDepthFirstIterator<DataT,LeafT,OctreeT>6.classpcl::octree::OctreeDepthFirstIterator<DataT,LeafT,OctreeT>7.classpcl::octree::OctreeLeafNodeIterator<DataT,LeafT,OctreeT>以上3個(gè)類都繼承于OctreeIteratorBase，分別實(shí)現(xiàn)深度優(yōu)先遍歷、廣度優(yōu)先遍歷、葉子節(jié)點(diǎn)迭代器，關(guān)鍵函數(shù)參考其基類。8.classpcl::octree::OctreePointCloud<PointT,LeafT,OctreeT>類OctreePointCloud為針對(duì)點(diǎn)云實(shí)現(xiàn)的八叉樹數(shù)據(jù)結(jié)構(gòu)與相關(guān)算法，基于該類繼承出多個(gè)子類，實(shí)現(xiàn)不同的點(diǎn)云處理或操作，如圖6-2所示。類OctreePointCloud關(guān)鍵成員函數(shù)：voidsetInputCloud(constPointCloudConstPtr&cloud_arg,constIndicesConstPtr&indices_arg=IndicesConstPtr())設(shè)置八叉樹管理的點(diǎn)云，其中cloud_arg為指向點(diǎn)云對(duì)象的指針，indices_arg為真正需要輸入的點(diǎn)云的索引序列。voidsetEpsilon(doubleeps)設(shè)置近鄰搜索時(shí)的誤差限。voidsetResolution(doubleresolution_arg)設(shè)置為點(diǎn)云建立的八叉樹結(jié)構(gòu)的分辨率，即體素的大小。voidaddPointsFromInputCloud()顯示調(diào)用將點(diǎn)云添加到八叉樹管理結(jié)構(gòu)中。voidaddPointFromCloud(constintpointIdx_arg,IndicesPtrindices_arg)添加對(duì)應(yīng)索引中的點(diǎn)到八叉樹中，其中pointIdx_arg為索引，indices_arg索引序列的指針。voidaddPointToCloud(constPointT&point_arg,PointCloudPtrcloud_arg,IndicesPtrindices_arg)添加點(diǎn)point_arg到點(diǎn)云cloud_arg的indices_arg索引下，同時(shí)添加到八叉樹中。boolisVoxelOccupiedAtPoint(constPointT&point_arg)const判斷點(diǎn)point_arg所處的空間是否存在八叉樹體素中。boolisVoxelOccupiedAtPoint(constdoublepointX_arg,constdoublepointY_arg,constdoublepointZ_arg)const判斷點(diǎn)(pointX_arg,pointY_arg,pointZ_arg)所處的空間是否存在八叉樹體素中。intgetOccupiedVoxelCenters(AlignedPointTVector&voxelCenterList_arg)const獲取所有被點(diǎn)云占據(jù)的體素的中心并存儲(chǔ)在voxelCenterList_arg中，返回值為被占據(jù)的體素的個(gè)數(shù)。intgetApproxIntersectedVoxelCentersBySegment(constEigen::Vector3f&origin,constEigen::Vector3f&end,AlignedPointTVector&voxel_center_list,floatprecision=0.2)用參數(shù)origin和end給定空間一線段，該函數(shù)求得與該線段相交的體素中心，存儲(chǔ)在voxel_center_list，并返回相交體素的個(gè)數(shù)。voiddeleteVoxelAtPoint(constPointT&point_arg)刪除指定點(diǎn)所在的八叉樹所管理的體素或葉子節(jié)點(diǎn)。voiddefineBoundingBox(constdoubleminX_arg,constdoubleminY_arg,constdoubleminZ_arg,constdoublemaxX_arg,constdoublemaxY_arg,constdoublemaxZ_arg)指定八叉樹的包圍盒，參數(shù)為三個(gè)維度的上下限，八叉樹中一旦存儲(chǔ)管理元素了，則包圍盒大小就不能再改變。voidgetBoudingBox(double&minX_arg,double&minY_arg,double&minZ_arg,double&maxX_arg,double&maxY_arg,double&maxZ_arg)const獲取包圍盒3個(gè)維度的上下限。doublegetVoxelSquaredDiameter(unsignedinttreeDepth_arg)const獲取八叉樹中指定深度對(duì)應(yīng)體素的內(nèi)切圓的直徑。doublegetVoxelSquaredSideLen(unsignedinttreeDepth_arg)const獲取八叉樹中指定深度對(duì)應(yīng)體素的立方體的邊長(zhǎng)。9.classpcl::octree::OctreePointCloudChangeDetector<PointT,LeafT>類OctreePointCloudChangeDetector實(shí)現(xiàn)了創(chuàng)建一八叉樹，該八叉樹由新增加的葉子節(jié)點(diǎn)組成，該八叉樹分辨率需要初始化，包圍盒可自適應(yīng)調(diào)整。類OctreePointCloudChangeDetector關(guān)鍵成員函數(shù)，未列出的參考其父類OctreePointCloud：intgetPointIndicesFromNewVoxels(std::vector<int>&indicesVector_arg,constintminPointsPerLeaf_arg=0)獲取緩存區(qū)中新添加的葉子節(jié)點(diǎn)，indicesVector_arg為新添加的葉子的索引向量，intminPointsPerLeaf_arg設(shè)置需要串行化的葉子中應(yīng)該包含點(diǎn)的最小數(shù)目。voidsetEpsilon(doubleeps)設(shè)置近鄰搜索時(shí)的誤差限。voidsetInputCloud(constPointCloudConstPtr&cloud_arg,constIndicesConstPtr&indices_arg=IndicesConstPtr())設(shè)置輸入點(diǎn)云，其中cloud_arg表示輸入的點(diǎn)云對(duì)象指針，indices_arg表示真正作為輸入的點(diǎn)集的索引向量。voidsetResolution(doubleresolution_arg)設(shè)置八叉樹分辨率。voidaddPointsFromInputCloud()添加利用setInputCloud函數(shù)設(shè)置的點(diǎn)云到八叉樹中。10.classpcl::octree::OctreePointCloudDensity<PointT,LeafT,OctreeT>類OctreePointCloudDensity實(shí)現(xiàn)了管理一八叉樹，其葉子節(jié)點(diǎn)并非存儲(chǔ)點(diǎn)云，只是對(duì)處于其葉子體素中的點(diǎn)云個(gè)數(shù)進(jìn)行存儲(chǔ)，即整個(gè)八叉樹的葉子節(jié)點(diǎn)存儲(chǔ)了輸入點(diǎn)云的密度空間分布。類OctreePointCloudDensity關(guān)鍵成員函數(shù)，未列出的參考其父類OctreePointCloud：unsignedintgetVoxelDensityAtPoint(constPointT&point_arg)const返回point_arg點(diǎn)所在的葉子節(jié)點(diǎn)體素的密度，即該點(diǎn)所在的體素中包含點(diǎn)的個(gè)數(shù)。11.classpcl::octree::OctreePointCloudOccupancy<PointT,LeafT,OctreeT>類OctreePointCloudOccupancy實(shí)現(xiàn)了管理一八叉樹，其葉子節(jié)點(diǎn)不存儲(chǔ)任何數(shù)據(jù)，只是對(duì)輸入點(diǎn)云所占據(jù)的空間通過葉子所處的體素來進(jìn)行標(biāo)志，這樣就可以對(duì)點(diǎn)云所占據(jù)空間的情況進(jìn)行評(píng)估和檢測(cè)。類OctreePointCloudOccupancy關(guān)鍵成員函數(shù)，未列出的參考其父類OctreePointCloud：voidsetOccupiedVoxelAtPoint(constPointT&point_arg)在點(diǎn)point_arg所在空間為八叉樹添加葉子節(jié)點(diǎn)，標(biāo)識(shí)其有點(diǎn)云占據(jù)該節(jié)點(diǎn)所在體素空間。voidsetOccupiedVoxelsAtPointsFromCloud(PointCloudPtrcloud_arg)在點(diǎn)集cloud_arg所在空間為八叉樹添加葉子節(jié)點(diǎn)，標(biāo)識(shí)其有點(diǎn)云占據(jù)該節(jié)點(diǎn)所在體素空間。12.classpcl::octree::OctreePointCloudPointVector<PointT,LeafT,OctreeT>類OctreePointCloudPointVector管理一八叉樹，該八叉樹葉子節(jié)點(diǎn)，存儲(chǔ)處在該節(jié)點(diǎn)體素中的點(diǎn)對(duì)應(yīng)的索引向量。類OctreePointCloudOccupancy關(guān)鍵成員函數(shù)，未列出的參考其父類OctreePointCloud：voidsetEpsilon(doubleeps)設(shè)置近鄰搜索時(shí)的誤差限。voidsetInputCloud(constPointCloudConstPtr&cloud_arg,constIndicesConstPtr&indices_arg=IndicesConstPtr())設(shè)置輸入點(diǎn)云，其中cloud_arg表示輸入的點(diǎn)云對(duì)象指針，indices_arg表示真正作為輸入的點(diǎn)集的索引向量。voidsetResolution(doubleresolution_arg)設(shè)置八叉樹分辨率。13.classpcl::octree::OctreePointCloudSinglePoint<PointT,LeafT,OctreeT>類OctreePointCloudSinglePoint管理一八叉樹，該八叉樹葉子節(jié)點(diǎn)，存儲(chǔ)處在該節(jié)點(diǎn)體素中的單個(gè)點(diǎn)的索引，其關(guān)鍵函數(shù)參考類OctreePointCloudPointVector。14.classpcl::octree::OctreePointCloudVoxelCentroid<PointT,LeafT,OctreeT>類OctreePointCloudVoxelCentroid管理一八叉樹，可提供被點(diǎn)云占據(jù)節(jié)點(diǎn)對(duì)應(yīng)體素的中心點(diǎn)坐標(biāo)。類OctreePointCloudVoxelCentroid關(guān)鍵成員函數(shù)，未列出的參考其父類OctreePointCloud：unsignedintgetVoxelCentroids(AlignedPointTVector&voxelCentroidList_arg)返回所有被點(diǎn)云占據(jù)的葉子節(jié)點(diǎn)的中心點(diǎn)組成的向量，存儲(chǔ)在voxelCentroidList_arg中，返回值為返回中心的個(gè)數(shù)。boolgetVoxelCentroidAtPoint(constPointT&point_arg,PointT&voxelCentroid_arg)返回點(diǎn)point_arg對(duì)應(yīng)的節(jié)點(diǎn)體素的中心點(diǎn)，存儲(chǔ)在voxelCentroid_arg中，返回值為true表示操作成功，否則表示操作失敗。boolgetVoxelCentroidAtPoint(constint&pointIdx_arg,PointT&voxelCentroid_arg)返回索引pointIdx_arg對(duì)應(yīng)點(diǎn)所代表的節(jié)點(diǎn)體素中心點(diǎn)，存儲(chǔ)在voxelCentroid_arg中，返回值為true表示成功，否則表示操作失敗。15.Classpcl::octree::OctreePointCloudSearch<PointT,LeafT,OctreeT>類OctreePointCloudSearch實(shí)現(xiàn)了基于八叉樹的點(diǎn)云近鄰高效搜索。類OctreePointCloudSearch關(guān)鍵成員函數(shù)：boolvoxelSearch(constPointT&point,std::vector<int>&pointIdx_data)給定查詢點(diǎn)point，通過point確定其所在的體素，返回體素中所有點(diǎn)的索引存儲(chǔ)在pointIdx_data。boolvoxelSearch(constintindex,std::vector<int>&pointIdx_data)功能同上函數(shù)，區(qū)別是查詢點(diǎn)通過index指定。intnearestKSearch(constPointCloud&cloud,intindex,intk,std::vector<int>&k_indices,std::vector<float>&k_sqr_distances)近鄰搜索，cloud為搜索的點(diǎn)云對(duì)象，index為查詢點(diǎn)的索引，k為搜索返回的近鄰個(gè)數(shù)，k_indices為返回近鄰索引向量，k_sqr_distances存儲(chǔ)近鄰點(diǎn)對(duì)應(yīng)的距離平方向量。intnearestKSearch(constPointT&p_q,intk,std::vector<int>&k_indices,std::vector<float>&k_sqr_distances)功能同上，p_q為指定的查詢點(diǎn)，其他參數(shù)類似。voidapproxNearestSearch(constPointCloud&cloud,intquery_index,int&result_index,float&sqr_distance)近似近鄰搜索，其他參數(shù)同上。intradiusSearch(constPointCloud&cloud,intindex,doubleradius,std::vector<int>&k_indices,std::vector<float>&k_sqr_distances,unsignedintmax_nn=0)獲取查詢點(diǎn)radius半徑內(nèi)的近鄰點(diǎn)集，cloud為搜索的點(diǎn)云對(duì)象，index為查詢點(diǎn)的索引，k為搜索返回的近鄰個(gè)數(shù)，k_indices為返回近鄰索引向量，k_sqr_distances存儲(chǔ)近鄰點(diǎn)對(duì)應(yīng)的距離平方向量，max_nn默認(rèn)為0，如果設(shè)置就返回半徑內(nèi)鄰域個(gè)數(shù)上限，返回值為返回領(lǐng)域點(diǎn)的個(gè)數(shù)。intgetIntersectedVoxelCenters(Eigen::Vector3forigin,Eigen::Vector3fdirection,AlignedPointTVector&voxelCenterList)const給定經(jīng)過點(diǎn)origin指向direction的直線，返回與該直線相交的點(diǎn)云對(duì)應(yīng)八叉樹的體素中心點(diǎn)組成的向量，并存儲(chǔ)在voxelCenterList中，返回值為相交體素個(gè)數(shù)。intgetIntersectedVoxelIndices(Eigen::Vector3forigin,Eigen::Vector3fdirection,std::vector<int>&k_indices)const功能同上函數(shù)，k_indices存儲(chǔ)相交體素的索引。intboxSearch(constEigen::Vector3f&min_pt,constEigen::Vector3f&max_pt,std::vector<int>&k_indices)const搜索處于指定立方體內(nèi)的點(diǎn)集，min_pt、max_pt指定立方體的左前下角坐標(biāo)及右后上方坐標(biāo)來定義立方體，k_indices存儲(chǔ)落在立方體內(nèi)的點(diǎn)的索引。voidsetInputCloud(constPointCloudConstPtr&cloud_arg,constIndicesConstPtr&indices_arg=IndicesConstPtr())指定構(gòu)建八叉樹的點(diǎn)云。voidsetEpsilon(doubleeps)設(shè)置搜索時(shí)的精度。voidsetResolution(doubleresolution_arg)設(shè)置八叉樹的體素分辨率。voidaddPointsFromInputCloud()顯示調(diào)用將點(diǎn)云添加到八叉樹管理結(jié)構(gòu)中。voidaddPointFromCloud(constintpointIdx_arg,IndicesPtrindices_arg)添加對(duì)應(yīng)索引中的點(diǎn)到八叉樹中，其中pointIdx_arg為索引，indices_arg索引序列的指針。voidaddPointToCloud(constPointT&point_arg,PointCloudPtrcloud_arg,IndicesPtrindices_arg)添加點(diǎn)point_arg到點(diǎn)云cloud_arg的indices_arg索引下，同時(shí)添加到八叉樹中。boolisVoxelOccupiedAtPoint(constPointT&point_arg)const判斷點(diǎn)point_arg所處的空間是否存在于八叉樹體素中。boolisVoxelOccupiedAtPoint(constdoublepointX_arg,constdoublepointY_arg,constdoublepointZ_arg)const判斷點(diǎn)(pointX_arg,pointY_arg,pointZ_arg)所處的空間是否存在于八叉樹體素中。intgetOccupiedVoxelCenters(AlignedPointTVector&voxelCenterList_arg)const返回所有被點(diǎn)云占據(jù)的體素的中心存儲(chǔ)在voxelCenterList_arg中，返回值為被占據(jù)的體素的個(gè)數(shù)。intgetApproxIntersectedVoxelCentersBySegment(constEigen::Vector3f&origin,constEigen::Vector3f&end,AlignedPointTVector&voxel_center_list,floatprecision=.2)用參數(shù)origin和end給定空間一線段，該函數(shù)求得與該線段相交的體素中心，存儲(chǔ)在voxel_center_list，并返回相交體素的個(gè)數(shù)。voiddeleteVoxelAtPoint(constPointT&point_arg)刪除指定點(diǎn)所在的八叉樹中管理的體素或葉子節(jié)點(diǎn)。voiddefineBoundingBox(constdoubleminX_arg,constdoubleminY_arg,constdoubleminZ_arg,constdoublemaxX_arg,constdoublemaxY_arg,constdoublemaxZ_arg)指定八叉樹的包圍盒，參數(shù)為3個(gè)維度的上下限，八叉樹中一旦存儲(chǔ)管理元素了，則包圍盒大小就不能再改變。voidgetBoudingBox(double&minX_arg,double&minY_arg,double&minZ_arg,double&maxX_arg,double&maxY_arg,double&maxZ_arg)const獲取包圍盒子3個(gè)維度的上下限。doublegetVoxelSquaredDiameter(unsignedinttreeDepth_arg)const獲取八叉樹中指定深度對(duì)應(yīng)體素的內(nèi)切圓的直徑。doublegetVoxelSquaredSideLen(unsignedinttreeDepth_arg)const獲取八叉樹中指定深度對(duì)應(yīng)體素的立方體的邊長(zhǎng)。6.3應(yīng)用實(shí)例解析6.3.1在PCL中如何實(shí)現(xiàn)點(diǎn)云壓縮點(diǎn)云由海量的數(shù)據(jù)集組成，這些數(shù)據(jù)集通過距離、顏色、法線等附加信息來描述空間三維點(diǎn)。此外，點(diǎn)云能以非常高的速率被創(chuàng)建出來，因此需要占用相當(dāng)大的存儲(chǔ)資源，一旦點(diǎn)云需要存儲(chǔ)或者通過速率受限制的通信信道進(jìn)行傳輸，提供針對(duì)這種數(shù)據(jù)的壓縮方法就變得十分有用。PCL庫(kù)提供了點(diǎn)云壓縮功能，它允許編碼壓縮所有類型的點(diǎn)云，如圖6-3所示，包括“無序”點(diǎn)云，它具有無參考點(diǎn)和變化的點(diǎn)尺寸、分辨率、分布密度和點(diǎn)順序等結(jié)構(gòu)特征。而且，底層的octree數(shù)據(jù)結(jié)構(gòu)允許從幾個(gè)輸入源高效地合并點(diǎn)云數(shù)據(jù)。下面解釋單個(gè)點(diǎn)云和點(diǎn)云數(shù)據(jù)流是如何高效壓縮的，在給出的例子中用PCL點(diǎn)云壓縮技術(shù)來壓縮用OpenNIGrabber抓取到的點(diǎn)云。在本書提供光盤的第6章例1文件夾中打開名為point_cloud_compression.cpp的代碼文件。1.代碼解釋說明下面詳細(xì)解析打開的源代碼。從主函數(shù)開始首先創(chuàng)建一個(gè)新的SimpleOpenNIViewer實(shí)例并調(diào)用它的run()方法。intmain(intargc,char**argv){SimpleOpenNIViewerv;v.run();return(0);}在run()函數(shù)中創(chuàng)建PointCloudCompression類的對(duì)象來編碼和解碼，這些對(duì)象把壓縮配置文件作為配置壓縮算法的參數(shù)，所提供的壓縮配置文件為OpenNI兼容設(shè)備采集到的點(diǎn)云預(yù)先確定的通用參數(shù)集。本例中使用MED_RES_ONLINE_COMPRESSION_WITH_COLOR配置參數(shù)集，它應(yīng)用5mm的編碼精度并且允許彩色紋理成分編碼，并進(jìn)一步優(yōu)化，用于快速在線壓縮。壓縮配置文件的完整列表及其配制方法可以在文件“/io/include/pcl/compression/compression_profiles.h”中找到。在PointCloudCompression構(gòu)造函數(shù)中使用MANUAL_CONFIGURATION屬性就可以手動(dòng)設(shè)置壓縮算法全部參數(shù)。boolshowStatistics=true;//設(shè)置在標(biāo)準(zhǔn)設(shè)備上輸出打印出壓縮結(jié)果信息//壓縮選項(xiàng)詳見/io/include/pcl/compression/compression_profiles.h3pcl::octree::compression_Profiles_ecompressionProfile=pcl::octree::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;//初始化壓縮與解壓縮對(duì)象，其中壓縮對(duì)象需要設(shè)定壓縮參數(shù)選項(xiàng)，解壓縮按照數(shù)據(jù)源自行判斷pointCloudEncoder=newpcl::octree::PointCloudCompression<pcl::PointXYZRGBA>(compressionProfile,showStatistics);pointCloudDecoder=newpcl::octree::PointCloudCompression<pcl::PointXYZRGBA>();下面的代碼為OpenNI兼容設(shè)備實(shí)例化一個(gè)新的采集器，并且啟動(dòng)循環(huán)回調(diào)接口，每從設(shè)備獲取一幀數(shù)據(jù)就調(diào)用回調(diào)函數(shù)一次，這里的回調(diào)函數(shù)實(shí)現(xiàn)數(shù)據(jù)壓縮和可視化解壓縮結(jié)果。//創(chuàng)建從OpenNI獲取點(diǎn)云的抓取對(duì)象pcl::Grabber*interface=newpcl::OpenNIGrabber();boost::function<void(constpcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)>f=boost::bind(&SimpleOpenNIViewer::cloud_cb_,this,_1);//建立回調(diào)函數(shù)//建立回調(diào)函數(shù)與回調(diào)信號(hào)之間綁定boost::signals2::connectionc=interface->registerCallback(f);//開始接收點(diǎn)云數(shù)據(jù)流interface->start();while(!viewer.wasStopped()){sleep(1);}interface->stop();在OpenNIGrabber采集循環(huán)執(zhí)行的回調(diào)函數(shù)cloud_cb_中，首先把獲取到的點(diǎn)云壓縮到stringstream緩沖區(qū)，下一步是解壓縮，它對(duì)壓縮了的二進(jìn)制數(shù)據(jù)進(jìn)行解碼，存儲(chǔ)在新的點(diǎn)云對(duì)象中，解碼了的點(diǎn)云被發(fā)送到點(diǎn)云可視化對(duì)象中進(jìn)行實(shí)時(shí)可視化。voidcloud_cb_(constpcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&cloud){if(!viewer.wasStopped()){std::stringstreamcompressedData;//存儲(chǔ)壓縮點(diǎn)云的字節(jié)流對(duì)象pcl::PointCloud<pcl::PointXYZRGBA>::PtrcloudOut//存儲(chǔ)輸出點(diǎn)云(newpcl::PointCloud<pcl::PointXYZRGBA>());PointCloudEncoder->encodePointCloud(cloud,compressedData);//壓縮點(diǎn)云PointCloudDecoder->decodePointCloud(compressedData,cloudOut);//解壓縮點(diǎn)云viewer.showCloud(cloudOut);//可視化解壓縮點(diǎn)云}}//在壓縮與解壓縮過程中，因?yàn)樵O(shè)置compressedData為true所以在標(biāo)準(zhǔn)輸出上打印出壓縮率幀數(shù)等信息2.編譯并運(yùn)行該程序利用光盤提供的CMakeLists.txt文件，在CMake中建立工程文件并生成相應(yīng)的可執(zhí)行文件，生成執(zhí)行文件后，就可以運(yùn)行了，在CMD中鍵入命令：…>point_cloud_compression.exe可以看到圖6-4所示結(jié)果，左邊為帶有RGB紋理信息的實(shí)時(shí)可視化結(jié)果，用戶縮放可視化結(jié)果可以看到經(jīng)過壓縮后點(diǎn)云進(jìn)行了重采樣，紋理信息有所丟失，但數(shù)據(jù)量有所減小，在實(shí)際應(yīng)用中需折中取舍。右邊則為實(shí)時(shí)壓縮信息輸出，可以看出壓縮的幀數(shù)、點(diǎn)數(shù)、壓縮率等信息。3.壓縮配置文件壓縮配置文件為PCL點(diǎn)云編碼器定義了參數(shù)集，并針對(duì)壓縮從OpenNI采集器獲取的普通點(diǎn)云進(jìn)行了優(yōu)化設(shè)置。注意，解碼對(duì)象不需要用參數(shù)表示，因?yàn)樗诮獯a時(shí)檢測(cè)并獲取對(duì)應(yīng)的編碼參數(shù)配置。下面的壓縮配置文件是可用的：LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR：分辨率1cm，無顏色，快速在線編碼LOW_RES_ONLINE_COMPRESSION_WITH_COLOR：分辨率1cm，有顏色，快速在線編碼MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR：分辨率5mm，無顏色，快速在線編碼MED_RES_ONLINE_COMPRESSION_WITH_COLOR：分辨率5mm，有顏色，快速在線編碼HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR：分辨率1mm，無顏色，快速在線編碼HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR：分辨率1mm，有顏色，快速在線編碼333333LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR：分辨率1cm，無顏色，高效離線編碼LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR：分辨率1cm，有顏色，高效離線編碼MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR：分辨率5mm，無顏色，高效離線編碼MED_RES_OFFLINE_COMPRESSION_WITH_COLOR：分辨率5mm，有顏色，高效離線編碼HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR：分辨率5mm，無顏色，高效離線編碼HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR：分辨率5mm，有顏色，高效離線編碼MANUAL_CONFIGURATION允許為高級(jí)參數(shù)化進(jìn)行手工配置。3333334.高級(jí)參數(shù)化為了能完全控制壓縮相關(guān)的參數(shù)，PointCloudCompression類的構(gòu)造函數(shù)可以在初始化時(shí)附加壓縮參數(shù)。請(qǐng)注意，為了啟用高級(jí)參數(shù)化，compressionProfile_arg參數(shù)需要被設(shè)置成MANUAL_CONFIGURATION。PointCloudCompression(compression_Profiles_ecompressionProfile_arg,boolshowStatistics_arg,constdoublepointResolution_arg,constdoubleoctreeResolution_arg,booldoVoxelGridDownDownSampling_arg,constunsignedintiFrameRate_arg,booldoColorEncoding_arg,constunsignedcharcolorBitResolution_arg)下面解釋高級(jí)參數(shù)化設(shè)置：compressionProfile_arg:為了啟用高級(jí)參數(shù)化，該參數(shù)應(yīng)該被設(shè)置成MANUAL_CONFIGURATION。showStatistics_arg：把壓縮相關(guān)的統(tǒng)計(jì)信息打印到標(biāo)準(zhǔn)輸出。pointResolution_arg：定義點(diǎn)坐標(biāo)的編碼精度，該參數(shù)應(yīng)該設(shè)置成小于傳感器精度的一個(gè)值。octreeResolution_arg：該參數(shù)定義展開了的octree的體素大小，較大的體素分辨率使得壓縮更快，但是壓縮質(zhì)量下降，這在較高的幀速率（上傳速率）和壓縮效率中間進(jìn)行了折中設(shè)置。doVoxelGridDownDownSampling_arg：如果激活該參數(shù)，那么只編碼分層octree的數(shù)據(jù)結(jié)構(gòu)，解碼對(duì)象在體素中心生成點(diǎn)，通過這種方法點(diǎn)云在壓縮期間被下采樣，同時(shí)達(dá)到了較高的壓縮性能。iFrameRate_arg：點(diǎn)云壓縮模式對(duì)點(diǎn)云進(jìn)行差分編碼壓縮，用這種方法對(duì)新引入的點(diǎn)云和之前編碼的點(diǎn)云之間的差分進(jìn)行編碼，以便獲得最大壓縮性能，用這種方法對(duì)新引入的點(diǎn)云和之前編碼的點(diǎn)云之間的差分進(jìn)行編碼，以便獲得最大壓縮性能，iFrameRate_arg允許指定數(shù)據(jù)流中的某一幀速率，在這一速率下傳輸?shù)狞c(diǎn)云就不進(jìn)行差分編碼壓縮（同視頻編碼中的I/P幀類似）。doColorEncoding_arg：該選項(xiàng)啟用彩色紋理成分編碼壓縮。colorBitResolution_arg：該參數(shù)定義每一個(gè)彩色成分編碼后所占的位數(shù)。5.PCL點(diǎn)云數(shù)據(jù)流壓縮的命令行工具PCL應(yīng)用程序工具中包含點(diǎn)云流數(shù)據(jù)壓縮命令行工具openni_stream_compression.exe，用戶可以查看選項(xiàng)的完整列表（注意：屏幕上的輸出可能不同）。該工具可以在安裝好的PCL的bin目錄下找到。用戶可以自行試驗(yàn)看看其強(qiáng)大的功能，具體參看其命令行幫助提示。例如它可以通過網(wǎng)絡(luò)進(jìn)行點(diǎn)云壓縮傳輸。為了通過TCP/IP傳輸壓縮點(diǎn)云，可以用下面的命令啟動(dòng)服務(wù)器：…>openni_stream-compression.exe–s它會(huì)監(jiān)聽6666端口看是否有接入鏈接請(qǐng)求，用下面的命令開啟客戶端：…>openni_stream_compression–cSERVER_NAME遠(yuǎn)程采集到的點(diǎn)云可以通過點(diǎn)云查看工具在本地顯示，筆者測(cè)試結(jié)果如圖6-5所示。6.3.2基于octree的空間劃分及搜索操作octree是一種用于管理稀疏3D數(shù)據(jù)的樹狀數(shù)據(jù)結(jié)構(gòu)，每個(gè)內(nèi)部節(jié)點(diǎn)都正好有8個(gè)子節(jié)點(diǎn)，本小節(jié)中將介紹如何用octree在點(diǎn)云數(shù)據(jù)中進(jìn)行空間劃分及近鄰搜索，特別解釋了如何完成“體素內(nèi)近鄰搜索（NeighborswithinVoxelSearch）”、“K近鄰搜索（KNearestNeighborSearch）”和“半徑內(nèi)近鄰搜索（NeighborswithinRadiusSearch）”。在本書提供光盤的第6章例2文件夾中，打開名為octree_search.cpp的代碼文件。1.代碼解釋說明下面解析打開的源代碼，首先定義并實(shí)例化一個(gè)PointCloud指針對(duì)象，并且用隨機(jī)點(diǎn)集賦值給它。pcl::PointCloud<pcl::PointXYZ>::Ptrcloud(newpcl::PointCloud<pcl::PointXYZ>);//創(chuàng)建點(diǎn)云數(shù)據(jù)cloud->width=1000;cloud->height=1;cloud->points.resize(cloud->width*cloud->height);for(size_ti=0;i<cloud->points.size();++i)//循環(huán)隨機(jī)產(chǎn)生點(diǎn)坐標(biāo)值{cloud->points[i].x=1024.0f*rand()/(RAND_MAX+1.0f);cloud->points[i].y=1024.0f*rand()/(RAND_MAX+1.0f);cloud->points[i].z=1024.0f*rand()/(RAND_MAX+1.0f);}然后創(chuàng)建一個(gè)octree實(shí)例，用設(shè)置分辨率進(jìn)行初始化，該octree用它的葉節(jié)點(diǎn)存放點(diǎn)索引向量，該分辨率參數(shù)描述最低一級(jí)octree的最小體素的尺寸，因此octree的深度是分辨率和點(diǎn)云空間維數(shù)的函數(shù)，如果知道點(diǎn)云的邊界框，應(yīng)該用defineBoundingBox方法把它分配給octree，然后通過點(diǎn)云指針把所有點(diǎn)增加到octree中。floatresolution=128.0f;pcl::octree::OctreePointCLoudSearch<pcl::PointXYZ>octree(resolution);//初始化octreeoctree.setInputCloud(cloud);//設(shè)置輸入點(diǎn)云octree.addPointsFromInputCloud();//構(gòu)建octree一旦PointCloud和octree聯(lián)系在一起，就能進(jìn)行搜索操作，這里使用的第一種搜索方法是“體素近鄰搜索”，它把查詢點(diǎn)所在的體素中其他點(diǎn)的索引作為查詢結(jié)果返回，結(jié)果以點(diǎn)索引向量的形式保存，因此搜索點(diǎn)和搜索結(jié)果之間的距離取決于octree的分辨率參數(shù)。std::vector<int>pointIdxVec;//存儲(chǔ)體素近鄰搜索的結(jié)果向量if(octree.voxelSearch(searchPoint,pointIdxVec))//執(zhí)行搜索，返回結(jié)果到pointIdxVec{std::cout<<“Neighborswithinvoxelsearchat(”<<searchPoint.x<<““<<searchPoint.y<<““<<searchPoint.z<<“)”<<std::endl;for(size_ti=0;i<pointIdxVec.size();++i)//打印搜索結(jié)果點(diǎn)坐標(biāo)std::cout<<““<<cloud->points[pointIdxVec[i]].x<<““<<cloud->points[pointIdxVec[i]].y<<““<<cloud->points[pointIdxVec[i]].z<<std::endl;}接下來介紹K近鄰搜索，本例中K被設(shè)置成10，“K近鄰搜索”方法把搜索結(jié)果寫到兩個(gè)分開的向量中，第一個(gè)pointIdxNKNSearch包含搜索結(jié)果（結(jié)果點(diǎn)的索引的向量），第二個(gè)向量保存相應(yīng)的搜索點(diǎn)和近鄰之間的距離平方。//K近鄰搜索intK=10;std::vector<int>pointIdxNKNSearch;//存儲(chǔ)k近鄰搜索點(diǎn)索引結(jié)果std::vector<float>pointNKNSquaredDistance;std::cout<<“Knearestneighborsearchat(“<<searchPoint.x<<““<<searchPoint.y<<““<<searchPoint.z<<“)withK=”<<K<<std::endl;if(octree.nearestKSearch(searchPoint,K,pointIdxNKNSearch,pointNKNSquaredDistance)>0){for(size_ti=0;i<pointIdxNKNSearch.size();++i)//打印搜索結(jié)果點(diǎn)坐標(biāo)Std::cout<<““<<cloud->points[pointIdxNKNSearch[i]].x<<““<<cloud->points[pointIdxNKNSearch[i]].y<<““<<cloud->points[pointIdxNKNSearch[i]].z<<“(squareddistance:”<<pointNKNSquaredDistance[i]<<“)”<<std::endl;}“半徑內(nèi)近鄰搜索”原理和“K近鄰搜索”類似，它的搜索結(jié)果被寫入兩個(gè)分開的向量中，這兩個(gè)向量分別存儲(chǔ)結(jié)果點(diǎn)的索引和對(duì)應(yīng)的距離平方。//半徑內(nèi)近鄰搜索std::vector<int>pointIdxRadiusSearch;std::vector<float>pointRadiusSquaredDistance;floatradius=256.0f*rand()/(RAND_MAX+1.0f);std::cout<<“Neighborswithinradiussearchat(”<<searchPoint.x<<““<<searchPoint.y<<““<<searchPoint.z<<“)withradius=”<<radius<<std::endl;if(octree.radiusSearch(searchPoint,radius,pointIdxRadiusSearch,pointRadiusSquaredDistance)>0){for(size_ti=0;i<pointIdxRadiusSearch.size();++i)Std::cout<<““<<cloud->points[pointIdxRadiusSearch[i]].x<<““<<cloud->points[pointIdxRadiusSearch[i]].y<<““<<cloud->points[pointIdxRadiusSearch[i]].z<<“(squareddistance:”<<pointRadiusSquaredDistance[i]<<“)”<<std::endl;}2.編譯并運(yùn)行該程序利用光盤提供的CMakeLists.txt文件，在CMake中建立工程文件，并生成相應(yīng)的可執(zhí)行文件，生成執(zhí)行文件后就可以運(yùn)行了，在CMD中鍵入命令：…>octreesearch.exe運(yùn)行結(jié)果如圖6-6所示，分別打印出不同搜索方式的輸出結(jié)果。3.Octree部分類關(guān)鍵點(diǎn)說明PCLoctree組件提供了幾個(gè)octree類型。它們各自的葉節(jié)點(diǎn)特征基本上是不同的。OctreePointCloudPointVector(等于OctreePointCloud)：該octree能夠保存每一個(gè)葉節(jié)點(diǎn)上的點(diǎn)索引列。OctreePointCloudSinglePoint：該octree類僅僅保存每一個(gè)葉節(jié)點(diǎn)上的單個(gè)點(diǎn)索引。僅僅保存最后分配給葉節(jié)點(diǎn)的點(diǎn)索引。OctreePointCloudOccupancy：該octree不存儲(chǔ)它的葉節(jié)點(diǎn)上的任何點(diǎn)信息。它能用于空間填充情況檢查。OctreePointCloudDensity：該octree存儲(chǔ)每一個(gè)葉節(jié)點(diǎn)體素中點(diǎn)的數(shù)目。它可以進(jìn)行空間點(diǎn)集密集程度查詢。如果需要高頻率創(chuàng)建octree，請(qǐng)參看octree雙緩沖技術(shù)實(shí)現(xiàn)（Octree2BufBase類）。該類在內(nèi)存中同時(shí)保存了兩個(gè)類似的octree對(duì)象。除了搜索操作之外也可以用于空間變化檢測(cè)。此外，高級(jí)內(nèi)存管理減少了octree建立過程中的內(nèi)存分配和釋放操作。雙緩沖技術(shù)對(duì)octree的實(shí)現(xiàn)可以通過設(shè)置模板參數(shù)“OctreeT”實(shí)例化不同的OctreePointCloud類。所有的octree都支持octree結(jié)構(gòu)和octree內(nèi)容的串行化和反串行化。6.3.3無序點(diǎn)云數(shù)據(jù)集的空間變化檢測(cè)octree是一種用于管理稀疏3D數(shù)據(jù)的樹狀數(shù)據(jù)結(jié)構(gòu)，本小節(jié)中將介紹如何利用octree實(shí)現(xiàn)用于多個(gè)無序點(diǎn)云之間的空間變化檢測(cè)，這些點(diǎn)云可能在尺寸、分辨率、密度和點(diǎn)順序等方面有所差異。通過遞歸地比較octree的樹結(jié)構(gòu)，可以鑒定出由octree產(chǎn)生的體素組成之間的區(qū)別所代表的空間變化，此外還解釋了如何使用PCL的octree“雙緩沖”技術(shù)，以便能實(shí)時(shí)地探測(cè)多個(gè)點(diǎn)云之間的空間組成差異。在本書提供光盤的第6章例3文件夾中，打開名為octree_change_detection.cpp的代碼文件。1.代碼解釋說明首先實(shí)例化OctreePointCloudChangeDetector類，并定義它的體素分辨率。srand((unsignedint)time(NULL));floatresolution=32.0f;//八叉樹分辨率即體素的大小//初始化空間變化檢測(cè)對(duì)象pcl::octree::OctreeP