speed up point cloud rendering

this is mostly caching some computations and using proper loop iterations

src/rviz/default_plugin/point_cloud2_display.cpp

@@ -111,21 +111,45 @@ void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr&
     return;
   }
 
-  uint32_t output_count = 0;
-  const uint8_t* ptr = &cloud->data.front();
-  for (size_t i = 0; i < point_count; ++i)
+  uint8_t* output_ptr = &filtered->data.front();
+  const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
+  size_t points_to_copy = 0;
+  for (; ptr < ptr_end; ptr += point_step)
   {
     float x = *reinterpret_cast<const float*>(ptr + xoff);
     float y = *reinterpret_cast<const float*>(ptr + yoff);
     float z = *reinterpret_cast<const float*>(ptr + zoff);
-    if (validateFloats(Ogre::Vector3(x, y, z)))
+    if (validateFloats(x) && validateFloats(y) && validateFloats(z))
     {
-      memcpy(&filtered->data.front() + (output_count * point_step), ptr, point_step);
-      ++output_count;
+      if (points_to_copy == 0)
+      {
+        // Only memorize where to start copying from
+        ptr_init = ptr;
+        points_to_copy = 1;
+      }
+      else
+      {
+        ++points_to_copy;
+      }
     }
-
-    ptr += point_step;
+    else
+    {
+      if (points_to_copy)
+      {
+        // Copy all the points that need to be copied
+        memcpy(output_ptr, ptr_init, point_step*points_to_copy);
+        output_ptr += point_step*points_to_copy;
+        points_to_copy = 0;
+      }
+    }
+  }
+  // Don't forget to flush what needs to be copied
+  if (points_to_copy)
+  {
+    memcpy(output_ptr, ptr_init, point_step*points_to_copy);
+    output_ptr += point_step*points_to_copy;
   }
+  uint32_t output_count = (output_ptr - &filtered->data.front()) / point_step;
 
   filtered->header = cloud->header;
   filtered->fields = cloud->fields;

src/rviz/default_plugin/point_cloud_common.cpp

