求3D點集中最近點的一個空間二叉樹實現

                  求3D點集中最近點的一個空間二叉樹實現
                   [email protected]  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個點