求3D点集中最近点的一个空间二叉树实现

                  求3D点集中最近点的一个空间二叉树实现
                   HouSisong@GMail.com  2010.09.12

 

(2011-03-04  修改节点内存泄漏,漏了一句m_nodePool.push_back(m_cur); )

 

tag:求3D点集中最近点,空间二叉树,点集最小包围球

摘要:
    这几天为了解决一个颜色空间匹配搜索速度太慢的问题(求3D点集中最近点),写了一个基于3D空间二叉树分割的算法的实现,

速度提高了几十倍;

正文：    

A: 问题简单描述
   已知3D点集U(较多),求出U中与给定点p最近的点;可能会非常多次 的给定不同的p点,而点集U不变;
 
B: 一些基础代码和最简单的一个实现和其简要优化
定义3D点:
//3D点 struct T3DPoint{ double x; double y; double z; must_inline T3DPoint(){} must_inline T3DPoint(double _x,double _y,double _z):x(_x),y(_y),z(_z){} must_inline T3DPoint(const T3DPoint& p):x(p.x),y(p.y),z(p.z) {} }; typedef std::vector<T3DPoint> T3DPointArray; template<class T> must_inline static T sqr(T x){ return x*x; } //返回两个点之间距离的平方 must_inline double getPointDistanceSqr(const T3DPoint& p0,const T3DPoint& p1){ return sqr(p0.x-p1.x)+sqr(p0.y-p1.y)+sqr(p0.z-p1.z); } //返回两个点之间的距离 must_inline double getPointDistance(const T3DPoint& p0,const T3DPoint& p1){ return sqrt(getPointDistanceSqr(p0,p1)); }

寻找最近点的算法简单实现:
const double csMaxDistance=1e300; //遍历算法 long getMinPointByLoop0(const T3DPoint* points,const long pointsCount,const T3DPoint& test_point,double& minDistance){ minDistance=csMaxDistance; long minIndex=-1; for (long i=0;i<pointsCount;++i){ double curMin=getPointDistance(test_point,points[i]); if (curMin<minDistance){ minDistance=curMin; minIndex=i; } } return minIndex; }
该算法遍历点集U(points[]),找寻点集中距离test_point最近的点,返回其坐标,算法复杂度为O(N);

简单优化:
前面的程序计算点之间的距离来比较大小,涉及到开方运算,比较慢;而比较的时候,改成比较距离的平方效果也一样,
函数返回距离的时候,在把最终结果开方就好了;代码如下:
//遍历算法 简单优化 long getMinPointByLoop1(const T3DPoint* points,const long pointsCount,const T3DPoint& test_point,double& minDistance){ double minSqr=csMaxDistance; long minIndex=-1; for (long i=0;i<pointsCount;++i){ double curMinSqr=getPointDistanceSqr(test_point,points[i]); if (curMinSqr<minSqr){ minSqr=curMinSqr; minIndex=i; } } if (minIndex>=0) minDistance=sqrt(minSqr); return minIndex; }

这个代码的速度是前面实现的3倍多;
当然该算法还可以继续优化的;但因为算法复杂度太高,进一步优化的意义不大;
下面给出一个平均复杂度O(log(N))的算法

C: 基于3D空间二叉树分割的算法的实现
   对点集U进行预处理,生成一棵3D空间二叉树:
     将点集U的最长轴按中值划分成2个新的点集；计算新形成的2个点集的最小包围球;
     如果新划分出的点集的点个数比较多,可以进一步划分,一直到不用划分;
(划分方法可以是该轴的中值、均值或平分个数的值等,程序里选择用中值划分;
包围体也可以为其它3D几何包围体,比如正6面体等,程序里选择球型包围体,算法实现更简单快速些)

   利用二叉树寻找离给定点最近的点:
      程序用深度优先顺序遍历二叉树,遇到有2分支的时候优先处理距离近的分支;
      快速排除一个点集的算法原理: 假设搜寻过程中,当前已经得到的离p点最近的点的距离为minD,
