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

其他 2018-05-10 05:20:28 阅读次数: 0

《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;
  }

}

猜你喜欢

转载自blog.csdn.net/sinat_24206709/article/details/78875424
48
今日推荐
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
报告：Django 仍然是 74% 开发者的首选
《2024 年一季度互联网投融资运行情况》研究报告
15 年前上了“FFmpeg 耻辱柱”，今天他还得谢谢咱——腾讯QQPlayer一雪前耻？
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
GCC 14.1 发布
周排行
curl的POST请求，封装方法
8.1.1. Integer Types
Java基础 Day05(个人复习整理)
Python - Django - 中间件 process_exception
小L的试卷
【Shell编程】 （函数）判断用户是否存在
python(css样式)
spring ant path 匹配原则 - 【笔记】
《JavaScript与JScript从入门到精通》(美)James.Jaworski.中译本.扫描版.pdf
Eclipse运行带参数的java程序
每日归档
更多
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)
2024-05-03(19)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022