C++ PCLVisualizer类代码示例

TEST_F(test_map, map_image_corners) {
    SKIP_IF_FAST

    vector<Point> corners;
    // select points close to corners, avoid NaNs
    int margin = 160;
    corners.push_back(Point(margin, margin));
    corners.push_back(Point(img.cols - margin, margin));
    corners.push_back(Point(img.cols - margin, img.rows - margin));
    corners.push_back(Point(margin, img.rows - margin));
    PointCloudT corners3d;
    mapToCloud(corners3d, corners, img, cloud); 
    EXPECT_EQ(corners.size(), corners3d.size());
    EXPECT_EQ(4, corners.size());
    EXPECT_EQ(4, corners3d.size());
    for (PointCloudT::iterator it = corners3d.begin(),
         end = corners3d.end(); it != end; it++) {
        cout << boost::format("%d: %8f,%8f,%8f") % (it - corners3d.begin()) % (*it).x % (*it).y % (*it).z << endl;
    }
    // show the whole stuff
    PCLVisualizer vis;
    addPose(vis, PoseRT());
    addMarkerPolygon3d(vis, corners3d);
    vis.addPointCloud(cloud);
    vis.spin();
}

int main()
{
  PolygonMesh human_1_mesh;
  loadPolygonFilePLY("../data/human_1.ply", human_1_mesh);
  PCLVisualizer* viewer = new PCLVisualizer("Mesh viewer");
  viewer->addPolygonMesh(human_1_mesh);
  while (!viewer->wasStopped()) { viewer->spinOnce(100); }
  viewer->close();
  delete viewer;
}

void
run (float pair_width, float voxel_size, float max_coplanarity_angle, int num_hypotheses_to_show)
{
  PointCloud<PointXYZ>::Ptr scene_points (new PointCloud<PointXYZ> ()), model_points (new PointCloud<PointXYZ> ());
  PointCloud<Normal>::Ptr scene_normals (new PointCloud<Normal> ()), model_normals (new PointCloud<Normal> ());

  // Get the points and normals from the scene
  if ( !vtk_to_pointcloud ("../../test/tum_table_scene.vtk", *scene_points, *scene_normals) )
    return;

  vtkPolyData *vtk_model = vtkPolyData::New ();
  // Get the points and normals from the scene
  if ( !vtk_to_pointcloud ("../../test/tum_amicelli_box.vtk", *model_points, *model_normals, vtk_model) )
    return;

  // The recognition object
  ObjRecRANSAC objrec (pair_width, voxel_size);
  objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle);
  objrec.addModel (*model_points, *model_normals, "amicelli", vtk_model);
  // Switch to the test mode in which only oriented point pairs from the scene are sampled
  objrec.enterTestModeTestHypotheses ();

  // The visualizer
  PCLVisualizer viz;

  CallbackParameters params(objrec, viz, *scene_points, *scene_normals, num_hypotheses_to_show);
  viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (&params));

  // Run the recognition and update the viewer
  update (&params);

#ifdef _SHOW_SCENE_POINTS_
  viz.addPointCloud (scene_points, "cloud in");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
#endif

#ifdef _SHOW_OCTREE_POINTS_
  PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ());
  objrec.getSceneOctree ().getFullLeavesPoints (*octree_points);
  viz.addPointCloud (octree_points, "octree points");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points");
  viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points");
#endif

#if defined _SHOW_OCTREE_NORMALS_ && defined _SHOW_OCTREE_POINTS_
  PointCloud<Normal>::Ptr octree_normals (new PointCloud<Normal> ());
  objrec.getSceneOctree ().getNormalsOfFullLeaves (*octree_normals);
  viz.addPointCloudNormals<PointXYZ,Normal> (octree_points, octree_normals, 1, 6.0f, "normals 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));
  }

  vtk_model->Delete ();
}