现在寻找到了点集u(包围球半径ur,球心坐标up); 如果 (p到up的距离 - ur) > minD ,那么该点集u和
其子点集都可以立即排除掉了;而且该判断表达式还可以改进为 p到up的距离的平方>(minD+ur)的平方
计算起来就更加快速了;
     (如果想用广度优先算法遍历二叉树,可以维持一个队列,储存当前没有被排除的点集,然后展开这些点集,快速排除不可能的点集,直到没有点集分
支可以展开;这种遍历方式可以利用的快速排除一个点集的算法原理为: 假设搜寻过程中,当前已经得
到的离p点最近的点集距离为minD,该点集包围球半径为ur,现在寻找到了点集v(包围球半径vr,球心坐
标vp); 如果 (p到vp的距离 - mr) > (minD+ur),那么该点集v和其子点集就可以立即排除掉;该判断表
达式也可以改进为 p到vp的距离的平方 > (minR+ur+mr)的平方 !    )

代码实现:
//3DDistance.h #ifndef _3DDistance_h_ #define _3DDistance_h_ #include <math.h> #include <vector> #ifdef _MSC_VER #define must_inline __forceinline #else #ifdef __GNUC__ #define must_inline __attribute__((always_inline)) #else #define must_inline inline #endif #endif //几个简单的工具函数 没有用std里的 template<class T> must_inline static T mysqr(T x){ return x*x; } template<class T,class T0> must_inline static T mymin(T x,T0 y){ if (x<=y) return x; else return y; } template<class T,class T0> must_inline static T mymax(T x,T0 y){ if (x>=y) return x; else return y; } template<class T> must_inline static void myswap(T& x,T& y){ T t(x); x=y; y=t; } //3D点 //支持按角标和数组方式访问 struct T3DPointData{ union { double vec[3]; struct { double x; double y; double z; }; }; void* data; must_inline T3DPointData(){} must_inline T3DPointData(double _x,double _y,double _z,void* _data):x(_x),y(_y),z(_z),data(_data){} must_inline T3DPointData(const T3DPointData& p):x(p.x),y(p.y),z(p.z),data(p.data) {} }; //返回两个点之间距离的平方 must_inline double getPointDistanceSqr(const T3DPointData& p0,const T3DPointData& p1){ return mysqr(p0.x-p1.x)+mysqr(p0.y-p1.y)+mysqr(p0.z-p1.z); } //返回两个点之间的距离 must_inline double getPointDistance(const T3DPointData& p0,const T3DPointData& p1){ return sqrt(getPointDistanceSqr(p0,p1)); } //空间二叉树的一个节点 struct T3DSpaceNode{ T3DSpaceNode* nodeList[2]; //指向两个子节点 T3DPointData* pointList; //该节点空间内的3D点数组 long pointCount; //该节点空间内的3D点个数 double r; //包围所有的点的球的半径 T3DPointData p0; //包围所有的点的球的球心 }; //用来快速分配节点的内存 class T3DSpaceNodePool{ private: std::vector<T3DSpaceNode*> m_nodePool; T3DSpaceNode* m_cur; T3DSpaceNode* m_end; void pushNewNodeList(){ const long csNewPoolNodeCount=512; m_cur=new T3DSpaceNode[csNewPoolNodeCount]; //memset(m_cur,0,size(T3DSpaceNode)*csNewPoolNodeCount); m_end=m_cur+csNewPoolNodeCount; m_nodePool.push_back(m_cur); } public: T3DSpaceNodePool():m_cur(0),m_end(0){} ~T3DSpaceNodePool(){ clear(); } void clear(){ m_cur=0; m_end=0; long nodeCount=(long)m_nodePool.size(); for (long i=0;i<nodeCount;++i) delete [] m_nodePool[i]; m_nodePool.clear(); } must_inline T3DSpaceNode* getANewNode(){ if (m_cur==m_end) pushNewNodeList(); return m_cur++; } }; //对点进行3D空间分割,建立空间二叉树 class T3DSpace{ private: T3DSpaceNode m_nodeTree; T3DSpaceNodePool m_nodePool; std::vector<T3DPointData> m_pointList; void createNode(T3DSpaceNode& node,T3DPointData* pointList,long pointCount); public: T3DSpace(); ~T3DSpace(){ clear(); } void clear(); must_inline void addPoint(const T3DPointData& pt){ m_pointList.push_back(pt); } void endAddPoint(); //返回点集中距离p0点最近的点,并且该点距离p0必须小于curMinDistance const T3DPointData* getMinDistancePoint(const T3DPointData& p0,double& curMinDistance) const; }; #endif //#ifndef _3DDistance_h_

