void run (const char* file_name, float voxel_size)
{
PointCloud<PointXYZ>::Ptr points_in (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr points_out (new PointCloud<PointXYZ> ());
// Get the points and normals from the input vtk file
if ( !vtk_to_pointcloud (file_name, *points_in) )
return;
// Build the octree with the desired resolution
ORROctree octree;
octree.build (*points_in, voxel_size);
// Now build the octree z-projection
ORROctreeZProjection zproj;
zproj.build (octree, 0.15f*voxel_size, 0.15f*voxel_size);
// The visualizer
PCLVisualizer viz;
show_octree(&octree, viz);
show_octree_zproj(&zproj, viz);
#ifdef _SHOW_POINTS_
// Get the point of every full octree leaf
octree.getFullLeafPoints (*points_out);
// Add the point clouds
viz.addPointCloud (points_in, "cloud in");
viz.addPointCloud (points_out, "cloud out");
// Change the appearance
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud out");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud out");
#endif
// Enter the main loop
while (!viz.wasStopped ())
{
//main loop of the visualizer
viz.spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
void updateViewer (ORROctree& octree, PCLVisualizer& viz, std::vector<ORROctree::Node*>::iterator leaf)
{
viz.removeAllShapes();
const float *b = (*leaf)->getBounds (), *center = (*leaf)->getData ()->getPoint ();
float radius = 0.1f*octree.getRoot ()->getRadius ();
// Add the main leaf as a cube
viz.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 0.0, 0.0, 1.0, "main cube");
// Get all full leaves intersecting a sphere with certain radius
std::list<ORROctree::Node*> intersected_leaves;
octree.getFullLeavesIntersectedBySphere(center, radius, intersected_leaves);
char cube_id[128];
int i = 0;
// Show the cubes
for ( std::list<ORROctree::Node*>::iterator it = intersected_leaves.begin () ; it != intersected_leaves.end () ; ++it )
{
sprintf(cube_id, "cube %i", ++i);
b = (*it)->getBounds ();
viz.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 1.0, 1.0, 0.0, cube_id);
}
// Get a random full leaf on the sphere defined by 'center' and 'radius'
ORROctree::Node *rand_leaf = octree.getRandomFullLeafOnSphere (center, radius);
if ( rand_leaf )
{
pcl::ModelCoefficients sphere_coeffs;
sphere_coeffs.values.resize (4);
sphere_coeffs.values[0] = rand_leaf->getCenter ()[0];
sphere_coeffs.values[1] = rand_leaf->getCenter ()[1];
sphere_coeffs.values[2] = rand_leaf->getCenter ()[2];
sphere_coeffs.values[3] = 0.5f*(b[1] - b[0]);
viz.addSphere (sphere_coeffs, "random_full_leaf");
}
}
void run (const char* file_name, float voxel_size)
{
PointCloud<PointXYZ>::Ptr points_in (new PointCloud<PointXYZ> ());
PointCloud<PointXYZ>::Ptr points_out (new PointCloud<PointXYZ> ());
PointCloud<Normal>::Ptr normals_in (new PointCloud<Normal> ());
PointCloud<Normal>::Ptr normals_out (new PointCloud<Normal> ());
// Get the points and normals from the input vtk file
#ifdef _SHOW_OCTREE_NORMALS_
if ( !vtk_to_pointcloud (file_name, *points_in, &(*normals_in)) )
return;
#else
if ( !vtk_to_pointcloud (file_name, *points_in, NULL) )
return;
#endif
// Build the octree with the desired resolution
ORROctree octree;
if ( normals_in->size () )
octree.build (*points_in, voxel_size, &*normals_in);
else
octree.build (*points_in, voxel_size);
// Get the first full leaf in the octree (arbitrary order)
std::vector<ORROctree::Node*>::iterator leaf = octree.getFullLeaves ().begin ();
// Get the average points in every full octree leaf
octree.getFullLeavesPoints (*points_out);
// Get the average normal at the points in each leaf
if ( normals_in->size () )
octree.getNormalsOfFullLeaves (*normals_out);
// The visualizer
PCLVisualizer viz;
// Register a keyboard callback
CallbackParameters params(octree, viz, leaf);
viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (¶ms));
// Add the point clouds
viz.addPointCloud (points_in, "cloud in");
viz.addPointCloud (points_out, "cloud out");
if ( normals_in->size () )
viz.addPointCloudNormals<PointXYZ,Normal> (points_out, normals_out, 1, 6.0f, "normals out");
// Change the appearance
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud out");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud out");
// Convert the octree to a VTK poly-data object
// show_octree(&octree, viz, true/*show full leaves only*/);
updateViewer (octree, viz, leaf);
// Enter the main loop
while (!viz.wasStopped ())
{
//main loop of the visualizer
viz.spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
void
pcl::recognition::ORROctreeZProjection::build (const ORROctree& input, float eps_front, float eps_back)
{
this->clear();
// Compute the bounding box of the full leaves
const vector<ORROctree::Node*>& full_leaves = input.getFullLeaves ();
std::array<float, 4> full_leaves_bounds;
if ( full_leaves.empty() )
return;
// The initialization run
full_leaves_bounds[0] = std::numeric_limits<float>::infinity();
full_leaves_bounds[1] = -std::numeric_limits<float>::infinity();
full_leaves_bounds[2] = std::numeric_limits<float>::infinity();
full_leaves_bounds[3] = -std::numeric_limits<float>::infinity();
for (const auto &leaf : full_leaves)
{
const auto bounds = leaf->getBounds ();
if ( bounds[0] < full_leaves_bounds[0] ) full_leaves_bounds[0] = bounds[0];
if ( bounds[1] > full_leaves_bounds[1] ) full_leaves_bounds[1] = bounds[1];
if ( bounds[2] < full_leaves_bounds[2] ) full_leaves_bounds[2] = bounds[2];
if ( bounds[3] > full_leaves_bounds[3] ) full_leaves_bounds[3] = bounds[3];
}
// Make some initializations
pixel_size_ = input.getVoxelSize();
inv_pixel_size_ = 1.0f/pixel_size_;
bounds_[0] = full_leaves_bounds[0]; bounds_[1] = full_leaves_bounds[1];
bounds_[2] = full_leaves_bounds[2]; bounds_[3] = full_leaves_bounds[3];
extent_x_ = full_leaves_bounds[1] - full_leaves_bounds[0];
extent_y_ = full_leaves_bounds[3] - full_leaves_bounds[2];
num_pixels_x_ = static_cast<int> (extent_x_/pixel_size_ + 0.5f); // we do not need to round, but it's safer due to numerical errors
num_pixels_y_ = static_cast<int> (extent_y_/pixel_size_ + 0.5f);
num_pixels_ = num_pixels_x_*num_pixels_y_;
// Allocate and initialize memory for the pixels and the sets
pixels_ = new Pixel**[num_pixels_x_];
sets_ = new Set**[num_pixels_x_];
for ( int i = 0 ; i < num_pixels_x_ ; ++i )
{
pixels_[i] = new Pixel*[num_pixels_y_];
sets_[i] = new Set*[num_pixels_y_];
for ( int j = 0 ; j < num_pixels_y_ ; ++j )
{
pixels_[i][j] = NULL;
sets_[i][j] = NULL;
}
}
int pixel_id = 0;
// Project the octree full leaves onto the xy-plane
for (const auto &full_leaf : full_leaves)
{
this->getPixelCoordinates (full_leaf->getCenter(), num_pixels_x_, num_pixels_y_);
// If there is no set/pixel and at this position -> create one
if ( sets_[num_pixels_x_][num_pixels_y_] == NULL )
{
pixels_[num_pixels_x_][num_pixels_y_] = new Pixel (pixel_id++);
sets_[num_pixels_x_][num_pixels_y_] = new Set (num_pixels_x_, num_pixels_y_);
full_pixels_.push_back (pixels_[num_pixels_x_][num_pixels_y_]);
full_sets_.push_back (sets_[num_pixels_x_][num_pixels_y_]);
}
// Insert the full octree leaf at the right position in the set
sets_[num_pixels_x_][num_pixels_y_]->insert (full_leaf);
}
// Now, at each occupied (i, j) position, get the longest connected component consisting of neighboring full leaves
for (const auto &full_set : full_sets_)
{
// Get the first node in the set
set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)>::iterator node = full_set->get_nodes ().begin ();
// Initialize
float best_min = (*node)->getBounds ()[4];
float best_max = (*node)->getBounds ()[5];
float cur_min = best_min;
float cur_max = best_max;
int id_z1 = (*node)->getData ()->get3dIdZ ();
int maxlen = 1;
int len = 1;
// Find the longest 1D "connected component" at the current (i, j) position
for ( ++node ; node != full_set->get_nodes ().end () ; ++node )
{
int id_z2 = (*node)->getData ()->get3dIdZ ();
cur_max = (*node)->getBounds()[5];
if ( id_z2 - id_z1 > 1 ) // This connected component is over
{
// Start a new connected component
cur_min = (*node)->getBounds ()[4];
len = 1;
}
else // This connected component is still ongoing
{
++len;
if ( len > maxlen )
{
// This connected component is the longest one
maxlen = len;
best_min = cur_min;
best_max = cur_max;
}
}
id_z1 = id_z2;
}
int i = full_set->get_x ();
int j = full_set->get_y ();
pixels_[i][j]->set_z1 (best_min - eps_front);
pixels_[i][j]->set_z2 (best_max + eps_back);
}
}