void show_octree (ORROctree* octree, PCLVisualizer& viz)
{
  vtkSmartPointer<vtkPolyData> vtk_octree = vtkSmartPointer<vtkPolyData>::New ();
  vtkSmartPointer<vtkAppendPolyData> append = vtkSmartPointer<vtkAppendPolyData>::New ();

  cout << "There are " << octree->getFullLeaves ().size () << " full leaves.\n";

  std::vector<ORROctree::Node*>& full_leaves = octree->getFullLeaves ();
  for (const auto &full_leaf : full_leaves)
    // Add it to the other cubes
    node_to_cube (full_leaf, append);

  // Save the result
  append->Update();
  vtk_octree->DeepCopy (append->GetOutput ());

  // Add to the visualizer
  vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
  vtkSmartPointer<vtkActor> octree_actor = vtkSmartPointer<vtkActor>::New();
  vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
  mapper->SetInputData (vtk_octree);
  octree_actor->SetMapper(mapper);

  // Set the appearance & add to the renderer
  octree_actor->GetProperty ()->SetColor (1.0, 1.0, 1.0);
  renderer->AddActor(octree_actor);
}

    void ObjectDetectionViewer::onUpdate(PCLVisualizer& visualizer) {
        Table::Collection tables;
        Object::Collection objects;
        {
            mutex::scoped_lock(resultMutex);
            tables = this->detectedTables;
            objects = this->detectedObjects;
        }
            
        visualizer.removeAllShapes();

        char strbuf[20];
        int tableId = 0;
        for (std::vector<Table::Ptr>::iterator table = tables->begin(); table != tables->end(); ++table) {
            sprintf(strbuf, "table%d", tableId++); 
            visualizer.addPolygon<Point>((*table)->getConvexHull(), 255, 0, 0, strbuf);
        }

        int objectId = 0;
        for (std::vector<Object::Ptr>::iterator object = objects->begin(); object != objects->end(); ++object) {
            sprintf(strbuf, "object%d", objectId++);
            visualizer.addPolygon<Point>((*object)->getBaseConvexHull(), 0, 255, 0, strbuf);
        }

        // TODO: print processing time in lower left corner (together with FPS)
    }

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

  // 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 show_octree_zproj (ORROctreeZProjection* zproj, PCLVisualizer& viz)
{
  cout << "There is (are) " << zproj->getFullPixels ().size () << " full pixel(s).\n";

  vtkSmartPointer<vtkAppendPolyData> upper_bound = vtkSmartPointer<vtkAppendPolyData>::New (), lower_bound = vtkSmartPointer<vtkAppendPolyData>::New ();
  const ORROctreeZProjection::Pixel *pixel;
  const float *b = zproj->getBounds ();
  float x, y, psize = zproj->getPixelSize ();
  int i, j, width, height;

  zproj->getNumberOfPixels (width, height);

  for ( i = 0, x = b[0] ; i < width ; ++i, x += psize )
  {
    for ( j = 0, y = b[2] ; j < height ; ++j, y += psize )
    {
      pixel = zproj->getPixel (i, j);

      if ( !pixel )
        continue;

      rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z1 (), lower_bound);
      rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z2 (), upper_bound);
    }
  }

  // Save the result
  upper_bound->Update();
  lower_bound->Update();

  // Add to the visualizer
  vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
  vtkSmartPointer<vtkActor> upper_actor = vtkSmartPointer<vtkActor>::New(), lower_actor = vtkSmartPointer<vtkActor>::New();
  vtkSmartPointer<vtkDataSetMapper> upper_mapper = vtkSmartPointer<vtkDataSetMapper>::New (), lower_mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
  upper_mapper->SetInputData (upper_bound->GetOutput ());
  upper_actor->SetMapper(upper_mapper);
  lower_mapper->SetInputData (lower_bound->GetOutput ());
  lower_actor->SetMapper(lower_mapper);

  // Set the appearance & add to the renderer
  upper_actor->GetProperty ()->SetColor (1.0, 0.0, 0.0);
  renderer->AddActor(upper_actor);
  lower_actor->GetProperty ()->SetColor (1.0, 1.0, 0.0);
  renderer->AddActor(lower_actor);
}

