56 lines
1.5 KiB
C
56 lines
1.5 KiB
C
|
#ifndef __RANSAC_H__
|
||
|
|
#define __RANSAC_H__
|
||
|
|
|
||
|
|
#include "shape.h"
|
||
|
|
#include "slam6d/scan.h"
|
||
|
|
#include "shapes/ransac_Boctree.h"
|
||
|
|
|
||
|
|
// TODO implement some parameters to modify ransac (maybe in CollisionShape?)
|
||
|
|
template <class T>
|
||
|
|
void Ransac(CollisionShape<T> &shape, Scan *scan, vector<T*> *best_points = 0) {
|
||
|
|
int best_score = 0;
|
||
|
|
CollisionShape<T> *best = shape.copy();
|
||
|
|
|
||
|
|
// stores 3 sample points
|
||
|
|
vector<T *> ps;
|
||
|
|
// create octree from the points
|
||
|
|
DataXYZ xyz(scan->get("xyz reduced"));
|
||
|
|
RansacOctTree<T> *oct = new RansacOctTree<T>(PointerArray<double>(xyz).get(), xyz.size(), 50.0 );
|
||
|
|
|
||
|
|
cout << "start 5000 iterations" << endl;
|
||
|
|
for(int i = 0; i < 5000; i++) {
|
||
|
|
ps.clear();
|
||
|
|
// randomly select points from the octree
|
||
|
|
oct->DrawPoints(ps, shape.getNrPoints());
|
||
|
|
// compute shape parameters from points
|
||
|
|
if ( shape.hypothesize(ps) ) {
|
||
|
|
// count number of points on the shape
|
||
|
|
int r = oct->PointsOnShape(shape);
|
||
|
|
if (r > best_score) {
|
||
|
|
if (best) delete best;
|
||
|
|
// remember this best fitted shape
|
||
|
|
best_score = r;
|
||
|
|
best = shape.copy();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
cout << "5000 iterations done" << endl;
|
||
|
|
if (best_points) {
|
||
|
|
best_points->clear();
|
||
|
|
oct->PointsOnShape(*best, *best_points);
|
||
|
|
cout << "Nr points before refinement " << best_points->size() << endl;
|
||
|
|
best->refine(best_points);
|
||
|
|
best_points->clear();
|
||
|
|
oct->PointsOnShape(*best, *best_points);
|
||
|
|
cout << "Nr points after refinement " << best_points->size() << endl;
|
||
|
|
}
|
||
|
|
shape = *best;
|
||
|
|
delete best;
|
||
|
|
|
||
|
|
delete oct;
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|