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