int main (int argc, char *argv[])
{

  if (argc < 3) {
    cout << "Enter the two files for registration ..\n";
    return -1;
  }

  pcl::console::setVerbosityLevel (pcl::console::L_DEBUG);

  string sourcefile = argv[1];
  string targetfile = argv[2];

  CloudPtr cloud1 ( new Cloud );
  CloudPtr cloud2 ( new Cloud );

  readPCDBinaryFile (sourcefile.c_str (), cloud1);
  readPCDBinaryFile (targetfile.c_str (), cloud2);


  //pcl::IterativeClosestPointNonLinear <Point, Point> icp;
  pcl::IterativeClosestPoint <Point, Point> icp;
  icp.setInputSource (cloud1);
  icp.setInputTarget (cloud2);
  icp.setMaximumIterations (2500);
  icp.setTransformationEpsilon (0.0000001);


  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
  //Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity ();
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  CloudPtr output (new Cloud);
  icp.align (*output, init_guess);

  //pcl::transformPointCloud (*cloud1, *output, icp.getFinalTransformation ());

  std::cout << "ICP NL has converged:" << icp.hasConverged ()
            << " score: " << icp.getFitnessScore () << std::endl;

  cout << "--------- Final transformation ---------------\n";
  cout << icp.getFinalTransformation () << "\n\n";


  CloudPtr cloud1_ig (new Cloud);
  pcl::transformPointCloud (*cloud1, *cloud1_ig, init_guess);

  PCLVisualizer* p = new PCLVisualizer (argc, argv, "Registration");
  int vp1 = 1;
  p->createViewPort (0.0, 0.0, 0.5, 1.0, vp1);
  int vp2 = 2;
  p->createViewPort (0.5, 0.0, 1.0, 1.0, vp2);
  p->setBackgroundColor (113.0/255, 113.0/255, 154.0/255);


  int color[3] = { 255, 0, 0};
  displayPointCloud (p, cloud1_ig, color, (char *) "opcloud1", vp1);

  color[0] = 0; color[1] = 255; color[2] = 0;
  displayPointCloud (p, cloud2, color, (char *) "opcloud2", vp1);

  color[0] = 0; color[1] = 255; color[2] = 0;
  displayPointCloud (p, output, color, (char *) "opcloud11", vp2);

  color[0] = 255; color[1] = 0; color[2] = 0;
  displayPointCloud (p, cloud2, color, (char *) "opcloud12", vp2);

  p->spin ();

  return 0;
}

void show_octree (ORROctree* octree, PCLVisualizer& viz, bool show_full_leaves_only)
{
    vtkSmartPointer<vtkPolyData> vtk_octree = vtkSmartPointer<vtkPolyData>::New ();
    vtkSmartPointer<vtkAppendPolyData> append = vtkSmartPointer<vtkAppendPolyData>::New ();

    cout << "There are " << octree->getFullLeaves ().size () << " full leaves.\n";

    if ( show_full_leaves_only )
    {
        std::vector<ORROctree::Node*>& full_leaves = octree->getFullLeaves ();
        for ( std::vector<ORROctree::Node*>::iterator it = full_leaves.begin () ; it != full_leaves.end () ; ++it )
            // Add it to the other cubes
            node_to_cube (*it, append);
    }
    else
    {
        ORROctree::Node* node;

        std::list<ORROctree::Node*> nodes;
        nodes.push_back (octree->getRoot ());

        while ( !nodes.empty () )
        {
            node = nodes.front ();
            nodes.pop_front ();

            // Visualize the node if it has children
            if ( node->getChildren () )
            {
                // Add it to the other cubes
                node_to_cube (node, append);
                // Add all the children to the working list
                for ( int i = 0 ; i < 8 ; ++i )
                    nodes.push_back (node->getChild (i));
            }
            // If we arrived at a leaf -> check if it's full and visualize it
            else if ( node->getData () )
                node_to_cube (node, append);
        }
    }

    // Just print the leaf size
    std::vector<ORROctree::Node*>::iterator first_leaf = octree->getFullLeaves ().begin ();
    if ( first_leaf != octree->getFullLeaves ().end () )
        printf("leaf size = %f\n", (*first_leaf)->getBounds ()[1] - (*first_leaf)->getBounds ()[0]);

    // Save the result
    append->Update();
    vtk_octree->DeepCopy (append->GetOutput ());

    // Add to the visualizer
    vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
    vtkSmartPointer<vtkActor> octree_actor = vtkSmartPointer<vtkActor>::New();
    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
    mapper->SetInput(vtk_octree);
    octree_actor->SetMapper(mapper);

    // Set the appearance & add to the renderer
    octree_actor->GetProperty ()->SetColor (1.0, 1.0, 1.0);
    octree_actor->GetProperty ()->SetLineWidth (1);
    octree_actor->GetProperty ()->SetRepresentationToWireframe ();
    renderer->AddActor(octree_actor);
}

