本文共 31232 字，大约阅读时间需要 104 分钟。

最新更新： 2021.4.22

为了了解细节，可以看c++

 

 推荐

 推荐

 

八叉树系列文章

 

应用：

点云的降采样：八叉树采样（Octree）

游戏物理引擎(二) 碰撞检测之Broad-Phase

PCL库学习笔记（Octree寻找邻近点及其应用）

---

最新更新： 2021.4.20

二、八叉树建立过程

① 设定最大递归深度；

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

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

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

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

⑥ 重复③，直到达到最大递归深度；

三、八叉树和k-d树比较

八叉树算法的算法实现简单，但大数据量点云数据下，其使用比较困难的是最小粒度（叶节点）的确定，粒度较大时，有的节点数据量可能仍比较大，后续查询效率仍比较低，反之，粒度较小，八叉树的深度增加，需要的内存空间也比较大（每个非叶子节点需要八个指针），效率也降低。而等分的划分依据，使得在数据重心有偏斜的情况下，受划分深度限制，其效率不是太高。

k-d在邻域查找上比较有优势，但在大数据量的情况下，若划分粒度较小时，建树的开销也较大，但比八叉树灵活些。在小数据量的情况下，其搜索效率比较高，但在数据量增大的情况下，其效率会有一定的下降，一般是线性上升的规律。

也有将八叉树和k-d树结合起来的应用，应用八叉树进行大粒度的划分和查找，而后使用k-d树进行细分，效率会有一定的提升，但其搜索效率变化也与数据量的变化有一个线性关系

这一方法的主要优点在于可以非常方便地实现有广泛用途的集合运算(例如可以求两个物体的并、交、差等运算)，而这些恰是其它表示方法比较难以处理或者需要耗费许多计算资源的地方。

四、八叉树三维数据结构

用八叉树来表示三维形体，既可以看成是四叉树方法在三维空间的推广，也可以认为是用三维体素阵列表示形体方法的一种改进。八叉树的逻辑结构如下：

假设要表示的形体V可以放在一个充分大的正方体C内，C的边长为2n(偶数)，形体V=C，它的八叉树可以用以下的递归方法来定义：

八叉树的每个节点与C的一个子立方体对应，树根与C本身相对应，如果V＝C，那么V的八叉树仅有树根，如果V≠C，则将C等分为八个子立方体，每个子立方体与树根的一个子节点相对应。只要某个子立方体不是完全空白或完全为V所占据，就要被八等分（图2-5-1），从而对应的节点也就有了八个子节点。这样的递归判断、分割一直要进行到节点所对应的立方体或是完全空白，或是完全为V占据，或是其大小已是预先定义的体素大小，并且对它与V之交作一定的“舍入”，使体素或认为是空白的，或认为是V占据的。

 

该文章写的八叉树建立过程和下面的这个例子过程不一样，大家可以尝试弄完下面的代码再看这个

1.log.py

