Fix point cloud bounds

src/rviz/ogre_helpers/point_cloud.cpp

@@ -481,7 +481,6 @@ void PointCloud::addPoints(Point* points, uint32_t num_points)
   Ogre::AxisAlignedBox aabb;
   aabb.setNull();
   uint32_t current_vertex_count = 0;
-  bounding_radius_ = 0.0f;
   uint32_t vertex_size = 0;
   uint32_t buffer_size = 0;
   for (uint32_t current_point = 0; current_point < num_points; ++current_point)
@@ -540,7 +539,6 @@ void PointCloud::addPoints(Point* points, uint32_t num_points)
     }
 
     aabb.merge(p.position);
-    bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength());
 
     float x = p.position.x;
     float y = p.position.y;
@@ -571,6 +569,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);
+  bounding_radius_ = bounding_box_.getHalfSize().length();
   ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <=
              rend->getBuffer()->getNumVertices());
 
@@ -620,13 +619,12 @@ void PointCloud::popPoints(uint32_t num_points)
 
   // reset bounds
   bounding_box_.setNull();
-  bounding_radius_ = 0.0f;
   for (uint32_t i = 0; i < point_count_; ++i)
   {
     Point& p = points_[i];
     bounding_box_.merge(p.position);
-    bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength());
   }
+  bounding_radius_ = bounding_box_.getHalfSize().length();
 
   shrinkRenderables();
 
@@ -813,17 +811,14 @@ void PointCloudRenderable::_notifyCurrentCamera(Ogre::Camera* camera)
 
 Ogre::Real PointCloudRenderable::getBoundingRadius() const
 {
-  return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength()));
+  return mBox.getHalfSize().length();
 }
 
 Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const
 {
-  Ogre::Vector3 vMin, vMax, vMid, vDist;
-  vMin = mBox.getMinimum();
-  vMax = mBox.getMaximum();
-  vMid = ((vMax - vMin) * 0.5) + vMin;
-  vDist = cam->getDerivedPosition() - vMid;
-
+  const Ogre::Vector3 vWorldCenter =
+      _getParentNodeFullTransform().transformAffine(getBoundingBox().getCenter());
+  const Ogre::Vector3 vDist = cam->getDerivedPosition() - vWorldCenter;
   return vDist.squaredLength();
 }
 

src/rviz/ogre_helpers/point_cloud.h

@@ -96,8 +96,7 @@ typedef std::vector<PointCloudRenderablePtr> V_PointCloudRenderable;
  * \brief A visual representation of a set of points.
  *
  * Displays a set of points using any number of Ogre BillboardSets.  PointCloud is optimized for sets of
- * points that change
- * rapidly, rather than for large clouds that never change.
+ * points that change rapidly, rather than for large clouds that never change.
  *
  * Most of the functions in PointCloud are not safe to call from any thread but the render thread.
  * Exceptions are clear() and addPoints(), which