コード例 #1
0
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));
  }
}
コード例 #2
0
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");
    }
}
コード例 #3
0
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*> (&params));

    // 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));
    }
}
コード例 #4
0
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);
  }
}