import vtkcolors = vtk.vtkNamedColors()class Point:    def __init__(self,x,y,z):        self.x=x        self.y=y        self.z=z    def printp(self):         print("(",self.x,",",self.y,",",self.z,")")     def imprimir(self,ren,r,g,b):        #sphere        sphereSource = vtk.vtkSphereSource()        sphereSource.SetCenter(self.x, self.y, self.z)        sphereSource.SetRadius(10)        # Make the surface smooth.        sphereSource.SetPhiResolution(100)        sphereSource.SetThetaResolution(100)        mapper = vtk.vtkPolyDataMapper()        mapper.SetInputConnection(sphereSource.GetOutputPort())        actor2 = vtk.vtkActor()        actor2.SetMapper(mapper)        actor2.GetProperty().SetColor(r,g,b)        ren.AddActor(actor2)        class Prism:    def __init__(self,x,y,z,w,h,l):        self.x=x        self.y=y        self.z=z        self.w=w        self.h=h        self.l=l    #判断插入的point是否超出所建立的树    #建树时候默认中心点为(200,200,200) 宽w 高h 长l 分别为200    def contains(self,point):        if point.x
   
    self.x-self.w \                and point.y
    
     self.y-self.h \                and point.z
     
      self.z-self.l:            return True        return False    def intersects(self,rang):        return not(rang.x-rang.w>self.x+self.w or rang.x+rang.w
      
       self.y+self.h or rang.y+rang.h
       
        self.z+self.l or rang.z+rang.l
       
      
     
    
   

2.main

# python3.8# -*- coding: utf-8 -*-# ---# @Software: PyCharm# @File: test2.py# @Author: ---# @Institution: BeiJing, China# @E-mail: lgdyangninghua@163.com# @Site: # @Time: 4月 21, 2021# ---from log import *import numpy as np# from random import choice# num = 400# point_cloud = []# for i in range(40):#     p=Point(choice(range(num)),choice(range(num)),choice(range(num)))#     #print(p.x, p.y, p.z)#     point_cloud.append([p.x, p.y, p.z])# point_cloud = np.array(point_cloud)# np.save("point_cloud.npy",point_cloud)# 载入40*3的随机点云 x,y,z的取值范围0-400point_cloud = np.load("point_cloud.npy")# 关于vtk的东西不予细纠colors = vtk.vtkNamedColors()#mouseclass MouseInteractorHighLightActor(vtk.vtkInteractorStyleTrackballCamera):    def __init__(self,parent=None):        self.AddObserver("LeftButtonPressEvent",self.leftButtonPressEvent)        self.LastPickedActor = None        self.LastPickedProperty = vtk.vtkProperty()    def leftButtonPressEvent(self,obj,event):        clickPos = self.GetInteractor().GetEventPosition()        picker = vtk.vtkPropPicker()        picker.Pick(clickPos[0], clickPos[1], 0, self.GetDefaultRenderer())        # get the new        self.NewPickedActor = picker.GetActor()        # If something was selected        if self.NewPickedActor:            # If we picked something before, reset its property            if self.LastPickedActor:                self.LastPickedActor.GetProperty().DeepCopy(self.LastPickedProperty)            # Save the property of the picked actor so that we can            # restore it next time            self.LastPickedProperty.DeepCopy(self.NewPickedActor.GetProperty())            # Highlight the picked actor by changing its properties            self.NewPickedActor.GetProperty().SetColor(1.0, 0.0, 0.0)            self.NewPickedActor.GetProperty().SetDiffuse(1.0)            self.NewPickedActor.GetProperty().SetSpecular(0.0)            # save the last picked actor            self.LastPickedActor = self.NewPickedActor        self.OnLeftButtonDown()        return# A renderer and render window# 建立八叉树，由于我们x,y,z的取值范围0-400，所以这里不用像其他代码一样求出点云的bbox区域(min_xyz和max_xyz)才建树# Prism有参数x y z w h l# 八叉树中每个正方体最大容纳4个点ren = vtk.vtkRenderer()num = 400boundary = Prism(num/2,num/2,num/2,num/2,num/2,num/2)ot = OctTree(boundary, 4, ren)rows,cols = point_cloud.shape[:2]# 递归分割空间插入所有点云，但是需要保证点云在建树的大正方体空间内for ind in range(rows):    m_point = point_cloud[ind,:]    p = Point(m_point[0], m_point[1], m_point[2])    ot.insert(p, ren)#画出所有点云#start Codex drawot.mostrar(0,ren)#end Codex draw#随机生成一个点# num2=vtk.vtkMath.Random(0, 600)# boundary2=Prism(num2,num2,num2,100,100,100)# 出空间的点，查找不到# num2=vtk.vtkMath.Random(400, 600)# boundary2=Prism(num2,num2,num2,100,100,100)#特意调了一个point 让与他相接的有两个点# 这里的70就是 dist<70的意思，至于top-k个最近邻，这个代码里面没有boundary2 = Prism(100,100,180,70,70,70)print("boundary2 x,y,z,w,h,l:", boundary2.x, boundary2.y, boundary2.z)f=[]ot.query(boundary2, f)for i in f:    i.printp()#Queryr = 0.4g = 0.5b = 0.8#cube querycube = vtk.vtkCubeSource()cube.SetXLength(boundary2.w*2)cube.SetYLength(boundary2.h*2)cube.SetZLength(boundary2.l*2)mapper = vtk.vtkPolyDataMapper()mapper.SetInputConnection(cube.GetOutputPort())actor = vtk.vtkActor()actor.SetMapper(mapper)actor.GetProperty().SetColor(r, g, b)actor.GetProperty().SetOpacity(0.2)actor.SetPosition(boundary2.x, boundary2.y, boundary2.z)actor.RotateX(0)actor.RotateY(0)actor.RotateZ(0)ren.AddActor(actor)#points queryfor i in f:    sphereSource = vtk.vtkSphereSource()    sphereSource.SetCenter(i.x, i.y, i.z)    sphereSource.SetRadius(10)    # Make the surface smooth.    sphereSource.SetPhiResolution(100)    sphereSource.SetThetaResolution(100)    mapper = vtk.vtkPolyDataMapper()    mapper.SetInputConnection(sphereSource.GetOutputPort())    actor2 = vtk.vtkActor()    actor2.SetMapper(mapper)    actor2.GetProperty().SetColor(0,0,0)    ren.AddActor(actor2)ren.SetBackground(0.1,0.1,0.1)renWin= vtk.vtkRenderWindow()renWin.AddRenderer(ren)renWin.SetSize(300,300)iren = vtk.vtkRenderWindowInteractor()iren.SetRenderWindow(renWin)# add the custom stylestyle = MouseInteractorHighLightActor()style.SetDefaultRenderer(ren)iren.SetInteractorStyle(style)iren.Initialize()iren.Start()

还有其他写的较好的代码：

 

关于最近邻：

体素近邻搜索：

vector<int>pointIdxVec;    //最近点索引 octree.voxelSearch(searchPoint,PointIdxVec); K近邻搜索： octree.nearestKSearch(searchPoint,K,索引向量，距离） 其中索引向量，距离是返回值

计算当前空间每个子空间体素中心(建树时候已经有)到searchPoint的距离, 然后push这个子空间，

如果该子空间还可分，继续访问，进行距离计算push

如果达到了叶子节点级别(没有再划分子空间的节点)，停止，

遍历这个容器，

判断 候选搜索对象的距离是否小于最佳点距离(刚开始设置一个非常大的值)

通过栈的特点

然后选出top-k

如果超过k，只选k个

半径内近邻搜索： octree.radiusSearch(searchPoint,半径,索引向量，距离）

可参考PCL源代码：

//template
   
     boolpcl::octree::OctreePointCloudSearch
    
     ::voxelSearch (const PointT& point,                                                                          std::vector
     
      & point_idx_data){  assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");  OctreeKey key;  bool b_success = false;  // generate key  this->genOctreeKeyforPoint (point, key);  LeafContainerT* leaf = this->findLeaf (key);  if (leaf)  {    (*leaf).getPointIndices (point_idx_data);    b_success = true;  }  return (b_success);}//template
      
        boolpcl::octree::OctreePointCloudSearch
       
        ::voxelSearch (const int index,                                                                          std::vector
        
         & point_idx_data){ const PointT search_point = this->getPointByIndex (index); return (this->voxelSearch (search_point, point_idx_data));}//template
         
           intpcl::octree::OctreePointCloudSearch
          
           ::nearestKSearch (const PointT &p_q, int k, std::vector
           
             &k_indices, std::vector
            
              &k_sqr_distances){ assert(this->leaf_count_>0); assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); k_indices.clear (); k_sqr_distances.clear (); if (k < 1) return 0; unsigned int i; unsigned int result_count; prioPointQueueEntry point_entry; std::vector
             
               point_candidates; OctreeKey key; key.x = key.y = key.z = 0; // initialize smallest point distance in search with high value double smallest_dist = std::numeric_limits
              
               ::max (); getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates); result_count = static_cast
               
                 (point_candidates.size ()); k_indices.resize (result_count); k_sqr_distances.resize (result_count); for (i = 0; i < result_count; ++i) { k_indices [i] = point_candidates [i].point_idx_; k_sqr_distances [i] = point_candidates [i].point_distance_; } return static_cast
                
                  (k_indices.size ());}//template
                 
                   intpcl::octree::OctreePointCloudSearch
                  
                   ::nearestKSearch (int index, int k, std::vector
                   
                     &k_indices, std::vector
                    
                      &k_sqr_distances){ const PointT search_point = this->getPointByIndex (index); return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));}//template
                     
                       doublepcl::octree::OctreePointCloudSearch
                      
                       ::getKNearestNeighborRecursive ( const PointT & point, unsigned int K, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth, const double squared_search_radius, std::vector
                       
                        & point_candidates) const{ std::vector
                        
                          search_heap; search_heap.resize (8); unsigned char child_idx; 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 (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
                         
                          ::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 (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast
                          
                            (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
                           
                             decoded_point_vector; const LeafNode* child_leaf = static_cast
                            
                              (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]); // calculate point distance to search point 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_ = 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);}
                            
                           
                          
                         
                        
                       
                      
                     
                    
                   
                  
                 
                
               
              
             
            
           
          
         
        
       
      
     
    
   

---

---

 

void Octree::InsertPoint(        const Eigen::Vector3d& point,        const std::function
   
    
     ()>& f_init,        const std::function
     
      
       )>& f_update) {    if (root_node_ == nullptr) {        if (max_depth_ == 0) {            root_node_ = f_init();        } else {            root_node_ = std::make_shared
       
        ();        }    }    auto root_node_info =            std::make_shared
        
         (origin_, size_, 0, 0); InsertPointRecurse(root_node_, root_node_info, point, f_init, f_update);}void Octree::InsertPointRecurse( const std::shared_ptr
         
          & node, const std::shared_ptr
          
           & node_info, const Eigen::Vector3d& point, const std::function
           
            
             ()>& f_init, const std::function
             
              
               )>& f_update) { if (!IsPointInBound(point, node_info->origin_, node_info->size_)) { return; } if (node_info->depth_ > max_depth_) { return; } else if (node_info->depth_ == max_depth_) { if (auto leaf_node = std::dynamic_pointer_cast
               
                (node)) { f_update(leaf_node); } else { utility::LogError( "Internal error: leaf node must be OctreeLeafNode"); } } else { if (auto internal_node = std::dynamic_pointer_cast
                
                 (node)) { // Get child node info std::shared_ptr
                 
                   child_node_info = internal_node->GetInsertionNodeInfo(node_info, point); // Create child node with factory function size_t child_index = child_node_info->child_index_; if (internal_node->children_[child_index] == nullptr) { if (node_info->depth_ == max_depth_ - 1) { internal_node->children_[child_index] = f_init(); } else { internal_node->children_[child_index] = std::make_shared
                  
                   (); } } // Insert to the child InsertPointRecurse(internal_node->children_[child_index], child_node_info, point, f_init, f_update); } else { utility::LogError( "Internal error: internal node must be " "OctreeInternalNode"); } }}
                  
                 
                
               
              
             
            
           
          
         
        
       
      
     
    
   

 

---

 

来源：

OcTree.m

classdef OcTree < handle% OcTree point decomposition in 3D%    OcTree is used to create a tree data structure of bins containing 3D%    points. Each bin may be recursively decomposed into 8 child bins.%%    OT = OcTree(PTS) creates an OcTree from an N-by-3 matrix of point%    coordinates.%%    OT = OcTree(...,'PropertyName',VALUE,...) takes any of the following%    property values:%%     binCapacity - Maximum number of points a bin may contain. If more%                   points exist, the bin will be recursively subdivided.%                   Defaults to ceil(numPts/10).%     maxDepth    - Maximum number of times a bin may be subdivided.%                   Defaults to INF.%     maxSize     - Maximum size of a bin edge. If any dimension of a bin %                   exceeds maxSize, it will be recursively subdivided.%                   Defaults to INF.%     minSize     - Minimum size of a bin edge. Subdivision will stop after %                   any dimension of a bin gets smaller than minSize.%                   Defaults to 1000*eps.%     style       - Either 'equal' (default) or 'weighted'. 'equal' %                   subdivision splits bins at their central coordinate%                   (ie, one bin subdivides into 8 equally sized bins).%                   'weighted' subdivision divides bins based on the mean%                   of all points they contain. Weighted subdivision is%                   slightly slower than equal subdivision for a large%                   number of points, but it can produce a more efficient%                   decomposition with fewer subdivisions.%%    Example 1: Decompose 200 random points into bins of 20 points or less,%             then display each bin with its points in a separate colour.%        pts = (rand(200,3)-0.5).^2;%        OT = OcTree(pts,'binCapacity',20);        %        figure%        boxH = OT.plot;%        cols = lines(OT.BinCount);%        doplot3 = @(p,varargin)plot3(p(:,1),p(:,2),p(:,3),varargin{:});%        for i = 1:OT.BinCount%            set(boxH(i),'Color',cols(i,:),'LineWidth', 1+OT.BinDepths(i))%            doplot3(pts(OT.PointBins==i,:),'.','Color',cols(i,:))%        end%        axis image, view(3)%%    Example 2: Decompose 200 random points into bins of 10 points or less,%             shrunk to minimallly encompass their points, then display.%        pts = rand(200,3);%        OT = OcTree(pts,'binCapacity',10,'style','weighted');%        OT.shrink%        figure%        boxH = OT.plot;%        cols = lines(OT.BinCount);%        doplot3 = @(p,varargin)plot3(p(:,1),p(:,2),p(:,3),varargin{:});%        for i = 1:OT.BinCount%            set(boxH(i),'Color',cols(i,:),'LineWidth', 1+OT.BinDepths(i))%            doplot3(pts(OT.PointBins==i,:),'.','Color',cols(i,:))%        end%        axis image, view(3)%%% OcTree methods:%     shrink            - Shrink each bin to tightly encompass its children%     query             - Ask which bins a new set of points belong to.%     plot, plot3       - Plots bin bounding boxes to the current axes.%% OcTree properties:%     Points            - The coordinate of points in the decomposition.%     PointBins         - Indices of the bin that each point belongs to.%     BinCount          - Total number of bins created.%     BinBoundaries     - BinCount-by-6 [MIN MAX] coordinates of bin edges.%     BinDepths         - The # of subdivisions to reach each bin.%     BinParents        - Indices of the bin that each bin belongs to.%     Properties        - Name/Val pairs used for creation (see help above)%% See also qtdecomp.%   Created by Sven Holcombe.%   1.0     - 2013-03 Initial release%   1.1     - 2013-03 Added shrinking bins and allocate/deallocate space%%   Please post comments to the FEX page for this entry if you have any%   bugs or feature requests.        properties        Points;        PointBins;        BinCount;        BinBoundaries;        BinDepths;        BinParents = zeros(0,1);        Properties;    end        methods                function this = OcTree(pts,varargin)            % This is the OcTree header line            validateattributes(pts,{'numeric'},...                {'real','finite','nonnan','ncols', 3},...                mfilename,'PTS')                        % Initialise a single bin surrounding all given points            numPts = size(pts,1);            this.BinBoundaries = [min(pts,[],1) max(pts,[],1)];            this.Points = pts;            this.PointBins = ones(numPts,1);            this.BinDepths = 0;            this.BinParents(1) = 0;            this.BinCount = 1;                        % Allow custom setting of Properties            IP = inputParser;            IP.addParamValue('binCapacity',ceil(numPts)/10);            IP.addParamValue('maxDepth',inf);            IP.addParamValue('maxSize',inf);            IP.addParamValue('minSize',1000 * eps);            IP.addParamValue('style','equal');            IP.parse(varargin{:});            this.Properties = IP.Results;                        % Return on empty or trivial bins            if numPts<2, return; end                        % Start dividing!            this.preallocateSpace;            this.divide(1);            this.deallocateSpace;        end                % MATLAB performs better if arrays that grow are initialised,        % rather than grown during a loop. These two functions do just that        % before and after the identification of new beens.        function preallocateSpace(this)            numPts = size(this.Points,1);            numBins = numPts;            if isfinite(this.Properties.binCapacity)                numBins = ceil(2*numPts/this.Properties.binCapacity);            end            this.BinDepths(numBins) = 0;            this.BinParents(numBins) = 0;            this.BinBoundaries(numBins,1) = 0;        end        function deallocateSpace(this)            this.BinDepths(this.BinCount+1:end) = [];            this.BinParents(this.BinCount+1:end) = [];            this.BinBoundaries(this.BinCount+1:end,:) = [];        end                function divide(this, startingBins)            % Loop over each bin we will consider for division            for i = 1:length(startingBins)                binNo = startingBins(i);                                % Prevent dividing beyond the maximum depth                if this.BinDepths(binNo)+1 >= this.Properties.maxDepth                    continue;                end                                % Prevent dividing beyond a minimum size                                thisBounds = this.BinBoundaries(binNo,:);                binEdgeSize = diff(thisBounds([1:3;4:6]));                minEdgeSize = min(binEdgeSize);                maxEdgeSize = max(binEdgeSize);                if minEdgeSize < this.Properties.minSize                    continue;                end                                % There are two conditions under which we should divide                % this bin. 1: It's bigger than maxSize. 2: It contains                % more points than binCapacity.                oldCount = this.BinCount;                if nnz(this.PointBins==binNo) > this.Properties.binCapacity                    this.divideBin(binNo);                    this.divide(oldCount+1:this.BinCount);                    continue;                end                if maxEdgeSize>this.Properties.maxSize                    this.divideBin(binNo);                    this.divide(oldCount+1:this.BinCount);                    continue;                end            end        end                function divideBin(this,binNo)            % Gather the new points (a bit more efficient to copy once)            binPtMask = this.PointBins==binNo;            thisBinsPoints = this.Points(binPtMask,:);                        % Get the old corner points and the new division point            oldMin = this.BinBoundaries(binNo,1:3);            oldMax = this.BinBoundaries(binNo,4:6);            if strcmp('weighted',this.Properties.style) && any(binPtMask)                newDiv = mean(thisBinsPoints,1);            else                newDiv = mean([oldMin; oldMax], 1);            end                        % Build the new boundaries of our 8 subdivisions            minMidMax = [oldMin newDiv oldMax];            newBounds = minMidMax([...                1 2 3 4 5 6;                1 2 6 4 5 9;                1 5 3 4 8 6;                1 5 6 4 8 9;                4 2 3 7 5 6;                4 2 6 7 5 9;                4 5 3 7 8 6;                4 5 6 7 8 9]);                        % Determine to which of these 8 bins each current point belongs            binMap = cat(3,[0 0 0],[0 0 1],[0 1 0],[0 1 1],...                [1 0 0],[1 0 1],[1 1 0],[1 1 1]);            gtMask = bsxfun(@gt, thisBinsPoints, newDiv);            [~,binAssignment] = max(all(bsxfun(@eq,gtMask,binMap),2),[],3);            % [~, binAssignment] = ismember(gtMask,binMap,'rows'); % A little slower than above.                        % Make the new bins and reassign old points to them            newBinInds = this.BinCount+1:this.BinCount+8;            this.BinBoundaries(newBinInds,:) = newBounds;            this.BinDepths(newBinInds) = this.BinDepths(binNo)+1;            this.BinParents(newBinInds) = binNo;            this.PointBins(binPtMask) = newBinInds(binAssignment);            this.BinCount = this.BinCount + 8;        end                function shrink(this)            % Shrink all bins to bound only the points they contain            % WARNING: this operation creates gaps in the final space not            % covered by a bin. Only shrink OcTree structures when you only            % intend to use the points used to create the tree to query the            % tree space.            binChildren = arrayfun(@(i)find(this.BinParents==i),1:this.BinCount,'Un',0)';            binIsLeaf = cellfun(@isempty, binChildren);            for i = find(binIsLeaf(:))'                binShrink_recurse(i, true)            end                        function binShrink_recurse(binNo, isLeafBin)                % Build a list of all points that fall within one of the                % bins to be checked, and the list of which point falls in                % which bin.                oldBoundaryMin = this.BinBoundaries(binNo,1:3);                oldBoundaryMax = this.BinBoundaries(binNo,4:6);                if isLeafBin                    % Shrink bin based on child POINTS                    ptsMask = this.PointBins==binNo;                    if ~any(ptsMask)                        % No points, shrink the bin to infinitely small                        proposedBoundaries = [oldBoundaryMin oldBoundaryMin];                    else                        pts = this.Points(ptsMask,:);                        proposedBoundaries = [...                            max([oldBoundaryMin; min(pts,[],1)]) ...                            min([oldBoundaryMax; max(pts,[],1)])];                    end                else                    % Shrink bin based on child BINS                    childBoundaries = this.BinBoundaries(binChildren{binNo},:);                    proposedBoundaries = [min(childBoundaries(:,1:3),[],1) max(childBoundaries(:,4:6),[],1)];                end                                if ~isequal(proposedBoundaries, [oldBoundaryMin oldBoundaryMax])                    % We just shrunk the boundary. Make it official and                    % check the parent                    this.BinBoundaries(binNo,:) = proposedBoundaries;                    parentBin = this.BinParents(binNo);                    if parentBin>0                        binShrink_recurse(parentBin, false)                    end                end            end        end                function binNos = query(this, newPts, queryDepth)            % Get the OcTree bins that new query points belong to.            %            % BINS = OT.query(NEWPTS) searches the OcTree object OT and            % returns an N-by-1 vector of BINS giving the bin index in            % which each of the N points in NEWPTS is contained. For any            % query points outside all bins in OT, the index -1 is            % returned.            %            % BINS = OT.query(NEWPTS,DEPTH) restricts the search to DEPTH            % levels in the OT bin tree. Note that the first bin            % (containing all other bins in OT) has DEPTH = 1.            if nargin<3                queryDepth = max(this.BinDepths);            end                        numPts = size(newPts,1);            newPts = permute(newPts,[3 2 1]);            binNos = ones(numPts,1)*-1;                                    binChildren = arrayfun(@(i)find(this.BinParents==i),1:this.BinCount,'Un',0)';            binIsLeaf = cellfun(@isempty, binChildren);            ptQuery_recurse(1:numPts, this.BinParents==0, 0)                        function ptQuery_recurse(newIndsToCheck_, binsToCheck, depth)                % Build a list of all points that fall within one of the                % bins to be checked, and the list of which point falls in                % which bin.                boundsToCheck = this.BinBoundaries(binsToCheck,:);                [ptInBounds, subBinNo] = max(all(...                    bsxfun(@ge, newPts(:,:,newIndsToCheck_), boundsToCheck(:,1:3)) & ...                    bsxfun(@le, newPts(:,:,newIndsToCheck_), boundsToCheck(:,4:6))...                    ,2),[],1);                            if ~all(ptInBounds)                    % Special case usually when depth=0, where a point may                    % fall outside the bins entirely. This should only                    % happen once so let's fix it once and let subsequent                    % code rely on all points being in bounds                    binNos(newIndsToCheck_(~ptInBounds)) = -1;                    newIndsToCheck_(~ptInBounds) = [];                    subBinNo(~ptInBounds) = [];                end                binNosToAssign = binsToCheck(subBinNo);                newIndsToAssign = newIndsToCheck_;                binNos(newIndsToAssign) = binNosToAssign;                                % Allow a free exit when we reach a certain depth                if depth>=queryDepth                    return;                end                                % Otherwise, for all of the points we just placed into                % bins, check which of the children of those bins those                % same points fall into                [unqBinNos, ~, unqGrpNos] = unique(binNosToAssign);                for i = 1:length(unqBinNos)                    thisPtMask = unqGrpNos==i;                    if ~binIsLeaf(unqBinNos(i))                        ptQuery_recurse(newIndsToCheck_(thisPtMask), binChildren{unqBinNos(i)}, depth+1)                    end                end                            end        end                function h = plot(this,varargin)            % OcTree.plot plots bin bounding boxes of an OcTree object            %            % H = OT.plot('name',value,...) allows you to specify any            % properties of the bounding box lines that you would normally            % supply to a plot(...,'name',value) command, and returns plot            % object handles (one per bin) to H.            hold on;            h = zeros(this.BinCount,1);            for i = 1:this.BinCount                binMinMax = this.BinBoundaries(i,:);                pts = cat(1, binMinMax([...                    1 2 3; 4 2 3; 4 5 3; 1 5 3; 1 2 3;...                    1 2 6; 4 2 6; 4 5 6; 1 5 6; 1 2 6; 1 2 3]),...                    nan(1,3), binMinMax([4 2 3; 4 2 6]),...                    nan(1,3), binMinMax([4 5 3; 4 5 6]),...                    nan(1,3), binMinMax([1 5 3; 1 5 6]));                h(i) = plot3(pts(:,1),pts(:,2),pts(:,3),varargin{:});            end        end        function h = plot3(this,varargin)            % OcTree.plot plots bin bounding boxes of an OcTree            %            % See also OcTree.plot            h = this.plot(varargin{:});        end    endend

main.m

pts = (rand(200,3)-0.5).^2;OT = OcTree(pts,'binCapacity',20);        figureboxH = OT.plot;cols = lines(OT.BinCount);doplot3 = @(p,varargin)plot3(p(:,1),p(:,2),p(:,3),varargin{:});for i = 1:OT.BinCount   set(boxH(i),'Color',cols(i,:),'LineWidth', 1+OT.BinDepths(i))   doplot3(pts(OT.PointBins==i,:),'.','Color',cols(i,:))endaxis image, view(3)pts = rand(200,3);OT = OcTree(pts,'binCapacity',10,'style','weighted');OT.shrinkfigureboxH = OT.plot;cols = lines(OT.BinCount);doplot3 = @(p,varargin)plot3(p(:,1),p(:,2),p(:,3),varargin{:});for i = 1:OT.BinCount   set(boxH(i),'Color',cols(i,:),'LineWidth', 1+OT.BinDepths(i))   doplot3(pts(OT.PointBins==i,:),'.','Color',cols(i,:))endaxis image, view(3)

 

转载地址：http://nkwlf.baihongyu.com/