@@ -820,10 +820,7 @@ bool PointCloudCommon::transformCloud(const CloudInfoPtr& cloud_info, bool updat
   cloud_points.clear();
 
   size_t size = cloud_info->message_->width * cloud_info->message_->height;
-  PointCloud::Point default_pt;
-  default_pt.color = Ogre::ColourValue(1, 1, 1);
-  default_pt.position = Ogre::Vector3::ZERO;
-  cloud_points.resize(size, default_pt);
+  cloud_points.resize(size);
 
   {
     boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
@@ -850,17 +847,26 @@ bool PointCloudCommon::transformCloud(const CloudInfoPtr& cloud_info, bool updat
       return false;
     }
 
-    xyz_trans->transform(cloud_info->message_, PointCloudTransformer::Support_XYZ, transform, cloud_points);
-    color_trans->transform(cloud_info->message_, PointCloudTransformer::Support_Color, transform, cloud_points);
+    bool res = xyz_trans->transform(cloud_info->message_, PointCloudTransformer::Support_XYZ, transform, cloud_points);
+    res &= color_trans->transform(cloud_info->message_, PointCloudTransformer::Support_Color, transform, cloud_points);
+    // In case there was a problem, assign a default value
+    if (!res)
+    {
+      PointCloud::Point default_pt;
+      default_pt.color = Ogre::ColourValue(1, 1, 1);
+      default_pt.position = Ogre::Vector3::ZERO;
+      cloud_points.clear();
+      cloud_points.resize(size, default_pt);
+    }
   }
 
-  for (size_t i = 0; i < size; ++i)
+  for (V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
   {
-    if (!validateFloats(cloud_points[i].position))
+    if (!validateFloats(cloud_point->position))
     {
-      cloud_points[i].position.x = 999999.0f;
-      cloud_points[i].position.y = 999999.0f;
-      cloud_points[i].position.z = 999999.0f;
+      cloud_point->position.x = 999999.0f;
+      cloud_point->position.y = 999999.0f;
+      cloud_point->position.z = 999999.0f;
     }
   }
 

src/rviz/default_plugin/point_cloud_transformers.cpp

@@ -304,15 +304,15 @@ bool XYZPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
   const uint32_t zoff = cloud->fields[zi].offset;
   const uint32_t point_step = cloud->point_step;
   const uint32_t num_points = cloud->width * cloud->height;
-  uint8_t const* point = &cloud->data.front();
-  for (uint32_t i = 0; i < num_points; ++i, point += point_step)
+  uint8_t const* point_x = &cloud->data.front() + xoff;
+  uint8_t const* point_y = &cloud->data.front() + yoff;
+  uint8_t const* point_z = &cloud->data.front() + zoff;
+  for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, point_x += point_step,
+    point_y += point_step, point_z += point_step)
   {
-    float x = *reinterpret_cast<const float*>(point + xoff);
-    float y = *reinterpret_cast<const float*>(point + yoff);
-    float z = *reinterpret_cast<const float*>(point + zoff);
-
-    Ogre::Vector3 pos(x, y, z);
-    points_out[i].position = pos;
+    iter->position.x = *reinterpret_cast<const float*>(point_x);
+    iter->position.y = *reinterpret_cast<const float*>(point_y);
+    iter->position.z = *reinterpret_cast<const float*>(point_z);
   }
 
   return true;
@@ -346,16 +346,22 @@ bool RGB8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
   int32_t index = findChannelIndex(cloud, "rgb");
 
   const uint32_t off = cloud->fields[index].offset;
+  uint8_t const* rgb_ptr = &cloud->data.front() + off;
   const uint32_t point_step = cloud->point_step;
-  const uint32_t num_points = cloud->width * cloud->height;
-  uint8_t const* point = &cloud->data.front();
-  for (uint32_t i = 0; i < num_points; ++i, point += point_step)
+
+  // Create a look-up table for colors
+  float rgb_lut[256];
+  for(int i = 0; i < 256; ++i)
   {
-    uint32_t rgb = *reinterpret_cast<const uint32_t*>(point + off);
-    float r = ((rgb >> 16) & 0xff) / 255.0f;
-    float g = ((rgb >> 8) & 0xff) / 255.0f;
-    float b = (rgb & 0xff) / 255.0f;
-    points_out[i].color = Ogre::ColourValue(r, g, b);
+    rgb_lut[i] = float(i)/255.0f;
+  }
+  for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end(); ++iter, rgb_ptr += point_step)
+  {
+    uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
+    iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
+    iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
+    iter->color.b = rgb_lut[rgb & 0xff];
+    iter->color.a = 1.0f;
   }
 
   return true;

src/rviz/ogre_helpers/point_cloud.cpp

@@ -537,6 +537,7 @@ void PointCloud::addPoints(Point* points, uint32_t num_points)
         ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices());
         vbuf->unlock();
         rend->setBoundingBox(aabb);
+        bounding_box_.merge(aabb);
       }
 
       buffer_size = std::min<int>( VERTEX_BUFFER_CAPACITY, (num_points - current_point)*vpp );
@@ -572,11 +573,10 @@ void PointCloud::addPoints(Point* points, uint32_t num_points)
     }
     else
     {
-      Ogre::Root::getSingletonPtr()->convertColourValue( p.color, &color );
+      root->convertColourValue( p.color, &color );
     }
 
     aabb.merge(p.position);
-    bounding_box_.merge( p.position );
     bounding_radius_ = std::max( bounding_radius_, p.position.squaredLength() );
 
     float x = p.position.x;
@@ -606,6 +606,7 @@ void PointCloud::addPoints(Point* points, uint32_t num_points)
 
   op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart;
   rend->setBoundingBox(aabb);
+  bounding_box_.merge(aabb);
   ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices());
 
   vbuf->unlock();