Skip to content

Fix misc point cloud ogre helper bounds issues #1644

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 4 commits into
base: noetic-devel
Choose a base branch
from
Open

Fix misc point cloud ogre helper bounds issues #1644

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
21476d7
ogre helpers point_cloud: fix bounding radius bug
Sep 20, 2019
5455c25
ogre helpers point_cloud: fix bug in getSquaredViewDepth
Nov 27, 2019
cfb3f81
ogre helpers point_cloud: calculate bounding radius
Dec 4, 2019
c0315f3
ogre helpers point_cloud: add size into bounds
Dec 4, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 7 additions & 12 deletions src/rviz/ogre_helpers/point_cloud.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -539,8 +540,8 @@ void PointCloud::addPoints(Point* points, uint32_t num_points)
root->convertColourValue(p.color, &color);
}

aabb.merge(p.position);
bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength());
aabb.merge(p.position + point_size_offset);
aabb.merge(p.position - point_size_offset);
Comment on lines +543 to +544
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

While I agree that the point size should be considered, I suggest doing so once in the end.
Also, you should adapt the bounding box(es) as soon as the point size changed via setDimensions().


float x = p.position.x;
float y = p.position.y;
Expand Down Expand Up @@ -571,6 +572,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_);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm afraid boundingRadiusFromAABB() is wrong as it computes the radius of the sphere around the origin that includes the bbox. However, here we need the radius of the local bounding sphere, don't we?

Only later they introduced boundingRadiusFromAABBCentered() to compute the latter.

ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <=
rend->getBuffer()->getNumVertices());

Expand Down Expand Up @@ -620,13 +622,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();

Expand Down Expand Up @@ -801,18 +802,12 @@ 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
{
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());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The previous code attempted to compute the (squared) distance of the cam to the center of the bbox.
However, the new code computes the closest (squared) distance of the cam to the bbox!

}

void PointCloudRenderable::getWorldTransforms(Ogre::Matrix4* xform) const
Expand Down