void
visualize (const ModelLibrary::HashTable* hash_table)
{
  const ModelLibrary::HashTableCell* cells = hash_table->getVoxels ();
  size_t max_num_entries = 0;
  int id3[3], num_cells = hash_table->getNumberOfVoxels ();
  double cell_center[3];

  vtkPoints* sphere_centers = vtkPoints::New (VTK_DOUBLE);
  vtkDoubleArray* scale = vtkDoubleArray::New ();
  scale->SetNumberOfComponents(1);

  // Get the positions of the spheres (one sphere per full cell)
  for (int i = 0 ; i < num_cells ; ++i, ++cells)
  {
    // Does the cell have any entries?
    if (cells->size ())
    {
      // Insert the center of the current voxel in the point set
      hash_table->compute3dId (i, id3);
      hash_table->computeVoxelCenter (id3, cell_center);
      sphere_centers->InsertNextPoint (cell_center);

      // Save the number of entries
      scale->InsertNextValue (static_cast<double> (cells->size ()));

      // Get the max
      if (cells->size () > max_num_entries)
        max_num_entries = cells->size ();
    }
  }

  PCLVisualizer vis;
  vis.setBackgroundColor (0.1, 0.1, 0.1);

  // Is there something to visualize?
  if (max_num_entries)
  {
    // Compute the factor which maps all the scale values in (0, 1]
    double factor = 1.0/static_cast<double> (max_num_entries);
    // Set the true scale
    for (vtkIdType i = 0 ; i < scale->GetNumberOfTuples () ; ++i)
      scale->SetValue(i, factor*scale->GetValue (i));

    // Input for the glyph object: the centers + scale
    vtkPolyData *positions = vtkPolyData::New ();
    positions->SetPoints (sphere_centers);
    positions->GetPointData ()->SetScalars (scale);
    // The spheres
    vtkSphereSource* sphere_src = vtkSphereSource::New ();
    sphere_src->SetPhiResolution(8);
    sphere_src->SetThetaResolution(8);
    sphere_src->SetRadius(0.5*hash_table->getVoxelSpacing ()[0]);

    // Now that we have the points and the corresponding scalars, build the glyph object
    vtkGlyph3D *glyph = vtkGlyph3D::New ();
    glyph->SetScaleModeToScaleByScalar ();
    glyph->SetColorModeToColorByScalar ();
    glyph->SetInput (positions);
    glyph->SetSource (sphere_src->GetOutput ());
    glyph->Update ();

    vtkSmartPointer<vtkPolyData> glyph_output (glyph->GetOutput ());
    vis.addModelFromPolyData(glyph_output);

    // Cleanup
    glyph->Delete ();
    positions->Delete ();
    sphere_src->Delete ();
  }

  vis.spin();

  // Cleanup
  sphere_centers->Delete();
  scale->Delete();
}