//3DDistance.cpp #include "3DDistance.h" //#include <algorithm> T3DSpace::T3DSpace(){ memset(&m_nodeTree,0,sizeof(T3DSpaceNode)); } void T3DSpace::clear(){ memset(&m_nodeTree,0,sizeof(T3DSpaceNode)); m_nodePool.clear(); m_pointList.clear(); } void T3DSpace::endAddPoint(){ if (m_pointList.size()>0) createNode(m_nodeTree,&m_pointList[0],m_pointList.size()); } /* //最小包围球算法 struct TCircle{ T3DPointData p0; double rSqr; }; TCircle getMinCircle3Point(T3DPointData* pointList){ //求3个边距离 判断最长边对应夹角是否>=90度(c^2>=a^2+b^2) // 是,则最长边为圆直径 ... return; // 否,则返回过3各点的圆 (圆的圆心与3各点同面) } TCircle getMinCircle4Point(T3DPointData* pointList,long& out_index){ //选出4点其中的3个点,使由它们生成的一个包含这4个点的圆为最小 } TCircle getMinCircle(T3DPointData* pointList,long pointCount){ TCircle rt; switch (pointCount){ case 0:{ rt.p0=T3DPointData(0,0,0,0); rt.rSqr=0; }break; case 1:{ rt.p0=pointList[0]; rt.rSqr=0; }break; case 2:{ rt.p0.x=(pointList[0].x+pointList[1].x)*0.5; rt.p0.y=(pointList[0].y+pointList[1].y)*0.5; rt.p0.z=(pointList[0].z+pointList[1].z)*0.5; rt.rSqr=getPointDistanceSqr(rt.p0,pointList[0]); }break; case 3:{ rt=getMinCircle3Point(pointList); }break; case 4:{ long out_index=-1; rt=getMinCircle4Point(pointList,out_index); }break; default:{ // 1. 在点集中任取3点A,B,C。 // 2. 作一个包含A,B,C三点的最小圆。 // 3. 在点集中找出距离第2步所建圆圆心最远的D点，若D点已在圆内或圆周上， // 则该圆即为所求的圆，算法结束.则，执行第4步。 // 4. 在A,B,C,D中选3个点,使由它们生成的一个包含这4个点的圆为最小，这3 // 点成为新的A,B,C，返回执行第2步。若在第4步生成的圆的圆周只通过A,B,C,D // 中的两点，则圆周上的两点取成新的A和B,从另两点中任取一点作为新的C。 }break; } return rt; }*/ long splitPointList(T3DPointData* pointList,long pointCount,double mValue0,double mValue1,int vPos){ double mValue=(mValue0+mValue1)*0.5; //这里按vPos轴的中值把点分成2部分 也可以用其他分割方案 比如个数/中值等 long insert_i=0; for (long i=0;i<pointCount;++i){ if (pointList[i].vec[vPos]<=mValue){ myswap(pointList[i],pointList[insert_i]); ++insert_i; } } return insert_i; //返回第一部分的点数目 } //创建一个节点, 并可能创建2子节点(递归调用来创建) void T3DSpace::createNode(T3DSpaceNode& node,T3DPointData* pointList,long pointCount){ node.pointList=pointList; node.pointCount=pointCount; node.nodeList[0]=0; node.nodeList[1]=0; node.p0.data=0; if (pointCount==1){ node.p0=pointList[0]; node.r=0; return; } long i; //get box double x0=pointList[0].x; double x1=x0; double y0=pointList[0].y; double y1=y0; double z0=pointList[0].z; double z1=z0; for (i=1;i<pointCount;++i){ x0=mymin(x0,pointList[i].x); x1=mymax(x1,pointList[i].x); y0=mymin(y0,pointList[i].y); y1=mymax(y1,pointList[i].y); z0=mymin(z0,pointList[i].z); z1=mymax(z1,pointList[i].z); } //获得点集的最小包围球 这里不实现了 代码量较大 下面的代码是简单实现 //TCircle c=getMinCircle(pointList,pointCount); //node.p0=c.p0; //node.r=1.00001*sqrt(c.rSqr); //球心 node.p0.x=(x1+x0)*0.5; node.p0.y=(y1+y0)*0.5; node.p0.z=(z1+z0)*0.5; //获得球的半径 //node.r=1.00001*getPointDistance(node.p0,T3DPointData(x0,y0,z0,0)); //简易算法获得半径 double rs=0; for (i=0;i<pointCount;++i){ double crs=getPointDistanceSqr(node.p0,pointList[i]); if (crs>rs) rs=crs; } node.r=1.00001*sqrt(rs);//乘以一个系数以避免浮点运算可能产生的误差使浮点比较出差错 double dx=x1-x0; double dy=y1-y0; double dz=z1-z0; if (dx+dy+dz==0){ pointCount=1; node.pointCount=pointCount; } const long csMaxPointCount=16; //页节点最大容纳的点个数 if (pointCount>csMaxPointCount){ //分裂边长最大的轴 double mValue0; double mValue1; int vPos; if ((dx>=dy)&&(dx>=dz)){ mValue0=x0; mValue1=x1; vPos=0; }else if ((dy>=dx)&&(dy>=dz)){ mValue0=y0; mValue1=y1; vPos=1; }else {//if ((dz>=dx)&&(dz>=dy)){ mValue0=z0; mValue1=z1; vPos=2; } long leftCount=splitPointList(pointList,pointCount,mValue0,mValue1,vPos); if ((leftCount>0)&&(leftCount<pointCount)){ node.nodeList[0]=m_nodePool.getANewNode(); createNode(*node.nodeList[0],pointList,leftCount); node.nodeList[1]=m_nodePool.getANewNode(); createNode(*node.nodeList[1],pointList+leftCount,pointCount-leftCount); }else{ //assert(false); //不可能执行到这里,除非代码有bug } } } //深度优先算法的一个实现 void _getMinDistancePoint(const T3DSpaceNode& node,const T3DPointData& p0,const T3DPointData*& curMinPoint,double& curMinDistance){ if (node.nodeList[0]==0){ double dmSqr=mysqr(curMinDistance); T3DPointData* minPoint=0; for (long i=0;i<node.pointCount;++i){ double diSqr=getPointDistanceSqr(p0,node.pointList[i]); if (diSqr<dmSqr){ dmSqr=diSqr; minPoint=&node.pointList[i]; } } if (minPoint!=0){ curMinPoint=minPoint; curMinDistance=sqrt(dmSqr); } }else{ T3DSpaceNode* node0=node.nodeList[0]; T3DSpaceNode* node1=node.nodeList[1]; double dSqr0=getPointDistanceSqr(p0,node0->p0); double dSqr1=getPointDistanceSqr(p0,node1->p0); if (dSqr0>dSqr1){//优先处理近一些的点集 myswap(node0,node1); myswap(dSqr0,dSqr1); } //快速排除不可能的分支 if (dSqr0<mysqr(curMinDistance+node0->r)){ _getMinDistancePoint(*node0,p0,curMinPoint,curMinDistance); } if (dSqr1<mysqr(curMinDistance+node1->r)){ _getMinDistancePoint(*node1,p0,curMinPoint,curMinDistance); } } } const T3DPointData* T3DSpace::getMinDistancePoint(const T3DPointData& p0,double& curMinDistance)const{ double dSqr=getPointDistanceSqr(p0,m_nodeTree.p0); if (dSqr<mysqr(curMinDistance+m_nodeTree.r)) { const T3DPointData* curMinPoint=0; _getMinDistancePoint(m_nodeTree,p0,curMinPoint,curMinDistance); return curMinPoint; }else return 0; //快速排除 } 

