3dpcp/include/shapes/NumberRecOctree.h

101 lines
3.3 KiB
C
Raw Normal View History

2012-09-16 12:33:11 +00:00
#ifndef NRRANSAC_OCTREE_H
#define NRRANSAC_OCTREE_H
#include "shapes/geom_math.h"
#include "shapes/ransac_Boctree.h"
#include "slam6d/Boctree.h"
template <class T=double>
class NumberRecOctTree : public RansacOctTree<T> {
public:
// NumberRecOctTree(vector<T *> &pts, T voxelSize, PointType<T> _pointtype = PointType<T>() ) : RansacOctTree<T>(pts, voxelSize, _pointtype) {}
NumberRecOctTree(vector<const T *> &pts, T voxelSize, PointType _pointtype = PointType() ) : RansacOctTree<T>(pts, voxelSize, _pointtype) {}
long PointsOnNumber(double plane[4], double maxdist, double _center[3], double radius) {
setNumber(plane, _center, radius, maxdist);
return PointsOnNumber(*BOctTree<T>::root, BOctTree<T>::center, BOctTree<T>::size);
}
void PointsOnNumber(double plane[4], double maxdist, double _center[3], double radius, vector<T *> &n) {
setNumber(plane, _center, radius, maxdist);
PointsOnNumber(*BOctTree<T>::root, BOctTree<T>::center, BOctTree<T>::size, n);
}
protected:
long PointsOnNumber(bitoct &node, T *center, T size) {
if (!SphereInAABB(center[0], center[1], center[2], size)) {
return 0;
}
T ccenter[3];
bitunion<T> *children;
bitoct::getChildren(node, children);
long result = 0;
for (short i = 0; i < 8; i++) {
if ( ( 1 << i ) & node.valid ) { // if ith node exists
childcenter(center, ccenter, size, i); // childrens center
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
// check if leaf contains plane
if ( SphereInAABB(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
pointrep *points = children->points;
unsigned int length = points[0].length;
T *point = &(points[1].v); // first point
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if( closeToPlane(point) )
result++;
point += BOctTree<T>::POINTDIM;
}
}
} else { // recurse
result += PointsOnNumber( children->node, ccenter, size/2.0);
}
++children; // next child
}
}
return result;
}
void PointsOnNumber(bitoct &node, T *center, T size, vector<T*> &n) {
if (!SphereInAABB(center[0], center[1], center[2], size)) {
return ;
}
T ccenter[3];
bitunion<T> *children;
bitoct::getChildren(node, children);
for (short i = 0; i < 8; i++) {
if ( ( 1 << i ) & node.valid ) { // if ith node exists
childcenter(center, ccenter, size, i); // childrens center
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
// check if leaf contains plane
if ( SphereInAABB(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
pointrep *points = children->points;
unsigned int length = points[0].length;
T *point = &(points[1].v); // first point
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if( closeToPlane(point) )
n.push_back(point);
point+=BOctTree<T>::POINTDIM;
}
}
} else { // recurse
PointsOnNumber( children->node, ccenter, size/2.0, n);
}
++children; // next child
}
}
}
};
#endif