PCL8叉树应用

一、点云相关的词语解释

    1）体素：如上图所示，一个正方体空间被分割2次，形成64个边长相等的小正方体，这每个小正方体就被称为该八叉树的体素；

    2）深度：深度=正方体被切割的次数+1。如上图所示，一个正方体空间被分割2次，因此，该八叉树的深度就是3；

    3）分辨率：八叉树的分辨率就是体素的边长。如上图所示，假设大正方体的边长为1m，那么，该八叉树的体素的边长是0.25m，即，该八叉树的分辨率就是0.25。注意，PCL中点的距离单位默认就是m，因此，当进行点云压缩时，如果说给定的分辨率参数是5cm，那么，编程传入的参数就是0.05。这是输入体素的最小值和最大值索引，此时会将体素编号获取出来，然后将体素内的所有点的缩影都插入到vector中。

二、8叉树的实现步骤

    1）设定最大递归深度；

    2）找出场景的最大尺寸，并以此尺寸建立第一个立方体；

    3）依序将单位元元素丢入能被包含没有子节点的立方体；

    4）若没有达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体；

    5）若发现子立方体所分配到单位元元素数量不为零且跟父节点立方体是一样的，则该子立方体停止细分，因为根据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则怎么切数目还是一样，会赵成无穷切割的情形。

    6）重复步骤c，直至达到最大递归深度。

三、PCL实现8叉树的常用函数

        1）OctreePointCloudPointVector(等于OctreePointCloud)：该八叉树能够保存每一个叶节点上的点索引列。

        2）OctreePointCloudSinglePoint:该八叉树类仅仅保存每一个叶节点上的单个点索引。仅仅保存最后分配给叶节点的点索引。

        3）OctreePointCloudOccupancy:该八叉树不存储它的叶节点上的任何点信息。他能用于空间填充情况检查。

        4）OctreePointCloudDensity：该八叉树存储每一个叶节点体素中的点的数目。它可以进行空间点集密集程序查询。

四、8叉树相关的K近邻的代码实现

getKNearestNeighborRecursive(const PointT& point,uindex_t K,const BranchNode* node,

         const OctreeKey& key,uindex_t tree_depth,const double squared_search_radius,

         std::vector<prioPointQueueEntry>& point_candidates) const

 {

           std::vector<prioBranchQueueEntry> search_heap;//根据距离排列节点优先级

           search_heap.resize(8);

          // 体素坐标原点

           OctreeKey new_key;

           double smallest_squared_dist = squared_search_radius;

           // get spatial voxel information

          //体素立方体的边长平方作为八叉树深度的函数

           double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth);

           // iterate over all children

   for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {

     if (this->branchHasChild(*node, child_idx)) {

       PointT voxel_center;

       search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));

       search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));

       search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));

       // generate voxel center point for voxel at key

       this->genVoxelCenterFromOctreeKey(

           search_heap[child_idx].key, tree_depth, voxel_center);

       //generate new priority queue element

       search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx);

       search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point);

     }

     else {

       search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity();

     }

   }

   std::sort(search_heap.begin(), search_heap.end());

   // iterate over all children in priority queue

   // check if the distance to search candidate is smaller than the best point distance

   // (smallest_squared_dist)

   while ((!search_heap.empty()) &&

          (search_heap.back().point_distance <

           smallest_squared_dist + voxelSquaredDiameter / 4.0 +

               sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) {

     const OctreeNode* child_node;

     // read from priority queue element

     child_node = search_heap.back().node;

     new_key = search_heap.back().key;

     if (child_node->getNodeType() == BRANCH_NODE) {

       // we have not reached maximum tree depth

       smallest_squared_dist =

           getKNearestNeighborRecursive(point,

                                        K,

                                        static_cast<const BranchNode*>(child_node),

                                        new_key,

                                        tree_depth + 1,

                                        smallest_squared_dist,

                                        point_candidates);

     }

     else {

       // we reached leaf node level

       Indices decoded_point_vector;

       const auto* child_leaf = static_cast<const LeafNode*>(child_node);

       // decode leaf node into decoded_point_vector

       (*child_leaf)->getPointIndices(decoded_point_vector);

       // Linearly iterate over all decoded (unsorted) points

       for (const auto& point_index : decoded_point_vector) {

         const PointT& candidate_point = this->getPointByIndex(point_index);

         // calculate point distance to search point

         float squared_dist = pointSquaredDist(candidate_point, point);

         // check if a closer match is found

         if (squared_dist < smallest_squared_dist) {

           prioPointQueueEntry point_entry;

           point_entry.point_distance_ = squared_dist;

           point_entry.point_idx_ = point_index;

           point_candidates.push_back(point_entry);

         }

       }

       std::sort(point_candidates.begin(), point_candidates.end());

       if (point_candidates.size() > K)

         point_candidates.resize(K);

       if (point_candidates.size() == K)

         smallest_squared_dist = point_candidates.back().point_distance_;

     }

     // pop element from priority queue

     search_heap.pop_back();

   }

   return (smallest_squared_dist);

 }