From 21476d7f36746fc5e22d7f914b909d9e570597ec Mon Sep 17 00:00:00 2001 From: "C. Andy Martin" Date: Fri, 20 Sep 2019 13:03:17 -0400 Subject: [PATCH 1/4] ogre helpers point_cloud: fix bounding radius bug The PointCloudRenderable returns an incorrect bounding radius when the furthest corner of the box is not the minimum or maximum corner. Use Ogre::Math::boundingRadiusFromAABB to return the correct value. --- 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 29b9777576..15e3585b82 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/src/rviz/ogre_helpers/point_cloud.cpp @@ -801,7 +801,7 @@ Ogre::HardwareVertexBufferSharedPtr PointCloudRenderable::getBuffer() Ogre::Real PointCloudRenderable::getBoundingRadius() const { - return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength())); + return Ogre::Math::boundingRadiusFromAABB(mBox); } Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const From 5455c2595d50b233d13739e12e73098e0ab74359 Mon Sep 17 00:00:00 2001 From: "C. Andy Martin" Date: Wed, 27 Nov 2019 07:10:50 -1000 Subject: [PATCH 2/4] ogre helpers point_cloud: fix bug in getSquaredViewDepth getSquaredViewDepth was not applying scene node transforms to the camera position, resulting in incorrect values of squared depth. Instead, find the distance in world coordinates from the bounding box to the camera. --- src/rviz/ogre_helpers/point_cloud.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index 15e3585b82..8dbe2112f1 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/src/rviz/ogre_helpers/point_cloud.cpp @@ -806,13 +806,7 @@ 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; - - return vDist.squaredLength(); + return getWorldBoundingBox().squaredDistance(cam->getDerivedPosition()); } void PointCloudRenderable::getWorldTransforms(Ogre::Matrix4* xform) const From cfb3f8134736604686453782158cfabe49892dbc Mon Sep 17 00:00:00 2001 From: "C. Andy Martin" Date: Wed, 4 Dec 2019 02:09:23 -1000 Subject: [PATCH 3/4] ogre helpers point_cloud: calculate bounding radius Instead of calculating the bounding radius of the entire cloud as we go, calculate it once at the end when updating the bounding box by calling Ogre::Math::boundingRadiusFromAABB on the bounding box. --- src/rviz/ogre_helpers/point_cloud.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index 8dbe2112f1..832aa47c13 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/src/rviz/ogre_helpers/point_cloud.cpp @@ -540,7 +540,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 +570,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_ = Ogre::Math::boundingRadiusFromAABB(bounding_box_); ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices()); @@ -620,13 +620,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_ = Ogre::Math::boundingRadiusFromAABB(bounding_box_); shrinkRenderables(); From c0315f313716c69e1fb98395c796b2fbb7d8bb7d Mon Sep 17 00:00:00 2001 From: "C. Andy Martin" Date: Wed, 4 Dec 2019 02:10:36 -1000 Subject: [PATCH 4/4] ogre helpers point_cloud: add size into bounds Add the size of the point visual into the bounding box. For small or zero sized points this makes little difference, but for large points (such as visualizing an octomap with large compressed regions), it is important to add the bounds to keep the points visible at the very edges of the cloud. --- src/rviz/ogre_helpers/point_cloud.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index 832aa47c13..1e1927de5b 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/src/rviz/ogre_helpers/point_cloud.cpp @@ -480,6 +480,7 @@ void PointCloud::addPoints(Point* points, uint32_t num_points) Ogre::AxisAlignedBox aabb; aabb.setNull(); + Ogre::Vector3 point_size_offset(width_/2.0, height_/2.0, depth_/2.0); uint32_t current_vertex_count = 0; bounding_radius_ = 0.0f; uint32_t vertex_size = 0; @@ -539,7 +540,8 @@ void PointCloud::addPoints(Point* points, uint32_t num_points) root->convertColourValue(p.color, &color); } - aabb.merge(p.position); + aabb.merge(p.position + point_size_offset); + aabb.merge(p.position - point_size_offset); float x = p.position.x; float y = p.position.y;