Improve removeShape and removeAllShapes performance

[VictorLamoine]

Fixes #1322.

All the cases are quite hard to understand and I'm pretty sure this is not perfect but this at least makes removeShape and removeAllShapes much faster in many situations.

Results

c550f3a

removeAllShapes (5000 shapes) took 3650 ms
removeAllShapes (5000 shapes) took 3575 ms
removeAllShapes (5000 shapes) took 3644 ms

Master

Results can go up to 486 secs when setLookUpTableID is not activated and LUT displayed.

PR branch

• With setLookUpTableID line activated

  • LUT not enabled

  removeAllShapes (5000 shapes) took 3628 ms
  removeAllShapes (5000 shapes) took 3581 ms
  removeAllShapes (5000 shapes) took 3648 ms
  

  • LUT enabled

  removeAllShapes (5000 shapes) took 3570 ms
  removeAllShapes (5000 shapes) took 3572 ms
  removeAllShapes (5000 shapes) took 3663 ms
  

• Without setLookUpTableID

  • LUT not enabled

  removeAllShapes (5000 shapes) took 5865 ms
  removeAllShapes (5000 shapes) took 3650 ms
  removeAllShapes (5000 shapes) took 5927 ms
  

  • LUT enabled

  removeAllShapes (5000 shapes) took 3634 ms
  removeAllShapes (5000 shapes) took 3867 ms
  removeAllShapes (5000 shapes) took 3662 ms
  

Test code:

#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>

#define NUMBER_OF_SHAPES 5000
bool remove_all_shapes = false;

void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
                       void* nothing)
{
  if (event.getKeySym () == "a" && event.keyDown ())
    remove_all_shapes = true;
}

int
main (int argc,
      char * argv[])
{
  pcl::visualization::PCLVisualizer viewer;
  pcl::console::TicToc time;

  for (int i = 0; i < NUMBER_OF_SHAPES; ++i)
  {
    pcl::PointXYZ center;
    center.x = 1024 * rand () / (RAND_MAX + 1.0f);
    center.y = 1024 * rand () / (RAND_MAX + 1.0f);
    center.z = 1024 * rand () / (RAND_MAX + 1.0f);
    viewer.addSphere (center, 0.05, "sphere_" + boost::lexical_cast<std::string> (i));
  }

  // Add colored mesh
  pcl::PolygonMesh mesh;
  pcl::io::loadPolygonFilePLY ("/home/victor/mesh.ply", mesh);
  vtkSmartPointer<vtkPolyData> poly_data = vtkSmartPointer<vtkPolyData>::New ();
  pcl::io::mesh2vtk (mesh, poly_data);
  vtkSmartPointer<vtkFloatArray> mesh_distances = vtkSmartPointer<vtkFloatArray>::New ();
  for (unsigned int i = 0; i < mesh.cloud.height * mesh.cloud.width; ++i)
    mesh_distances->InsertTuple1 (i, i);
  poly_data->GetPointData ()->SetScalars (mesh_distances);
  viewer.addModelFromPolyData (poly_data, "mesh");
  //viewer.setLookUpTableID("mesh"); // TODO: Remove this line if using an old PCL version (setLookUpTableID doesn't exist)

  viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();
    // The user pressed "a" :
    if (remove_all_shapes)
    {
      // removeAllShapes
      time.tic ();
      viewer.removeAllShapes ();
      std::cout << "removeAllShapes (" << NUMBER_OF_SHAPES << " shapes) took " << time.toc () << " ms" << std::endl;
      remove_all_shapes = false;
    }
  }

  return 0;
}

[VictorLamoine]

@JanHackenberg confirms that it solved the problem for him:
http://www.pcl-users.org/removeShape-performance-tp4018703p4039354.html