From a1a513057435b8543c71ceefae3959d70c571d04 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 21 Aug 2021 04:36:37 +0200 Subject: [PATCH 1/4] Fix linebreak in comment --- src/rviz/ogre_helpers/point_cloud.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/rviz/ogre_helpers/point_cloud.h b/src/rviz/ogre_helpers/point_cloud.h index 6ddefbd759..a93fe76d6d 100644 --- a/src/rviz/ogre_helpers/point_cloud.h +++ b/src/rviz/ogre_helpers/point_cloud.h @@ -96,8 +96,7 @@ typedef std::vector 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 From 3f400ce7b1d0705c7871f95c166198acc50fa97c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 20 Aug 2021 18:51:59 +0200 Subject: [PATCH 2/4] Fix bug in PointCloudRenderable::getSquaredViewDepth() getSquaredViewDepth() was not applying scene node transforms to the bounding box' center, resulting in incorrect squared distance to camera. --- src/rviz/ogre_helpers/point_cloud.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index 5861cdc170..ba5299171c 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/src/rviz/ogre_helpers/point_cloud.cpp @@ -818,12 +818,9 @@ Ogre::Real PointCloudRenderable::getBoundingRadius() const 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(); } From a885152699c8c11113c9b54225fef01ae493ef88 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 20 Aug 2021 18:55:36 +0200 Subject: [PATCH 3/4] Fix PointCloudRenderable::getBoundingRadius() Compute *local* bounding radius from bounding box size. --- src/rviz/ogre_helpers/point_cloud.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index ba5299171c..d1f38d2ab9 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/src/rviz/ogre_helpers/point_cloud.cpp @@ -813,7 +813,7 @@ 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 From 7ad787934c853708e924acff067fa850ecc7b803 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 20 Aug 2021 19:07:10 +0200 Subject: [PATCH 4/4] PointCloud: Fix calculation of bounding radius Bounding sphere should be local to bounding box, not consider max length of point vectors. If the cloud is shifted strongly into some direction, those lengths will be large, although the bounding box (and sphere) can be small. --- src/rviz/ogre_helpers/point_cloud.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index d1f38d2ab9..2025703cf2 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/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();