《PCL点云库学习&VS2010(X64)》Part 48 基于霍夫变换的点云平面检测法

《PCL点云库学习&VS2010(X64)》Part 48 基于霍夫变换的点云平面检测法

参考文献：

Dorit Borrmann, Jan Elseberg, Kai Lingemann, and Andreas Nüchter. The 3D Hough Transform for Plane Detection in Point Clouds - A Review and A new Accumulator Design, Journal 3D Research, Springer, Volume 2, Number 2, March 2011.

代码：

（1）随机霍夫变换

/**  * Randomized Hough Transform  */int Hough::RHT() {  if (!quiet) cout << "RHT" << endl;  Point p1, p2, p3;  double theta, phi, rho;  int planeSize = 2000;  unsigned int stop = (unsigned int)(allPoints->size()/100.0)*myConfigFileHough.Get_MinSizeAllPoints();  int plane = 1;  long start, end;  start = GetCurrentTimeInMilliSec();   int counter = 0;  while( allPoints->size() > stop &&           planes.size() < (unsigned int)myConfigFileHough.Get_MaxPlanes() &&          counter < (int)myConfigFileHough.Get_TrashMax()) {     unsigned int pint = (int) (((*allPoints).size())*(rand()/(RAND_MAX+1.0)));    p1 = (*allPoints)[pint];    pint = (int) (((*allPoints).size())*(rand()/(RAND_MAX+1.0)));    p2 = (*allPoints)[pint];    pint = (int) (((*allPoints).size())*(rand()/(RAND_MAX+1.0)));    p3 = (*allPoints)[pint];    // check distance    if(!distanceOK(p1, p2, p3)) continue;    //cout << "Distance OK" << endl;    // calculate Plane    if(calculatePlane(p1, p2, p3, theta, phi, rho)) {      // increment accumulator cell      if(acc->accumulate(theta, phi, rho)) {        end = GetCurrentTimeInMilliSec() - start;        start = GetCurrentTimeInMilliSec();        if (!quiet) cout << "Time for RHT " << plane << ": " << end << endl;         double * n = acc->getMax(rho, theta, phi);       if (!quiet) cout << rho << " " << theta << " " << phi << endl;        planeSize = deletePoints(n, rho);        delete[] n;       if (!quiet) cout << "Delete Points done " << plane << endl;        if(planeSize < (int)myConfigFileHough.Get_MinPlaneSize()) counter++;        end = GetCurrentTimeInMilliSec() - start;        start = GetCurrentTimeInMilliSec();        if(!quiet) cout << "Time for Polygonization " << plane << ": " << end << endl;         acc->resetAccumulator();        plane++;        if(!quiet) cout << "Planes " << planes.size() << endl;      }    }  }  /*  vector<Point>::iterator itr = allPoints->begin();  Point p;  start = GetCurrentTimeInMilliSec();   while(itr != allPoints->end()) {    p = *(itr);    cout << p.x << " " << p.y << " " << p.z << endl;    itr++;  }   */  return (int)planes.size();}

（2）标准霍夫变换方法

/** * Standard Hough Transform */void Hough::SHT() {  vector<Point>::iterator itr = allPoints->begin();  Point p;  long start, end;  start = GetCurrentTimeInMilliSec();   while(itr != allPoints->end()) {    p = *(itr);    acc->accumulate(p);    itr++;  }   end = GetCurrentTimeInMilliSec() - start;  start = GetCurrentTimeInMilliSec();  if (!quiet) cout << "Time for SHT: " << end << endl;   if(myConfigFileHough.Get_PeakWindow()) {    acc->peakWindow(myConfigFileHough.Get_WindowSize());  }  multiset<int*, maxcompare>* maxlist = acc->getMax();  multiset<int*, maxcompare>::iterator it = maxlist->begin();  int threshold = ((*it)[0] * myConfigFileHough.Get_PlaneRatio());  unsigned int stop = (int)(allPoints->size()/100.0)*myConfigFileHough.Get_MinSizeAllPoints();  while(it != maxlist->end() &&         stop < allPoints->size() &&         planes.size() < (unsigned int)myConfigFileHough.Get_MaxPlanes() &&         (*it)[0] > threshold) {    int* tmp = (*it);    double * polar = acc->getMax(tmp);    deletePoints(polar, polar[3]);     delete[] polar;    it++;  }   if (!quiet) cout << "Time for Polygonization: " << end << endl;   it = maxlist->begin();    for(;it != maxlist->end(); it++) {      int* tmp = (*it);      delete[] tmp;  }   delete maxlist; }

（3）概率霍夫变换

/** * Probabilistic Hough Transform */void Hough::PHT() {  unsigned int stop =  (int)(allPoints->size()/100.0)*myConfigFileHough.Get_MinSizeAllPoints();  bool *voted = new bool[allPoints->size()];  for(unsigned int i = 0; i < allPoints->size(); i++) {    voted[i] = false;  }  cout << stop << endl;  unsigned int i = 0;  while(i < stop && planes.size() < (unsigned int)myConfigFileHough.Get_MaxPlanes()) {    unsigned int pint = (int) (((*allPoints).size())*(rand()/(RAND_MAX+1.0)));    if(!voted[pint]) {      Point p = (*allPoints)[pint];      acc->accumulate(p);      i++;      voted[pint] = true;    }  }  // List of Maxima  if(myConfigFileHough.Get_PeakWindow()) {    acc->peakWindow(myConfigFileHough.Get_WindowSize());  }  multiset<int*, maxcompare>* maxlist = acc->getMax();//  cout << "Mean "  << acc->mean() << endl;//  cout << "Variance "  << acc->variance() << endl;  multiset<int*, maxcompare>::iterator it = maxlist->begin();  int threshold = ((*it)[0] * myConfigFileHough.Get_PlaneRatio());  while(it != maxlist->end() &&         stop < allPoints->size() &&         planes.size() < (unsigned int)myConfigFileHough.Get_MaxPlanes() &&         (*it)[0] > threshold) {    int* tmp = (*it);    double * tmp2 = acc->getMax(tmp);    deletePoints(tmp2, tmp2[3]);    delete [] tmp2;    it++;  }   it = maxlist->begin();    for(;it != maxlist->end(); it++) {      int* tmp = (*it);      delete[] tmp;  }   delete maxlist;   delete[] voted;}

（4）渐进概率霍夫变换

/** * Progressive Probabilistic Hough Transform */void Hough::PPHT() {  unsigned int stop =  (int)(allPoints->size()/100.0)*myConfigFileHough.Get_MinSizeAllPoints();  while(stop < allPoints->size() &&         planes.size() < (unsigned int)myConfigFileHough.Get_MaxPlanes()) {    bool *voted = new bool[allPoints->size()];    for(unsigned int i = 0; i < allPoints->size(); i++) {      voted[i] = false;    }    unsigned int pint;    do {       pint = (int) (((*allPoints).size())*(rand()/(RAND_MAX+1.0)));      Point p = (*allPoints)[pint];      if(!voted[pint]) {        double * angles = acc->accumulateRet(p);        if(angles[0] > -0.0001) {          double * n = polar2normal(angles[1], angles[2]);          deletePoints(n, angles[0]);          acc->resetAccumulator();          delete [] n;        } else {          voted[pint] = true;        }        delete [] angles;      }    } while(!voted[pint]);    delete[] voted;  }}

（5）自适应霍夫变换

/** * Adaptive Probabilistic Hough Transform */void Hough::APHT() {  int max = 0;  int maxpos = 0;  vector<int*> mergelist = vector<int*>();   int stability[20] = {0};   do {    //randomly select points and perform HT for them    vector<int*> altlist = mergelist;     mergelist = vector<int*>();     multiset<int*,valuecompare> maxlist = multiset<int*,valuecompare>();    for(int i = 0; i < 10; i++) {      unsigned int pint = (int) (((*allPoints).size())*(rand()/(RAND_MAX+1.0)));      Point p = (*allPoints)[pint];      int * max = acc->accumulateAPHT(p);      // store the maximum cell touched by the HT      maxlist.insert(max);    }    // merge active with previous list    multiset<int*,valuecompare>::iterator mi = maxlist.begin();     vector<int*>::iterator ai = altlist.begin();     int i = 0;    while(i < 20) {      bool mi_oder_ai = false;      if(ai == altlist.end()) {        if(mi == maxlist.end()) {          break;        } else {          mi_oder_ai = true;        }      } else if(mi == maxlist.end()) {        mi_oder_ai = false;      } else if((*mi)[0] <= (*ai[0])) {        mi_oder_ai = false;      } else {        mi_oder_ai = true;      }      int *tmp;      if(mi_oder_ai) {        tmp = (*mi);        mi++;      } else {        tmp = (*ai);        ai++;      }      bool insert = true;      for(vector<int*>::iterator it = mergelist.begin(); it != mergelist.end();    it++) {        if(myConfigFileHough.Get_AccumulatorType() != 2) {          if( sqrt((float)            ((*it)[3] - (tmp[3])) * ((*it)[3] - (tmp[3])) +            ((*it)[1] - (tmp[1])) * ((*it)[1] - (tmp[1])) +            ((*it)[2] - (tmp[2])) * ((*it)[2] - (tmp[2])) ) < 3.0f) {            insert = false;            break;          }        } else {           if( sqrt((float)            ((*it)[4] - (tmp[4])) * ((*it)[4] - (tmp[4])) +            ((*it)[1] - (tmp[1])) * ((*it)[1] - (tmp[1])) +            ((*it)[2] - (tmp[2])) * ((*it)[2] - (tmp[2])) +            ((*it)[3] - (tmp[3])) * ((*it)[3] - (tmp[3])) ) < 3.0f) {            insert = false;            break;          }        }      }      if(insert) {        mergelist.push_back(tmp);        i++;      }    }    // compare lists, calculate stability    i = 0;    for(unsigned int i = 1; i < altlist.size(); i++) {      for(unsigned int j = 0; j < i; j++) {        int * tmp1 = mergelist[j];        bool treffer = false;        for(unsigned int k = 0; k < i; k++) {          int * tmp2 = altlist[k];          if(myConfigFileHough.Get_AccumulatorType() != 2) {            if( sqrt((float)              ((tmp2)[3] - (tmp1[3])) * ((tmp2)[3] - (tmp1[3])) +              ((tmp2)[1] - (tmp1[1])) * ((tmp2)[1] - (tmp1[1])) +              ((tmp2)[2] - (tmp1[2])) * ((tmp2)[2] - (tmp1[2])) ) < 3.0f) {              treffer = true;              break;            }          } else {             if( sqrt((float)              ((tmp2)[4] - (tmp1[4])) * ((tmp2)[4] - (tmp1[4])) +              ((tmp2)[1] - (tmp1[1])) * ((tmp2)[1] - (tmp1[1])) +              ((tmp2)[2] - (tmp1[2])) * ((tmp2)[2] - (tmp1[2])) +              ((tmp2)[3] - (tmp1[3])) * ((tmp2)[3] - (tmp1[3])) ) < 3.0f) {              treffer = true;              break;            }          }        }        if(!treffer) {          stability[i-1] = -1;          break;        }      }      stability[i-1]++;    }    for(int i = mergelist.size(); i < 20; i++) {      stability[i] = 0;    }    // determine stability count,     // a set of n entries is considered to be stable, if all planes in the set    // are the maximal entries of the mergelist in this iteration and have also    // been the maximal entries in the previous iteration (the order in the set    // does not count). The stability count for a number n is the number of    // iterations for which the set of size n has been stable.    // The maximum stability count is the stability count for the size n, that    // is the maximum of all stability counts.    max = 0;    maxpos = 0;    for(int i = 0; i < 20; i++) {      if(stability[i] >= max) {        max = stability[i];        maxpos = i;      }    }    // repeat until maximum stability count exceeds a threshold  } while(max < (int)myConfigFileHough.Get_AccumulatorMax()  && stability[myConfigFileHough.Get_MaxPlanes()] <   myConfigFileHough.Get_AccumulatorMax() * myConfigFileHough.Get_PlaneRatio()  );  if(stability[myConfigFileHough.Get_MaxPlanes()] >= myConfigFileHough.Get_AccumulatorMax() * myConfigFileHough.Get_PlaneRatio()) {    maxpos = myConfigFileHough.Get_MaxPlanes();  }  for(int i = 0; i <= maxpos; i++) {    double * n = acc->getMax(mergelist[i]);     deletePoints(n, n[3]);    delete[] n;  }}