From 177981717c059b4ec9dcaec546c55b183f921a2b Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Sun, 9 Mar 2014 17:04:51 +0100 Subject: [PATCH] speed up point cloud rendering this is mostly caching some computations and using proper loop iterations --- .../default_plugin/point_cloud2_display.cpp | 40 +++++++++++++++---- .../default_plugin/point_cloud_common.cpp | 28 ++++++++----- .../point_cloud_transformers.cpp | 38 ++++++++++-------- src/rviz/ogre_helpers/point_cloud.cpp | 5 ++- 4 files changed, 74 insertions(+), 37 deletions(-) diff --git a/src/rviz/default_plugin/point_cloud2_display.cpp b/src/rviz/default_plugin/point_cloud2_display.cpp index d749aca23d..f4990f897b 100644 --- a/src/rviz/default_plugin/point_cloud2_display.cpp +++ b/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(ptr + xoff); float y = *reinterpret_cast(ptr + yoff); float z = *reinterpret_cast(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; diff --git a/src/rviz/default_plugin/point_cloud_common.cpp b/src/rviz/default_plugin/point_cloud_common.cpp index 79767768ea..51d5eacad2 100644 --- a/src/rviz/default_plugin/point_cloud_common.cpp +++ b/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; } } diff --git a/src/rviz/default_plugin/point_cloud_transformers.cpp b/src/rviz/default_plugin/point_cloud_transformers.cpp index c7ad05f944..5b7550703d 100644 --- a/src/rviz/default_plugin/point_cloud_transformers.cpp +++ b/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(point + xoff); - float y = *reinterpret_cast(point + yoff); - float z = *reinterpret_cast(point + zoff); - - Ogre::Vector3 pos(x, y, z); - points_out[i].position = pos; + iter->position.x = *reinterpret_cast(point_x); + iter->position.y = *reinterpret_cast(point_y); + iter->position.z = *reinterpret_cast(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(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(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; diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index b4ada1a54b..0390121ef5 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/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( 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();