点云处理算法核心-八叉树
一直想写一篇关于八叉树的博客，我的博客大概也快一年没更新了。八叉树的重要性其实不用我再次强调了吧，它涉及到算法的方方面面吧，也是三维点云数据处理的一个重要基石，从显示到交互再到算法八叉树无不扮演着极其重要的角色。当然并不是任何算法都会涉及到八叉树，将三维点云数据处理成二维有时也是一种比较常见的处理手段，毕竟二维的邻域处理要比三维的邻域处理简单的多。对于八叉树的研究博主本人其实很早就提上了日程，也付诸了相应的行动，一直想写一篇博客分享这方面的知识，一方面它涉及到的篇幅较大，所以每次一想到要写很久就放弃了。
对于八叉树，我想大家首先可能接触到的就是pcl的八叉树的邻域搜索，短短几句代码就实现了查找邻域的功能，我一开始觉得还挺好用的，但是用久了，不免产生了一个疑问，八叉树的具体实现机制是什么，但是网上似乎很少有关于八叉树的精细讲解，见得最多就是那个八叉树的树状图，此时唯一可行的办法，那就只能硬着头皮上源码了。
对于八叉树的印象，我想下面这个图大家见得是最多的。
typedef pcl::octree::OctreeLeafNode<OctreeContainerPointIndices> LeafNode;
typedef pcl::octree::OctreeBranchNode<OctreeContainerEmpty>BranchNode;
节点分为两种类型，根据英文名，博主姑且将其翻译为分支节点以及叶子节点，上图有文字标注，其中绿色的分支节点为空节点。就是我们常说的八叉树的子节点要么为8个，要么为0个，就是因为空节点的存在，空节点之所以存在就是因为其里面不存在数据点，这样也提高了遍历的效率，不然在满树的情况下，节点会多很多。
一、第一部分分块功能
第一步就是八叉树的分层，就是分几层，当然也可以指定八叉树叶子节点（就是盒子套盒子，那个最小的盒子）所对应包围盒的大小。当然分层的逻辑似乎更直观更好理解点。分层的思想其实也不难理解，大家可以理解为盒子里刚好填满8个盒子，直到设定的深度为止。举个例子，如果八叉树的深度为3，先将外包的大盒子分成八个，然后对这8个盒子又分别分成8个，然后再次分成8个。执行3次为止，当然空节点直接跳过。
深度为1 深度为2 深度为3
可以从上图看到，单从z方向这个维度来看，深度每增加一层，在维度上就会一分为二。
二、第二部分邻域搜索
为了使得逻辑清晰点，博主在这里附上了一个流程图，其实就是一个迭代的过程，对待每个分支节点重复流程1的操作。
1.一点包围盒邻域的获取
对于邻域搜索的问题，这里先以一个易于理解的例子切入，我们求一点包围盒内的邻域点。求一点包围盒范围内的邻域点算法实现的思路，即从根节点开始判断，判断每一个根节点是否与所设置的包围盒有交集，如果有继续查找其子节点，同样判断其子节点与该包围盒是否存在交集，直到访问叶子节点，如果该叶子节点与所设置包围盒有交集，便获取该叶子节点里的索引执行判定条件3（对于每个节点的判断无论是分支节点还是叶子节点都会执行一个判定条件，即这里的判定条件1与条件2，这里的条件1与条件2都是2个包围盒是否存在交集）。
流程：
a.这里我们设 预先设置的包围盒为boundingbox1（即我们所需获取的索引的包围盒），当前节点的包围盒为boundingbox2。
b.判定boundingbox1与boundingbox2是否存在交集，即流程图里的判定条件1与条件2（这里的条件是一致的）
c.如果到了叶子节点，且满足b，那么则要执行判定条件3，即判定Pi是否在boundingbox1里
与包围盒有交集的叶子节点 包围盒里的数据点 整体效果
2.一点半径邻域的获取
类似于这类思想，我们便可以求一点半径邻域内的数据点，所以可以先将球处理成包围盒子，唯一不同的是处理到叶子节点时，判定条件即为两点之间的点间距
与包围盒有交集的叶子节点 包围盒里的数据点 整体效果
3.一点K邻域的获取
对于k近邻会相对比较复杂一些，因为并不知道那些点距离自己最近，而且这些点可能来自于周围的数个叶子节点，所以算法处理一开始只能通过访问到的第一个叶子节点，然后执行以下操作
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_;
看这段代码就很直观了，升序排序，取到第K个值
对于下一个节点就会以smallest_squared_dist作为一个判断依据，因为总不能所有节点都去访问吧
float disThread = smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt(smallest_squared_dist * voxelSquaredDiameter);
if(search_heap.back().point_distance < disThread)
search_heap.back().point_distance<disThread 满足此条件
voxelSquaredDiameter 代表当前深度节点的包围盒的长度，smallest_squared_dist代表上次叶子节点获取的邻域点里第K个距离值（已经升序排序了，如果没找到K个邻域，那么smallest_squared_dist的值为初始化的值，一般设置较大）
search_heap.back().point_distance 代表当前层次的根节点包围盒的中心点与搜索点的距离
这个公式曾经在一篇论文看的过，具体是什么原理还真忘了，知道的小伙伴评论区留言赐教一番，感激不尽。
当然自己也大概抽象的理解了一下，大概要满足以下这种形式：
search_heap.back().point_distance - δ<smallest_squared_dist => search_heap.back().point_distance<smallest_squared_dist + δ
所以这个判定条件完全是可以自定义的，所以我也没去纠结这个问题了。亦步亦趋也不太好，还是得有一定的原创性。
以往对待一个问题比较爱刨根问底，因为这篇博文在数个月前就琢磨再写了，可惜一拖再拖，今年整个精神状态都不在线，所以这里请允许我偷个懒（惭愧惭愧），这篇博文的插图有点多，所以感觉确实是有点棘手。我对于整个八叉树的理解也远没到细致入微的境界，所以很多细节的地方都得去看源码。
在空闲的时间里，我尽量养成看源码的习惯，争取面面俱到做到有始有终，后续可能会陆续更新我对八叉树的一些理解。
下面来一张插图，不过没什么代表性吧，就是一张很常规的K近邻搜索图。
K近邻搜索结果
三、第三部分一点所在叶子节点的父节点及其兄弟节点
描述：获取一点所在的叶子节点，然后获取该叶子节点所在的父亲节点，并且获取该叶子节点的兄弟节点
1.这里定义八叉树的最大深度为 maxDepth，查询点为Pt
2.那么其父亲节点必定在maxDepth-1层，那么判定条件-----Pt在此层根节点的包围盒内。假设得到该父亲节点且命名为parentNode.
3.获取parentNode的子节点，判断子节点与Pt的空间关系，包含关系的属于当前节点，否则属于其兄弟节点
当然这个原理不是pcl库里面的，是博主自己的一个简单应用吧，逻辑也很简单，包括上文的半径内搜索，也是博主的一个简单应用，期待有更简单更高效的搜索方式的小伙伴们在评论区里共享一番，感激不尽
下面贴上几个示意图吧
红色部分为该节点，剩下为其兄弟节点 白色为父节点 整体效果
到此这篇博文已经结束，属于千呼万唤始出来系列，差不多一年没更新博客了，其实更新博客的速度也代表自己当前的一种状态，不过对于知识技术的认可不应该在何种情况下去打折扣，所以写这篇博客的目的一方面分享一下我在八叉树方面的一些浅显的认知，另一方面也提醒自己知识技能如尊严般重要，毕竟是自己的安身立命之本，因为你永远不知道未来等待自己的会是什么，所以就得把主动权早早的掌握在自己手里。生活得靠自己而不是靠施舍，所以如我一般的草根们还是得清醒的认知这一点。对未来是否无所畏惧其实很大程度取决于当前自己迈出的每一步。
接下来我会继续更新一些知识，当然大部分都来自于一些开源库，例如pcl或者Cloudcompare,又或者meshlab这些。期待下一次能有大的更新，这段时间我会抽本来不多的休息时间去多看看源码跟大家分享相关知识。
代码实现：
#pragma once #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/octree/octree_pointcloud_voxelcentroid.h> #include <pcl/octree/octree_impl.h> #include <pcl/common/time.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/visualization/point_cloud_handlers.h> #include <pcl/visualization/common/common.h> #include <pcl/octree/octree_pointcloud_voxelcentroid.h> #include <pcl/common/centroid.h> #include <pcl/filters/filter.h> #include <vtkRenderer.h> #include <vtkRenderWindow.h> #include <vtkCubeSource.h> #include <vtkCleanPolyData.h> #include "pcl/octree/octree_container.h" using namespace std; using namespace pcl; using namespace octree; namespace zx { typedef PointXYZ PointT; typedef pcl::octree::OctreeLeafNode<OctreeContainerPointIndices> LeafNode; typedef pcl::octree::OctreeBranchNode<OctreeContainerEmpty>BranchNode; float get2ptDis(PointT p1, PointT p2); class prioPointQueueEntry { public: /** \brief Empty constructor */ prioPointQueueEntry() : point_idx_(0), point_distance_(0) { } /** \brief Constructor for initializing priority queue entry. * \param[in] point_idx an index representing a point in the dataset given by \a setInputCloud * \param[in] point_distance distance of query point to voxel center */ prioPointQueueEntry(unsigned int& point_idx, float point_distance) : point_idx_(point_idx), point_distance_(point_distance) { } /** \brief Operator< for comparing priority queue entries with each other. * \param[in] rhs priority queue to compare this against */ bool operator< (const prioPointQueueEntry& rhs) const { return (this->point_distance_ < rhs.point_distance_); } /** \brief Index representing a point in the dataset given by \a setInputCloud. */ int point_idx_; /** \brief Distance to query point. */ float point_distance_; }; class prioBranchQueueEntry { public: /** \brief Empty constructor */ prioBranchQueueEntry() : node(), point_distance(0), key() { } /** \brief Constructor for initializing priority queue entry. * \param _node pointer to octree node * \param _key octree key addressing voxel in octree structure * \param[in] _point_distance distance of query point to voxel center */ prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance) : node(_node), point_distance(_point_distance), key(_key) { } /** \brief Operator< for comparing priority queue entries with each other. * \param[in] rhs the priority queue to compare this against */ bool operator < (const prioBranchQueueEntry rhs) const { return (this->point_distance > rhs.point_distance); } /** \brief Pointer to octree node. */ /*const*/ OctreeNode* node; /** \brief Distance to query point. */ float point_distance; /** \brief Octree key. */ OctreeKey key; }; struct PtIdxAndDis { int idx; float dis; PtIdxAndDis() { idx = -1; dis =-0.01; } bool operator <(PtIdxAndDis b) { return (dis < b.dis); } }; extern bool PtIdxAndDis_sortDis(PtIdxAndDis a, PtIdxAndDis b); class OctreeStudy1 { public: OctreeStudy1(double resolution); OctreeStudy1(); OctreeStudy1(int depth); ~OctreeStudy1(); public: pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud; pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVoxel; PointCloud<PointXYZRGB>::Ptr cloudre; pcl::octree::OctreePointCloud<pcl::PointXYZ> octree; BranchNode*rootNode; PointXYZ octreeMinPt, octreeMaxPt; int octreeMaxDepth; BranchNode*getRootNode(); vector<vector<int>> ptIndiceRsult; int curDepth; pcl::visualization::PCLVisualizer viz; bool show_cubes_; bool show_centroids_; bool show_original_points_; int point_size_; void setInputCloud(string path); void setMaxDepth(int depth); void saveDivideReSult(string path); void getVoxelCentroidsRecursive( BranchNode* branch_arg, pcl::octree::OctreeKey& key_arg, pcl::octree::OctreePointCloud<pcl::PointXYZ>::AlignedPointTVector &voxel_centroid_list_arg, vector<int>&PtVEC ); void compute(); void computeSpecifiedLayer(int layer); void computeToOctreeNodeVec(); void findLeafRecursive(const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg, LeafNode* result_arg); void clearVisual(); void showCubes(double voxelSideLen, bool wireframe = true); void run(); void update(); int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices); void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth, std::vector<int>& k_indices); void radiusSearch(PointXYZ pointT, float r, vector<PtIdxAndDis>&indice); void radiusSearchRecursive(PointXYZ pointT, float r, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth, std::vector<PtIdxAndDis>& k_indices); void nearestKSearch(PointT pt,int k,vector<int>&indice,vector<float>&disVec); float getKNearestNeighborRecursive( PointT &point, int K, BranchNode* node, OctreeKey& key, int tree_depth, double &squared_search_radius, std::vector<zx::prioPointQueueEntry>& point_candidates); double getKNearestNeighborRecursivePcl( PointXYZ point, unsigned int K, const BranchNode* node, const OctreeKey& key, int tree_depth, const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates); private: }; }
#include "octreeStudy.h" #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/octree/octree_pointcloud_voxelcentroid.h> #include <pcl/octree/octree_impl.h> #include <pcl/common/time.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/visualization/point_cloud_handlers.h> #include <pcl/visualization/common/common.h> #include <pcl/octree/octree_pointcloud_voxelcentroid.h> #include <pcl/common/centroid.h> #include <pcl/filters/filter.h> #include <vtkRenderer.h> #include <vtkRenderWindow.h> #include <vtkCubeSource.h> #include <vtkCleanPolyData.h> #include "pcl/octree/octree_container.h" #include <boost/random/shuffle_order.hpp> using namespace std; using namespace pcl; using namespace octree; namespace zx { double random(double start, double end) { return start + (end - start)*rand() / (RAND_MAX + 1.0); } float get2ptDis(PointT p1, PointT p2) { return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2)); } bool PtIdxAndDis_sortDis(PtIdxAndDis a, PtIdxAndDis b) { return (a.dis < b.dis); } OctreeStudy1::OctreeStudy1(double resolution) : octree(resolution), inputCloud(new pcl::PointCloud<pcl::PointXYZ>()), cloudVoxel(new pcl::PointCloud<pcl::PointXYZ>()), cloudre(new pcl::PointCloud<pcl::PointXYZRGB>()) { curDepth = 1; point_size_ = 1; viz.setWindowName("八叉树研究"); show_cubes_ = true; show_centroids_ = true; show_original_points_ = true; octreeMinPt = PointXYZ(0,0,0); octreeMaxPt = PointXYZ(0,0,0); octreeMaxDepth = -1; } OctreeStudy1::OctreeStudy1() : octree(1.0f), inputCloud(new pcl::PointCloud<pcl::PointXYZ>()), cloudVoxel(new pcl::PointCloud<pcl::PointXYZ>()), cloudre(new pcl::PointCloud<pcl::PointXYZRGB>()) { curDepth = 1; point_size_ = 1; viz.setWindowName("八叉树研究"); show_cubes_ = true; show_centroids_ = true; show_original_points_ = true; octreeMinPt = PointXYZ(-9999, -9999, -9999); octreeMaxPt = PointXYZ(-9999, -9999, -9999); octreeMaxDepth = -1; } OctreeStudy1::OctreeStudy1(int resolution) : octree(resolution), inputCloud(new pcl::PointCloud<pcl::PointXYZ>()), cloudVoxel(new pcl::PointCloud<pcl::PointXYZ>()) { curDepth = 1; point_size_ = 1; viz.setWindowName("八叉树研究"); show_cubes_ = true; show_centroids_ = true; show_original_points_ = true; octreeMinPt = PointXYZ(-9999, -9999, -9999); octreeMaxPt = PointXYZ(-9999, -9999, -9999); octreeMaxDepth = -1; } OctreeStudy1::~OctreeStudy1() { } void OctreeStudy1::setInputCloud(string path) { pcl::io::loadPCDFile(/*"tz_all_0-Cloud-1cm-target.pcd"*/path, *inputCloud); } void OctreeStudy1::getVoxelCentroidsRecursive( BranchNode* branch_arg, pcl::octree::OctreeKey& key_arg, pcl::octree::OctreePointCloud<pcl::PointXYZ>::AlignedPointTVector &voxel_centroid_list_arg, vector<int>&PtVEC ) { // child iterator unsigned char child_idx; // iterate over all children for (child_idx = 0; child_idx < 8; child_idx++) { // if child exist if (branch_arg->hasChild(child_idx)) { // add current branch voxel to key key_arg.pushBranch(child_idx); OctreeNode *child_node = branch_arg->getChildPtr(child_idx); switch (child_node->getNodeType()) { case BRANCH_NODE: { // recursively proceed with indexed child branch getVoxelCentroidsRecursive(static_cast<BranchNode*> (child_node), key_arg, voxel_centroid_list_arg, PtVEC); break; } case LEAF_NODE: { PointXYZ new_centroid; LeafNode* container = static_cast<LeafNode*> (child_node); vector<int> ptIndiceVec; container->getContainer().getPointIndices(ptIndiceVec); PtVEC.insert(PtVEC.end(), ptIndiceVec.begin(), ptIndiceVec.end()); //container->getContainer().getCentroid(new_centroid); //voxel_centroid_list_arg.push_back(new_centroid); break; } default: break; } // pop current branch voxel from key key_arg.popBranch(); } } } void OctreeStudy1::compute() { //pcl::octree::OctreePointCloud<pcl::PointXYZ> octree(0.1); //create octree structure octree.setInputCloud(inputCloud); //update bounding box automatically octree.defineBoundingBox(); //add points in the tree octree.addPointsFromInputCloud(); int maxdepth = octree.getTreeDepth(); //开始提取八叉树节点里的数据 int depth = maxdepth; pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it; pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it_end = octree.end(); pcl::PointXYZ pt_voxel_center; //pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVoxel(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointXYZ pt_centroid; float gridLength = std::sqrt(octree.getVoxelSquaredSideLen(depth)); for (tree_it = octree.begin(depth); tree_it != tree_it_end; ++tree_it) { // If the iterator is not at the right depth, continue int curNodeDepth = tree_it.getCurrentOctreeDepth(); int sss = octree.getTreeDepth(); cout << "curNodeDepth:" << curNodeDepth << endl; cout << "octree.getTreeDepth():" << sss << endl; //if (tree_it.getCurrentOctreeDepth() != (unsigned int)depth) // continue; // Compute the point at the center of the voxel which represents the current OctreeNode Eigen::Vector3f voxel_min, voxel_max; octree.getVoxelBounds(tree_it, voxel_min, voxel_max); pt_voxel_center.x = (voxel_min.x() + voxel_max.x()) / 2.0f; pt_voxel_center.y = (voxel_min.y() + voxel_max.y()) / 2.0f; pt_voxel_center.z = (voxel_min.z() + voxel_max.z()) / 2.0f; cloudVoxel->points.push_back(pt_voxel_center); // If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode if (/*octree.getTreeDepth() */curNodeDepth == (unsigned int)depth) { pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode* container = static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode()); //container->getContainer().getCentroid(pt_centroid); std::vector<int> pointerVec; container->getContainer().getPointIndices(pointerVec); } else if (curNodeDepth == depth - 1) { vector<int >PtVec; pcl::octree::OctreeKey dummy_key; pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids; getVoxelCentroidsRecursive( static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::BranchNode*> (*tree_it), dummy_key, voxelCentroids, PtVec); } } } //OctreeNode void OctreeStudy1::computeToOctreeNodeVec() { //create octree structure octree.setInputCloud(inputCloud); //update bounding box automatically octree.defineBoundingBox(); //add points in the tree octree.addPointsFromInputCloud(); int maxdepth = octree.getTreeDepth(); pcl::PointXYZ pt_voxel_center; pcl::PointXYZ pt_centroid; float gridLength = std::sqrt(octree.getVoxelSquaredSideLen(maxdepth)); pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it; vector<pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator> IteratorStateVec; while (tree_it != octree.end()) { //OctreeIteratorBase<pcl::PointXYZ> // tree_it.getCurrentOctreeDepth() IteratorStateVec.push_back(tree_it); //tree_it. //OctreeNodeVec.push_back(tree_it.getCurrentOctreeNode()); tree_it++; } for (tree_it = octree.begin(maxdepth); tree_it != octree.end(); ++tree_it) { // If the iterator is not at the right depth, continue int curNodeDepth = tree_it.getCurrentOctreeDepth(); int sss = octree.getTreeDepth(); cout << "curNodeDepth:" << curNodeDepth << endl; cout << "octree.getTreeDepth():" << sss << endl; //if (tree_it.getCurrentOctreeDepth() != (unsigned int)depth) // continue; // Compute the point at the center of the voxel which represents the current OctreeNode Eigen::Vector3f voxel_min, voxel_max; octree.getVoxelBounds(tree_it, voxel_min, voxel_max); pt_voxel_center.x = (voxel_min.x() + voxel_max.x()) / 2.0f; pt_voxel_center.y = (voxel_min.y() + voxel_max.y()) / 2.0f; pt_voxel_center.z = (voxel_min.z() + voxel_max.z()) / 2.0f; cloudVoxel->points.push_back(pt_voxel_center); // If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode if (/*octree.getTreeDepth() */curNodeDepth == (unsigned int)maxdepth) { pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode* container = static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode()); //container->getContainer().getCentroid(pt_centroid); std::vector<int> pointerVec; container->getContainer().getPointIndices(pointerVec); } else if (curNodeDepth == maxdepth - 1) { vector<int >PtVec; pcl::octree::OctreeKey dummy_key; pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids; getVoxelCentroidsRecursive( static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::BranchNode*> (*tree_it), dummy_key, voxelCentroids, PtVec); } } } void getLeafChildNodeItrater(BranchNode *parentNode,vector<int>&ptsVec) { for (int i = 0; i < 8;i++) { if (parentNode->hasChild(i)) { switch (parentNode->getChildPtr(i)->getNodeType()) { case BRANCH_NODE: getLeafChildNodeItrater(static_cast<BranchNode*>(parentNode->getChildPtr(i)), ptsVec); break; case LEAF_NODE: LeafNode*childNode=static_cast<LeafNode*>(parentNode->getChildPtr(i)); vector<int>ptsVecT; childNode->getContainer().getPointIndices(ptsVecT); ptsVec.insert(ptsVec.end(), ptsVecT.begin(), ptsVecT.end()); break; } } } } void getBranchChildNode(BranchNode *parentNode) { for (int i = 0; i < 8; i++) { if (parentNode->hasChild(i)) { switch (parentNode->getChildPtr(i)->getNodeType()) { case BRANCH_NODE: //zx::BranchNode *childNode = static_cast<zx::BranchNode*>(parentNode->getChildPtr(i)); break; case LEAF_NODE: zx::LeafNode*childNode1 = static_cast<zx::LeafNode*>(parentNode->getChildPtr(i)); break; } } } } void OctreeStudy1::findLeafRecursive(const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg, LeafNode* result_arg) { // index to branch child unsigned char child_idx; // find branch child from key child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); OctreeNode* child_node = (*branch_arg)[child_idx]; if (child_node) { switch (child_node->getNodeType()) { case BRANCH_NODE: // we have not reached maximum tree depth BranchNode* child_branch; child_branch = static_cast<BranchNode*> (child_node); findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg); break; case LEAF_NODE: // return existing leaf node LeafNode* child_leaf; child_leaf = static_cast<zx::LeafNode*> (child_node); result_arg = child_leaf; //result_arg = child_leaf->getContainerPtr(); break; } } } BranchNode* OctreeStudy1::getRootNode() { octree.setInputCloud(inputCloud); //update bounding box automatically octree.defineBoundingBox(); //add points in the tree octree.addPointsFromInputCloud(); octreeMaxDepth = octree.getTreeDepth(); pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it; pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it_end = octree.end(); tree_it = octree.begin(octreeMaxDepth); rootNode = static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::BranchNode*> (*tree_it); // for (tree_it = octree.begin(maxdepth); tree_it != tree_it_end; ++tree_it) // { // } double min_x_arg, min_y_arg, min_z_arg, max_x_arg, max_y_arg, max_z_arg; min_x_arg = min_y_arg = min_z_arg = max_x_arg = max_y_arg = max_z_arg = 0.0; octree.getBoundingBox(min_x_arg, min_y_arg, min_z_arg, max_x_arg, max_y_arg, max_z_arg); octreeMinPt = PointXYZ(min_x_arg, min_y_arg, min_z_arg); octreeMaxPt = PointXYZ(max_x_arg, max_y_arg, max_z_arg); return rootNode; } void OctreeStudy1::computeSpecifiedLayer(int layerCurIdx) { curDepth = layerCurIdx; bool isLeaf = false; octree.setInputCloud(inputCloud); octree.defineBoundingBox(); octree.addPointsFromInputCloud(); int maxdepth = octree.getTreeDepth(); if (layerCurIdx > maxdepth) { layerCurIdx = maxdepth; isLeaf = true; } if (layerCurIdx == maxdepth) { isLeaf = true; } int depth = maxdepth; pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it; pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it_end = octree.end(); pcl::PointXYZ pt_voxel_center; pcl::PointXYZ pt_centroid; float gridLength = std::sqrt(octree.getVoxelSquaredSideLen(layerCurIdx)); for (tree_it = octree.begin(depth); tree_it != tree_it_end; ++tree_it) { // If the iterator is not at the right depth, continue int curNodeDepth = tree_it.getCurrentOctreeDepth(); int sss = octree.getTreeDepth(); if (curNodeDepth == layerCurIdx) { Eigen::Vector3f voxel_min, voxel_max; octree.getVoxelBounds(tree_it, voxel_min, voxel_max); pt_voxel_center.x = (voxel_min.x() + voxel_max.x()) / 2.0f; pt_voxel_center.y = (voxel_min.y() + voxel_max.y()) / 2.0f; pt_voxel_center.z = (voxel_min.z() + voxel_max.z()) / 2.0f; cloudVoxel->points.push_back(pt_voxel_center); } // If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode vector<int >PtVec; if (curNodeDepth == layerCurIdx) { if (isLeaf) { pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode* container = static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode()); if (container) { container->getContainer().getPointIndices(PtVec); } } else { pcl::octree::OctreeKey dummy_key; pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids; getVoxelCentroidsRecursive( static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::BranchNode*> (*tree_it), dummy_key, voxelCentroids, PtVec); } } ptIndiceRsult.push_back(PtVec); } cloudVoxel->width = cloudVoxel->points.size(); cloudVoxel->height =1; cloudVoxel->is_dense = false; PCDWriter write; write.write("cloudVoxel.pcd", *cloudVoxel); } void OctreeStudy1::setMaxDepth(int depth) { PointXYZ minpt, maxpt; getMinMax3D(*inputCloud,minpt,maxpt); float xlen = maxpt.x - minpt.x; float ylen = maxpt.y - minpt.y; float zlen = maxpt.z - minpt.z; float maxLen = 0; if (xlen>ylen) { maxLen = xlen; } else { maxLen = ylen; } if (maxLen<zlen) { maxLen = zlen; } float resolutionT = (maxLen / pow(2, depth))*1.01; octree.setResolution(resolutionT); } void OctreeStudy1::saveDivideReSult(string path) { PointCloud<PointXYZRGB>::Ptr cloudre(new PointCloud<PointXYZRGB>()); int pcSize = inputCloud->points.size(); for (int i = 0; i < ptIndiceRsult.size(); i++) { int r = rand() % 255 + 1; int g = rand() % 255 + 1; int b = random(0, 1) * 255; for (int j = 0; j < ptIndiceRsult[i].size(); j++) { int id = ptIndiceRsult[i][j]; if (id>=pcSize) { continue; } pcl::PointXYZRGB pt; pt.x = inputCloud->points[id].x; pt.y = inputCloud->points[id].y; pt.z = inputCloud->points[id].z; pt.r = r; pt.g = g; pt.b = b; cloudre->points.push_back(pt); } } cloudre->height = 1; cloudre->width = inputCloud->points.size(); cloudre->is_dense = false; ofstream outfile(path, ios::out); for (int i = 0; i < cloudre->points.size(); i++) { int r = cloudre->points[i].r; int g = cloudre->points[i].g; int b = cloudre->points[i].b; outfile << cloudre->points[i].x << " " << cloudre->points[i].y << " " << cloudre->points[i].z << " " << r << " " << g << " " << b << "\n"; } outfile.close(); cout << "结果存储成功！" << endl; } void OctreeStudy1::clearVisual() { //remove cubes if any vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer(); while (renderer->GetActors()->GetNumberOfItems() > 0) renderer->RemoveActor(renderer->GetActors()->GetLastActor()); //remove point clouds if any viz.removePointCloud("cloud"); viz.removePointCloud("cloud_centroid"); } void OctreeStudy1::showCubes(double voxelSideLen, bool wireframe) { vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New(); // Create every cubes to be displayed double s = voxelSideLen / 2.0; for (size_t i = 0; i < cloudVoxel->points.size(); i++) { double x = cloudVoxel->points[i].x; double y = cloudVoxel->points[i].y; double z = cloudVoxel->points[i].z; vtkSmartPointer<vtkCubeSource> wk_cubeSource = vtkSmartPointer<vtkCubeSource>::New(); wk_cubeSource->SetBounds(x - s, x + s, y - s, y + s, z - s, z + s); wk_cubeSource->Update(); #if VTK_MAJOR_VERSION < 6 appendFilter->AddInput(wk_cubeSource->GetOutput()); #else appendFilter->AddInputData(wk_cubeSource->GetOutput()); #endif } // Remove any duplicate points vtkSmartPointer<vtkCleanPolyData> cleanFilter = vtkSmartPointer<vtkCleanPolyData>::New(); cleanFilter->SetInputConnection(appendFilter->GetOutputPort()); cleanFilter->Update(); //Create a mapper and actor vtkSmartPointer<vtkPolyDataMapper> multiMapper = vtkSmartPointer<vtkPolyDataMapper>::New(); multiMapper->SetInputConnection(cleanFilter->GetOutputPort()); vtkSmartPointer<vtkActor> multiActor = vtkSmartPointer<vtkActor>::New(); multiActor->SetMapper(multiMapper); multiActor->GetProperty()->SetColor(1.0, 1.0, 0.0); multiActor->GetProperty()->SetAmbient(1.0); multiActor->GetProperty()->SetLineWidth(1); multiActor->GetProperty()->EdgeVisibilityOn(); multiActor->GetProperty()->SetOpacity(1.0); if (wireframe) { multiActor->GetProperty()->SetRepresentationToWireframe(); } else { multiActor->GetProperty()->SetRepresentationToSurface(); } // Add the actor to the scene viz.getRenderWindow()->GetRenderers()->GetFirstRenderer()->AddActor(multiActor); // Render and interact viz.getRenderWindow()->Render(); } void OctreeStudy1::run() { while (!viz.wasStopped()) { //main loop of the visualizer viz.spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } } void OctreeStudy1::update() { clearVisual(); if (show_cubes_) { //show octree as cubes showCubes(std::sqrt(octree.getVoxelSquaredSideLen(curDepth))); } if (show_centroids_) { //show centroid points pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloudVoxel, "x"); viz.addPointCloud(cloudVoxel, color_handler, "cloud_centroid"); viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud_centroid"); } if (/*show_original_points_*/0) { //show origin point cloud pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(inputCloud, "z"); viz.addPointCloud(inputCloud, color_handler, "cloud"); viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud"); } if (show_original_points_) { //show origin point cloud //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(inputCloud, "z"); viz.addPointCloud(cloudre, "cloudre"); viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloudre"); //viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud"); } } int OctreeStudy1::boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) { OctreeKey key; key.x = key.y = key.z = 0; k_indices.clear(); getRootNode(); boxSearchRecursive(min_pt, max_pt, /*this->root_node_*/rootNode, key, 1, k_indices); return (static_cast<int> (k_indices.size())); } void OctreeStudy1::boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth, std::vector<int>& k_indices) { // child iterator unsigned char child_idx; // iterate over all children for (child_idx = 0; child_idx < 8; child_idx++) { const OctreeNode* child_node; child_node = node->getChildPtr(child_idx); if (!child_node) continue; OctreeKey new_key; int temp = 1; if (child_idx == 0 || child_idx == 1 || child_idx == 4 || child_idx == 5) { temp = 0; } new_key.x = key.x * 2 + (int)(child_idx / 4); new_key.y = key.y * 2 + temp; new_key.z = key.z * 2 + (int)(child_idx % 2); // voxel corners Eigen::Vector3f min_pt1; Eigen::Vector3f max_pt1; double min_x_arg, min_y_arg, min_z_arg, max_x_arg, max_y_arg, max_z_arg; min_x_arg=min_y_arg=min_z_arg=max_x_arg=max_y_arg=max_z_arg=0.0; octree.getBoundingBox(min_x_arg, min_y_arg, min_z_arg, max_x_arg, max_y_arg, max_z_arg); // get voxel coordinates //this->genVoxelBoundsFromOctreeKey(new_key, tree_depth, lower_voxel_corner, upper_voxel_corner); { // calculate voxel size of current tree depth double voxel_side_len = /*this->resolution_*/octree.getResolution() * static_cast<double> (1 << (octree.getTreeDepth() - tree_depth)); // calculate voxel bounds min_pt1(0) = static_cast<float> (static_cast<double> (new_key.x) * voxel_side_len + min_x_arg); min_pt1(1) = static_cast<float> (static_cast<double> (new_key.y) * voxel_side_len + min_y_arg); min_pt1(2) = static_cast<float> (static_cast<double> (new_key.z) * voxel_side_len + min_z_arg); max_pt1(0) = static_cast<float> (static_cast<double> (new_key.x + 1) * voxel_side_len + min_x_arg); max_pt1(1) = static_cast<float> (static_cast<double> (new_key.y + 1) * voxel_side_len + min_y_arg); max_pt1(2) = static_cast<float> (static_cast<double> (new_key.z + 1) * voxel_side_len + min_z_arg); } // test if search region overlap with voxel space if (!((min_pt1(0) > max_pt(0)) || (min_pt(0) > max_pt1(0)) || (min_pt1(1) > max_pt(1)) || (min_pt(1) > max_pt1(1)) || (min_pt1(2) > max_pt(2)) || (min_pt(2) > max_pt1(2)))) { if (tree_depth < /*this->octree_depth_*/octree.getTreeDepth()) { // we have not reached maximum tree depth boxSearchRecursive(min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices); } else { // we reached leaf node level size_t i; std::vector<int> decoded_point_vector; bool bInBox; const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node); // decode leaf node into decoded_point_vector (**child_leaf).getPointIndices(decoded_point_vector); ptIndiceRsult.push_back(decoded_point_vector); // Linearly iterate over all decoded (unsorted) points for (i = 0; i < decoded_point_vector.size(); i++) { const PointT& candidate_point = inputCloud->points[decoded_point_vector[i]]/* this->getPointByIndex(decoded_point_vector[i])*/; // check if point falls within search box bInBox = ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) && (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) && (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2))); if (bInBox) // add to result vector k_indices.push_back(decoded_point_vector[i]); } } } } } void OctreeStudy1::radiusSearch(PointXYZ pointT, float r, vector<PtIdxAndDis>&indice) { OctreeKey key; key.x = key.y = key.z = 0; indice.clear(); getRootNode(); radiusSearchRecursive(pointT, r, static_cast<BranchNode*>(rootNode), key, 1, indice); sort(indice.begin(), indice.end()); } void OctreeStudy1::radiusSearchRecursive(PointXYZ pointT, float r, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth, std::vector<PtIdxAndDis>& k_indices) { int child_idx; for (child_idx = 0; child_idx < 8; child_idx++) { OctreeNode *childNode = node->getChildPtr(child_idx); if (!childNode) { continue; } OctreeKey new_key; int temp = 1; if (child_idx == 0 || child_idx == 1 || child_idx == 4 || child_idx == 5) { temp = 0; } new_key.x = key.x * 2 + (int)(child_idx / 4); new_key.y = key.y * 2 + temp; new_key.z = key.z * 2 + (int)(child_idx % 2); double min_x_arg, min_y_arg, min_z_arg, max_x_arg, max_y_arg, max_z_arg; min_x_arg = min_y_arg = min_z_arg = max_x_arg = max_y_arg = max_z_arg = 0.0; octree.getBoundingBox(min_x_arg, min_y_arg, min_z_arg, max_x_arg, max_y_arg, max_z_arg); float boxLen = (float)pow(2, octree.getTreeDepth() - tree_depth)*octree.getResolution(); PointXYZ minPt, maxPt; minPt.x = new_key.x*boxLen + min_x_arg; minPt.y = new_key.y*boxLen + min_y_arg; minPt.z = new_key.z*boxLen + min_z_arg; maxPt.x = (new_key.x + 1)*boxLen + min_x_arg; maxPt.y = (new_key.y + 1)*boxLen + min_y_arg; maxPt.z = (new_key.z + 1)*boxLen + min_z_arg; //预设一个包围盒 PointXYZ preMinpt, preMaxpt; preMinpt.x = pointT.x - r; preMinpt.y = pointT.y - r; preMinpt.z = pointT.z - r; preMaxpt.x = pointT.x + r; preMaxpt.y = pointT.y + r; preMaxpt.z = pointT.z + r; bool isInbox = ( preMinpt.x > maxPt.x || preMinpt.y > maxPt.y || preMinpt.z > maxPt.z || preMaxpt.x < minPt.x || preMaxpt.y < minPt.y || preMaxpt.z < minPt.z ); if (!isInbox) { if (tree_depth<octree.getTreeDepth())//如果是根节点 { radiusSearchRecursive(pointT, r, static_cast<BranchNode*>(childNode), new_key, tree_depth + 1, k_indices); } else //如果是叶子节点 { vector<int> ptVec; LeafNode *child_leaf = static_cast<LeafNode*>(childNode); (*child_leaf)->getPointIndices(ptVec); ptIndiceRsult.push_back(ptVec); for (int i = 0; i < ptVec.size();i++) { PointXYZ curPt = inputCloud->points[ptVec[i]]; if (get2ptDis(curPt, pointT)<=r) { PtIdxAndDis ptInfo; ptInfo.idx = ptVec[i]; ptInfo.dis = get2ptDis(curPt, pointT); k_indices.push_back(ptInfo); } } } } } } void OctreeStudy1::nearestKSearch(PointT pt, int k, vector<int>&indice, vector<float>&disVec) { indice.clear(); disVec.clear(); indice.resize(k); disVec.resize(k); OctreeKey key; key.x = key.y = key.z = 0; getRootNode(); vector<zx::prioPointQueueEntry> point_candidates; double initialDis = 10000000; getKNearestNeighborRecursive(pt, k, rootNode, key, 1, initialDis, point_candidates); for (int i = 0; i < point_candidates.size();i++) { indice[i] = point_candidates[i].point_idx_; disVec[i] = point_candidates[i].point_distance_; } } float OctreeStudy1::getKNearestNeighborRecursive(PointT &point, int K, BranchNode* node, OctreeKey& key, int tree_depth, double &squared_search_radius, std::vector<zx::prioPointQueueEntry>& point_candidates) { std::vector<prioBranchQueueEntry> search_heap; search_heap.resize(8); double smallest_squared_dist = squared_search_radius; // get spatial voxel information double voxelSquaredDiameter = (float)pow(2, octree.getTreeDepth() - tree_depth)*octree.getResolution(); for (int child_idx = 0; child_idx < 8; child_idx++) { OctreeNode *childNode = node->getChildPtr(child_idx); if (childNode) { PointT voxel_center; OctreeKey new_keyT; int temp = 1; if (child_idx == 0 || child_idx == 1 || child_idx == 4 || child_idx == 5) { temp = 0; } new_keyT.x = key.x * 2 + (int)(child_idx / 4); new_keyT.y = key.y * 2 + temp; new_keyT.z = key.z * 2 + (int)(child_idx % 2); voxel_center.x = (new_keyT.x + 1.0 / 2)*voxelSquaredDiameter + octreeMinPt.x; voxel_center.y = (new_keyT.y + 1.0 / 2)*voxelSquaredDiameter + octreeMinPt.y; voxel_center.z = (new_keyT.z + 1.0 / 2)*voxelSquaredDiameter + octreeMinPt.z; search_heap[child_idx].node = childNode; search_heap[child_idx].point_distance = pow(get2ptDis(point, voxel_center), 2); search_heap[child_idx].key = new_keyT; } else { search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity(); } } sort(search_heap.begin(), search_heap.end()); //降序 float disThread = smallest_squared_dist +voxelSquaredDiameter / 4.0 + sqrt(smallest_squared_dist * voxelSquaredDiameter); while ( (!search_heap.empty()) && (search_heap.back().point_distance <disThread) ) { cout << "当前节点深度：" << tree_depth << ",,距离阈值：" << disThread << endl; OctreeNode* child_node; // read from priority queue element child_node = search_heap.back().node; OctreeKey new_key = search_heap.back().key; if (tree_depth < octreeMaxDepth) { // we have not reached maximum tree depth smallest_squared_dist = getKNearestNeighborRecursive(point, K, static_cast<BranchNode*> (child_node), new_key, tree_depth + 1, smallest_squared_dist, point_candidates); cout << "####进入根节点：" << tree_depth << ",," << smallest_squared_dist << endl; } else { cout << "----进入子节点：" << tree_depth <<",,"<< smallest_squared_dist << endl; float squared_dist; vector<int> decoded_point_vector; LeafNode* child_leaf = static_cast<LeafNode*> (child_node); (*child_leaf)->getPointIndices(decoded_point_vector); for (int i = 0; i < decoded_point_vector.size(); i++) { PointT candidate_point = inputCloud->points[decoded_point_vector[i]]; squared_dist = pow(get2ptDis(point, candidate_point), 2); //squared_dist = get2ptDis(candidate_point, point)/**get2ptDis(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_ = decoded_point_vector[i]; 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_; } search_heap.pop_back(); } //while end return smallest_squared_dist; } double OctreeStudy1::getKNearestNeighborRecursivePcl( PointXYZ point, unsigned int K, const BranchNode* node, const OctreeKey& key, int tree_depth, const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates) { std::vector<prioBranchQueueEntry> search_heap; search_heap.resize(8); int child_idx; OctreeKey new_key; double smallest_squared_dist = squared_search_radius; // get spatial voxel information double voxelSquaredDiameter = /*this->getVoxelSquaredDiameter(tree_depth);*/ (float)pow(2, octree.getTreeDepth() - tree_depth)*octree.getResolution(); // iterate over all children for (child_idx = 0; child_idx < 8; child_idx++) { OctreeNode *childNode = node->getChildPtr(child_idx); if (childNode) { 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); voxel_center.x = (search_heap[child_idx].key.x + 1.0 / 2)*voxelSquaredDiameter + octreeMinPt.x; voxel_center.y = (search_heap[child_idx].key.y + 1.0 / 2)*voxelSquaredDiameter + octreeMinPt.y; voxel_center.z = (search_heap[child_idx].key.z + 1.0 / 2)*voxelSquaredDiameter + octreeMinPt.z; // generate new priority queue element search_heap[child_idx].node = /*this->getBranchChildPtr(*node, child_idx)*/childNode; search_heap[child_idx].point_distance =/* pointSquaredDist(voxel_center, point)*/pow(get2ptDis(point, voxel_center), 2); } 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_*/)) { OctreeNode* child_node; // read from priority queue element child_node = search_heap.back().node; new_key = search_heap.back().key; if (tree_depth < /*this->octree_depth_*/octreeMaxDepth) { //static_cast</*const*/ BranchNode*> // we have not reached maximum tree depth smallest_squared_dist = getKNearestNeighborRecursivePcl(point, K, static_cast<BranchNode*> (child_node), new_key, tree_depth + 1, smallest_squared_dist, point_candidates); } else { // we reached leaf node level float squared_dist; size_t i; std::vector<int> decoded_point_vector; const LeafNode* 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 (i = 0; i < decoded_point_vector.size(); i++) { const PointT& candidate_point = /*this->getPointByIndex(decoded_point_vector[i])*/inputCloud->points[decoded_point_vector[i]]; // calculate point distance to search point squared_dist = /*pointSquaredDist(candidate_point, point);*/ pow(get2ptDis(point, candidate_point), 2); // 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_ = decoded_point_vector[i]; 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); } }
}
//测试用例 { pcl::octree::OctreePointCloud<pcl::PointXYZ> octree(0.1); pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile("tz_all_0-Cloud-1cm-target.pcd", *inputCloud);//create octree structure octree.setInputCloud(inputCloud); //update bounding box automatically octree.defineBoundingBox(); //add points in the tree octree.addPointsFromInputCloud(); int maxdepth = octree.getTreeDepth(); int depth = maxdepth; pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it; pcl::octree::OctreePointCloud<pcl::PointXYZ>::Iterator tree_it_end = octree.end(); pcl::PointXYZ pt_voxel_center; pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVoxel(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointXYZ pt_centroid; float gridLength = std::sqrt(octree.getVoxelSquaredSideLen(depth)); for (tree_it = octree.begin(depth); tree_it != tree_it_end; ++tree_it) { // If the iterator is not at the right depth, continue int curNodeDepth = tree_it.getCurrentOctreeDepth(); int sss = octree.getTreeDepth(); cout << "curNodeDepth:" << curNodeDepth << endl; cout << "octree.getTreeDepth():" << sss << endl; //if (tree_it.getCurrentOctreeDepth() != (unsigned int)depth) // continue; // Compute the point at the center of the voxel which represents the current OctreeNode Eigen::Vector3f voxel_min, voxel_max; octree.getVoxelBounds(tree_it, voxel_min, voxel_max); pt_voxel_center.x = (voxel_min.x() + voxel_max.x()) / 2.0f; pt_voxel_center.y = (voxel_min.y() + voxel_max.y()) / 2.0f; pt_voxel_center.z = (voxel_min.z() + voxel_max.z()) / 2.0f; cloudVoxel->points.push_back(pt_voxel_center); // If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode if (/*octree.getTreeDepth() */curNodeDepth == (unsigned int)depth) { pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode* container = static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode()); //container->getContainer().getCentroid(pt_centroid); std::vector<int> pointerVec; container->getContainer().getPointIndices(pointerVec); } else if (curNodeDepth == depth - 2) { vector<int >PtVec; pcl::octree::OctreeKey dummy_key; pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids; getVoxelCentroidsRecursive( static_cast<pcl::octree::OctreePointCloud<pcl::PointXYZ>::BranchNode*> (*tree_it), dummy_key, voxelCentroids, PtVec); } } }
（ps：最近很多人对源码比较感兴趣，这里将源码贴上，同样希望各位前辈以及同学能发扬资源共享的精神将点云算法这个行业继续推向前进。）