int
main (int argc, char** argv)
{
  // Read command line arguments.
  for (char c; (c = getopt (argc, argv, "s:hc:r:")) != -1; ) 
  {
    switch (c) 
    {
      case 'c':
        coordinate_frame = RangeImage::CoordinateFrame (strtol (optarg, NULL, 0));
        break;
      case 'r':
      {
        angular_resolution = strtod (optarg, NULL);
        cout << PVARN(angular_resolution);
        break;
      }
      case 's':
      {
        source = strtol (optarg, NULL, 0);
        if (source < 0  ||  source > 2)
        {
          cout << "Source "<<source<<" is unknown.\n";
          exit (0);
        }
        cout << "Receiving "<<(source==0 ? "point clouds" : (source==1 ? "depth images" : "disparity images"))<<".\n";
        break;
      }
      case 'h':
      default:
        printUsage (argv[0]);
        exit (0);
    }
  }
  angular_resolution = deg2rad (angular_resolution);
  
  ros::init (argc, argv, "tutorial_range_image_live_viewer");
  ros::NodeHandle node_handle;
  ros::Subscriber subscriber, subscriber2;
  
  if (node_handle.resolveName("input")=="/input")
    std::cerr << "Did you forget input:=<your topic>?\n";
  
  if (source == 0)
    subscriber = node_handle.subscribe ("input", 1, cloud_msg_cb);
  else if (source == 1)
  {
    if (node_handle.resolveName("input2")=="/input2")
      std::cerr << "Did you forget input2:=<your camera_info_topic>?\n";
    subscriber = node_handle.subscribe ("input", 1, depth_image_msg_cb);
    subscriber2 = node_handle.subscribe ("input2", 1, camera_info_msg_cb);
  }
  else if (source == 2)
    subscriber  = node_handle.subscribe ("input",  1, disparity_image_msg_cb);
  
  PCLVisualizer viewer (argc, argv, "Live viewer - point cloud");
  RangeImageVisualizer range_image_widget("Live viewer - range image");
  
  RangeImagePlanar* range_image_planar;
  RangeImage* range_image;
  if (source==0)
    range_image = new RangeImage;
  else
  {
    range_image_planar = new RangeImagePlanar;
    range_image = range_image_planar;
  }
  
  while (node_handle.ok ())
  {
    usleep (10000);
    viewer.spinOnce (10);
    RangeImageVisualizer::spinOnce ();
    ros::spinOnce ();
    
    if (source==0)
    {
      // If no new message received: continue
      if (!cloud_msg || cloud_msg == old_cloud_msg)
        continue;
      old_cloud_msg = cloud_msg;
      
      Eigen::Affine3f sensor_pose(Eigen::Affine3f::Identity());
      PointCloud<PointWithViewpoint> far_ranges;
      RangeImage::extractFarRanges(*cloud_msg, far_ranges);
      if (pcl::getFieldIndex(*cloud_msg, "vp_x")>=0)
      {
        PointCloud<PointWithViewpoint> tmp_pc;
        fromROSMsg(*cloud_msg, tmp_pc);
        Eigen::Vector3f average_viewpoint = RangeImage::getAverageViewPoint(tmp_pc);
        sensor_pose = Eigen::Translation3f(average_viewpoint) * sensor_pose;
      }
      PointCloud<PointType> point_cloud;
      fromROSMsg(*cloud_msg, point_cloud);
      range_image->createFromPointCloud(point_cloud, angular_resolution, deg2rad(360.0f), deg2rad(180.0f),
                                        sensor_pose, coordinate_frame, noise_level, min_range, border_size);
    }
    else if (source==1)
    {
      // If no new message received: continue
//.........这里部分代码省略.........

 void PointCloudViewer::onInit(PCLVisualizer& visualizer) {
     // position viewport 3m behind kinect, but look around the point 2m in front of it
     visualizer.setCameraPosition(0., 0., -3., 0., 0., 2., 0., -1., 0.);
     visualizer.setCameraClipDistances(1.0, 10.0);
     visualizer.setBackgroundColor(0.3, 0.3, 0.8);
 }

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

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