C++ PCLVisualizer::addCube方法代码示例

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
visualize (const ModelLibrary::HashTable& hash_table)
{
  PCLVisualizer vis;
  vis.setBackgroundColor (0.1, 0.1, 0.1);

  const ModelLibrary::HashTableCell* cells = hash_table.getVoxels ();
  size_t max_num_entries = 0;
  int i, id3[3], num_cells = hash_table.getNumberOfVoxels ();
  float half_side, b[6], cell_center[3], spacing = hash_table.getVoxelSpacing ()[0];
  char cube_id[128];

  // Just get the maximal number of entries in the cells
  for ( i = 0 ; i < num_cells ; ++i, ++cells )
  {
    if (cells->size ()) // That's the number of models in the cell (it's maximum one, since we loaded only one model)
    {
      size_t num_entries = (*cells->begin ()).second.size(); // That's the number of entries in the current cell for the model we loaded
      // Get the max number of entries
      if ( num_entries > max_num_entries )
        max_num_entries = num_entries;
    }
  }

  // Now, that we have the max. number of entries, we can compute the
  // right scale factor for the spheres
  float s = (0.5f*spacing)/static_cast<float> (max_num_entries);

  cout << "s = " << s << ", max_num_entries = " << max_num_entries << endl;

  // Now, render a sphere with the right radius at the right place
  for ( i = 0, cells = hash_table.getVoxels () ; i < num_cells ; ++i, ++cells )
  {
    // Does the cell have any entries?
    if (cells->size ())
    {
      hash_table.compute3dId (i, id3);
      hash_table.computeVoxelCenter (id3, cell_center);

      // That's half of the cube's side length
      half_side = s*static_cast<float> ((*cells->begin ()).second.size ());

      // Adjust the bounds of the cube
      b[0] = cell_center[0] - half_side; b[1] = cell_center[0] + half_side;
      b[2] = cell_center[1] - half_side; b[3] = cell_center[1] + half_side;
      b[4] = cell_center[2] - half_side; b[5] = cell_center[2] + half_side;

      // Set the id
      sprintf (cube_id, "cube %i", i);

      // Add to the visualizer
      vis.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 1.0, 1.0, 0.0, cube_id);
    }
  }

  vis.addCoordinateSystem(1.5, "global");
  vis.resetCamera ();

  // Enter the main loop
  while (!vis.wasStopped ())
  {
    vis.spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }
}