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