void VoxelGrid<PointSourceType>::setInput(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud)
{
if (input_cloud->points.size() > 0) {
source_cloud_ = input_cloud;
findBoundaries();
initialize();
scatterPointsToVoxelGrid();
computeCentroidAndCovariance();
buildOctree();
}
}
void Octree::buildOctree(BoundingBox** Box)
{
//if we get below the threshold number of objects in a box, don't subdivide any longer
if ( (*Box)->Storage.size() <= maxObjectsPerBox )
return;
//(*Box)->childBoxes = new BoundingBox*[8];
for (int i=0; i < 8; i++)
{
(*Box)->childBoxes[i] = new BoundingBox;
}
int count = 0;
Vector displacement;
point boxCenter( ((*Box)->boxMinExtent.px + (*Box)->boxMaxExtent.px)/2.0,
((*Box)->boxMinExtent.py + (*Box)->boxMaxExtent.py)/2.0,
((*Box)->boxMinExtent.pz + (*Box)->boxMaxExtent.pz)/2.0 );
Vector diagonal = ((*Box)->boxMaxExtent - (*Box)->boxMinExtent) / 2.0;
//if not, subdivide box into eight equal octants
for (int z=0; z < 2; z++)
{
for (int y=0; y < 2; y++)
{
for (int x=0; x < 2; x++)
{
displacement.set(diagonal.px*(float)x, diagonal.py*(float)y, diagonal.pz*(float)z);
(*Box)->childBoxes[count++]->set((*Box)->boxMinExtent+displacement, boxCenter+displacement);
}
}
}
//now, see whether the children lie inside the child boxes
//this requires a triangle-box intersection and sphere-box intersection tests
for (int i=0; i < (*Box)->Storage.size(); i++)
{
for (int k=0; k < 8; k++)
{
if ( (*Box)->Storage[i]->getRadius() > 0.0 )
{
if ( boxSphereIntersection(*((*Box)->childBoxes[k]), *((*Box)->Storage[i])) )
{
//(*Box)->childBoxes[k]->numObjects++;
(*Box)->childBoxes[k]->Storage.push_back((*Box)->Storage[i]);
}
}
else
{
for (int j=0; j < (*Box)->Storage[i]->pNumTriangles; j++)
{
if ( boxTriangleIntersection(*((*Box)->childBoxes[k]), (*Box)->Storage[i]->Triangle_Array[j]) )
{
//add obj inc numObj, break
(*Box)->childBoxes[k]->Storage.push_back((*Box)->Storage[i]);
//(*Box)->childBoxes[k]->numObjects++;
break;
}
}
}
}
}
//now, recursively go to each of the children and see if they need to be subdivided
for (int i=0; i < 8; i++)
{
buildOctree(&((*Box)->childBoxes[i]));
}
}
void Octree::OctreeInit(BoundingBox** Box, Object* worldList, int worldNumObjects)
{
(*Box) = new BoundingBox;
//(*Box)->set(boxMinExtent, boxMaxExtent);
//(*Box)->Storage = new Object*[worldNumObjects];
(*Box)->Storage.resize(worldNumObjects);
for (int i=0; i < worldNumObjects; i++)
{
(*Box)->Storage[i] = &worldList[i];
}
//(*Box)->numObjects = worldNumObjects;
maxObjectsPerBox = 10;
//initialize the box bounds by going through the objects in the scene and finding the outer bounds
float xmin = 5000.0;
float xmax = -5000.0;
float ymin = 5000.0;
float ymax = -5000.0;
float zmin = 5000.0;
float zmax = -5000.0;
Vector sphereDispx(1.0,0.0,0.0);
Vector sphereDispy(0.0,1.0,0.0);
Vector sphereDispz(0.0,0.0,1.0);
for (int i=0; i < worldNumObjects; i++)
{
if ( worldList[i].getRadius() > 0.0 )
{
point xplus = (worldList[i].getCenter() + sphereDispx);
point xminus = xplus*-1.0;
point yplus = (worldList[i].getCenter() + sphereDispy);
point yminus = yplus*-1.0;
point zplus = (worldList[i].getCenter() + sphereDispz);
point zminus = zplus*-1.0;
if ( xplus.px > xmax )
xmax = xplus.px;
else if ( xminus.px < xmin )
xmin = xminus.px;
if ( yplus.py > ymax )
ymax = yplus.py;
else if ( yminus.py < ymin )
ymin = yminus.py;
if ( zplus.pz > zmax )
zmax = zplus.pz;
else if ( zminus.pz < zmin )
zmin = zminus.pz;
}
else
{
for (int j=0; j < worldList[i].pNumTriangles; j++)
{
for (int k=0; k < 3; k++)
{
point currVertex = worldList[i].Triangle_Array[j].Vertex[k];
if ( currVertex.px > xmax )
xmax = currVertex.px;
else if ( currVertex.px < xmin )
xmin = currVertex.px;
if ( currVertex.py > ymax )
ymax = currVertex.py;
else if ( currVertex.py < ymin )
ymin = currVertex.py;
if ( currVertex.pz > zmax )
zmax = currVertex.pz;
else if ( currVertex.pz < zmin )
zmin = currVertex.pz;
}
}
}
}
(*Box)->set(point(xmin,ymin,zmin), point(xmax,ymax,zmax));
//printf("min: %.2f %.2f %.2f\n max: %.2f %.2f %.2f\n", xmin,ymin,zmin,xmax,ymax,zmax);
buildOctree(Box);
}
bool OctreeGenerator::onStart() {
LOG(LTRACE) << "OctreeGenerator::onStart\n";
buildOctree();
out_octree.write(octree);
return true;
}
t_octreeNode *buildOctree(Map *map, size_t level,
size_t minX, size_t maxX,
size_t minY, size_t maxY,
size_t minZ, size_t maxZ,
t_octreeNode *parent)
{
t_octreeNode *node;
size_t i;
size_t medianX = minX + ((maxX - minX)/ 2);
size_t medianY = minY + ((maxY - minY)/ 2);
size_t medianZ = minZ + ((maxZ - minZ)/ 2);
if (level < map->subdivisions)
{
if (!(node = malloc(sizeof(t_octreeNode))))
return NULL;
if (!(node->children = malloc(sizeof(t_octreeNode *) * 8)))
return NULL;
for (i = 0; i < 8; i++)
node->children[i] = NULL;
node->subdivision.minX = minX;
node->subdivision.maxX = maxX;
node->subdivision.medianX = medianX;
node->subdivision.minY = minY;
node->subdivision.maxY = maxY;
node->subdivision.medianY = medianY;
node->subdivision.minZ = minZ;
node->subdivision.maxZ = maxZ;
node->subdivision.medianZ = medianZ;
node->level = level;
node->type = 0;
node->parent = node->parent;
/* if (level == 2) */
/* { */
/* Display subdivision with colors */
/* colorOctree(map, */
/* minX, maxX, medianX, */
/* minY, maxY, medianY, */
/* minZ, maxZ, medianZ); */
/* } */
if (needToSubdivise(map, node))
{
// Bottom, right, front
node->children[0] = buildOctree(map, level + 1,
minX, medianX,
minY, medianY,
minZ, medianZ, node);
/* // Bottom, right, back */
node->children[1] = buildOctree(map, level + 1,
minX, medianX,
minY, medianY,
medianZ, maxZ, node);
/* // Bottom, left, front */
node->children[2] = buildOctree(map, level + 1,
medianX, maxX,
minY, medianY,
minZ, medianZ, node);
/* // Bottom, left, back */
node->children[3] = buildOctree(map, level + 1,
medianX, maxX,
minY, medianY,
medianZ, maxZ, node);
/* // Top, left, back */
node->children[4] = buildOctree(map, level + 1,
medianX, maxX,
medianY, maxY,
medianZ, maxZ, node);
/* // Top, right, back */
node->children[5] = buildOctree(map, level + 1,
minX, medianX,
medianY, maxY,
medianZ, maxZ, node);
/* // Top, right, front */
node->children[6] = buildOctree(map, level + 1,
minX, medianX,
medianY, maxY,
minZ, medianZ, node);
/* // Top, left, front */
node->children[7] = buildOctree(map, level + 1,
medianX, maxX,
medianY, maxY,
minZ, medianZ, node);
for (i = 0; i < 8; i++)
{
if (node->children[i])
node->children[i]->childId = i;
}
}
else
node->type =
map->buffer[node->subdivision.minX][node->subdivision.minY][node->subdivision.minZ];
}
else
return NULL;
return node;
}