D:完整测试代码

// 3DPointDistanceTest.cpp #include <stdio.h> #include <stdlib.h> #include <math.h> #include <iostream> #include <time.h> #include <vector> #ifdef _MSC_VER #define must_inline __forceinline #else #ifdef __GNUC__ #define must_inline __attribute__((always_inline)) #else #define must_inline inline #endif #endif //等待一个回车输入 static void waitInputChar(){ while (true){ int c=getchar(); if (c=='/n') break; } } //3D点 //支持按角标和数组方式访问 struct T3DPoint{ double x; double y; double z; must_inline T3DPoint(){} must_inline T3DPoint(double _x,double _y,double _z):x(_x),y(_y),z(_z){} must_inline T3DPoint(const T3DPoint& p):x(p.x),y(p.y),z(p.z) {} }; typedef std::vector<T3DPoint> T3DPointArray; template<class T> must_inline static T sqr(T x){ return x*x; } //返回两个点之间距离的平方 must_inline double getPointDistanceSqr(const T3DPoint& p0,const T3DPoint& p1){ return sqr(p0.x-p1.x)+sqr(p0.y-p1.y)+sqr(p0.z-p1.z); } //返回两个点之间的距离 must_inline double getPointDistance(const T3DPoint& p0,const T3DPoint& p1){ return sqrt(getPointDistanceSqr(p0,p1)); } const double csMaxDistance=1e300; //遍历算法 long getMinPointByLoop0(const T3DPoint* points,const long pointsCount,const T3DPoint& test_point,double& minDistance){ minDistance=csMaxDistance; long minIndex=-1; for (long i=0;i<pointsCount;++i){ double curMin=getPointDistance(test_point,points[i]); if (curMin<minDistance){ minDistance=curMin; minIndex=i; } } return minIndex; } //遍历算法 简单优化 long getMinPointByLoop1(const T3DPoint* points,const long pointsCount,const T3DPoint& test_point,double& minDistance){ double minSqr=csMaxDistance; long minIndex=-1; for (long i=0;i<pointsCount;++i){ double curMinSqr=getPointDistanceSqr(test_point,points[i]); if (curMinSqr<minSqr){ minSqr=curMinSqr; minIndex=i; } } if (minIndex>=0) minDistance=sqrt(minSqr); return minIndex; } //空间2叉树 #include "3DDistance.h" void createTree(const T3DPoint* points,const long pointsCount,T3DSpace& a3DSpace){ a3DSpace.clear(); for (long i=0;i<pointsCount;++i) a3DSpace.addPoint(T3DPointData(points[i].x,points[i].y,points[i].z,(void*)i)); a3DSpace.endAddPoint(); } long getMinPointByTree(const T3DSpace& a3DSpace,const T3DPoint& test_point,double& minDistance){ minDistance=csMaxDistance; const T3DPointData* p=a3DSpace.getMinDistancePoint(T3DPointData(test_point.x,test_point.y,test_point.z,0),minDistance); if (p==0) return -1; else return (long)p->data; } must_inline double randf(){ // -1,+1 double r=rand()*(1.0/RAND_MAX); if (rand()*2<RAND_MAX) return -r; else return r; } void test(const long pointsCount){ std::cout<<"--------------------------------------------"<<std::endl; std::cout<<"3D点集数量: "<<pointsCount<<std::endl; T3DPointArray points; T3DPointArray test_points; long i; for (i=0;i<pointsCount;++i){ points.push_back(T3DPoint(randf()*150.0,randf()*70.0,randf()*100.0)); } const long testCount=1000000; for (i=0;i<testCount;++i){ test_points.push_back(T3DPoint(randf()*243.0,randf()*120.0,randf()*310.0)); } //std::vector<long> minDistanceIndexArray0; //std::vector<long> minDistanceIndexArray1; //std::vector<long> minDistanceIndexArray2; { srand(0); std::cout<<" 遍历算法0"<<": "; clock_t t0=clock(); clock_t t1=t0; long runCount=0; long test_index=0; while(true){ double minDistance; long minIndex=getMinPointByLoop0(&points[0],pointsCount,test_points[test_index],minDistance); //minDistanceIndexArray0.push_back(minIndex); ++runCount; ++test_index; if (test_index>=testCount) test_index-=testCount; if (runCount%512==0){ t1=clock(); if ((t1-t0)>=5*CLOCKS_PER_SEC) //控制测试时间 break; } } long speed=(long)(0.5+runCount/((t1-t0)*(1.0/CLOCKS_PER_SEC))); std::cout<<"每秒处理"<<speed<<"个点"<<std::endl; } { srand(0); std::cout<<" 遍历算法1"<<": "; clock_t t0=clock(); clock_t t1=t0; long runCount=0; long test_index=0; while(true){ double minDistance; long minIndex=getMinPointByLoop1(&points[0],pointsCount,test_points[test_index],minDistance); //minDistanceIndexArray1.push_back(minIndex); ++runCount; ++test_index; if (test_index>=testCount) test_index-=testCount; if (runCount%512==0){ t1=clock(); if ((t1-t0)>=5*CLOCKS_PER_SEC) //控制测试时间 break; } } long speed=(long)(0.5+runCount/((t1-t0)*(1.0/CLOCKS_PER_SEC))); std::cout<<"每秒处理"<<speed<<"个点"<<std::endl; } { srand(0); std::cout<<" 空间2叉树算法: "; clock_t t0=clock(); clock_t t1=t0; T3DSpace a3DSpace; createTree(&points[0],pointsCount,a3DSpace); long runCount=0; long test_index=0; while(true){ double minDistance; long minIndex=getMinPointByTree(a3DSpace,test_points[test_index],minDistance); //minDistanceIndexArray2.push_back(minIndex); ++runCount; ++test_index; if (test_index>=testCount) test_index-=testCount; if (runCount%512==0){ t1=clock(); if ((t1-t0)>=5*CLOCKS_PER_SEC) //控制测试时间 break; } } long speed=(long)(0.5+runCount/((t1-t0)*(1.0/CLOCKS_PER_SEC))); std::cout<<"每秒处理"<<speed<<"个点"<<std::endl; } } /// // int main(){ std::cout<<" 请输入回车键开始测试(可以把进程优先级设置为“实时”)> "; waitInputChar(); std::cout<<std::endl; //for test( 64); test( 256); test( 1000); test( 10000); test( 100000); test(1000000); std::cout<<std::endl<<" 测试完成. "; waitInputChar(); return 0; } 

E:速度对比
   使用的编译器为vc2008;测试使用的CPU为i7 920;
--------------------------------------------
3D点集数量: 64
      遍历算法0: 每秒处理1134824个点
      遍历算法1: 每秒处理3564980个点
  空间2叉树算法: 每秒处理4871770个点
--------------------------------------------
3D点集数量: 256
      遍历算法0: 每秒处理291841个点
      遍历算法1: 每秒处理1059374个点
  空间2叉树算法: 每秒处理3144091个点
--------------------------------------------
3D点集数量: 1000
      遍历算法0: 每秒处理76064个点
      遍历算法1: 每秒处理295625个点
  空间2叉树算法: 每秒处理2068754个点
--------------------------------------------
3D点集数量: 10000
      遍历算法0: 每秒处理7643个点
      遍历算法1: 每秒处理30166个点
  空间2叉树算法: 每秒处理1111412个点
--------------------------------------------
3D点集数量: 100000
      遍历算法0: 每秒处理772个点
      遍历算法1: 每秒处理2984个点
  空间2叉树算法: 每秒处理604217个点
--------------------------------------------
3D点集数量: 1000000
      遍历算法0: 每秒处理83个点
      遍历算法1: 每秒处理272个点
  空间2叉树算法: 每秒处理272924个点