-
Notifications
You must be signed in to change notification settings - Fork 417
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.
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
fix missing nearest points in Octree<->Mesh distance result #427
fix missing nearest points in Octree<->Mesh distance result #427
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Good catch.
The big request is could you add a test that fails without your fix and passes with it? As long as we're cleaning things up, it would be great if we could add some rigor to the code base.
Reviewed 1 of 1 files at r1.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h, line 509 at r1 (raw file):
solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2); dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id, closest_p1, closest_p2);
BTW I was a bit surprised that this wasn't preconditioned by a test against drequest->enable_nearest_points
. I would've expected it. However, as I look through the rest of this file, it doesn't appear to do that anywhere. The oct-tree-shape distance query computes and reports the witness points without testing the request configuration. So, it looks like the authors of this file forgot about conditioning that functionality. At the very least, this makes the file consistent (even if it obviously still needs work to be consistent with other parts of FCL).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't know if I can provide the test soon. I'll have a look at the weekend. If I see an easy-enough entry point in the existing tests, I will try.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks. We appreciate the effort. The tests can be somewhat opaque, to say the least. Hence the strong desire to improve them. :)
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
As I need this functionality in a project of mine, I would like to thank the author for finding and fixing this error. I shortly tried the fix with a test case of mine. After applying the patch the After further analysis it seems that Is this an intentional behavior, a bug or there is simply no guarantee about the order of |
This is a bug coming from fcl/include/fcl/narrowphase/distance-inl.h Line 117 in 6aeb7a1
fcl/include/fcl/narrowphase/collision-inl.h Line 123 in 6aeb7a1
When the test is true, the test that is done is o2, o1 and the result should be inverted. Applying something similar to coal-library/coal@177fb1f should solve the issue. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@jmirabel Quick question on your links.
This PR is about octree, but the code you linked to seems to be about geom vs bvh. There is an "object type" of type OT_OCTREE
, I would assume that shape-octtree queries would report types of OT_GEOM
and OT_OCTREE
. If so, this particular test wouldn't affect it. Is that not the case?
@AlbertoMarnetto-Work The semantics of the query results is poorly documented at best. An example of this is #384. I would be wholly unsurprised if this is one of those local quirks that was never tested against a global truth. Certainly, looking at the documentation in distance_request.h
, there is no reference to the objects in the query request. But, that might be because such a promise is impossible. If the query is expressed in terms of a bounding volume hierarchy that is colliding with itself, you have no guarantee if a given pair comes in as (a, b) or (b, a). Still, while may not be a reasonable expectation for which object is o1 and which is o2, there is a very reasonable expectation that the first nearest point should always correspond to o1 and the second with o2.
You don't happen to have a code snippet that can reproduce this error, do you? That'd go great in an issue.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
You are right. For OcTree <-> BVH, the invertion of the result should happen here:
|
Sure, here is a minimal self-contained example, which could hopefully be recycled in a unit test. #include <boost/format.hpp>
#include <fcl/math/bv/OBBRSS.h>
#include <fcl/math/triangle.h>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/narrowphase/collision.h>
#include <fcl/narrowphase/distance.h>
#include <Eigen/Core>
#include <vector>
std::string format_vector3(const Eigen::Vector3d& vector)
{
return str(boost::format("[ %5.2f , %5.2f , %5.2f ]")
% vector.x() % vector.y() % vector.z());
}
int main()
{
// Part 1: build a tetrahedron as BVHModel
std::vector<Eigen::Vector3d> vertices
= { { 0, 0, 0 }, { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
std::vector<fcl::Triangle> triangles
= { { 0, 1, 2 }, { 0, 1, 3 }, { 0, 2, 3 }, { 1, 2, 3 } };
typedef fcl::BVHModel<fcl::OBBRSSd> BvhGeometry;
auto bvhGeometry = std::make_shared<BvhGeometry>();
bvhGeometry->beginModel();
bvhGeometry->addSubModel(vertices, triangles);
bvhGeometry->endModel();
const fcl::CollisionObjectd bvhCollisionObject(bvhGeometry);
// Part 2: build a tetrahedron as octree
auto ocTree = std::make_shared<octomap::OcTree>(/* resolution = */ 0.01);
for (double x = 0.; x < 1.01; x += 0.05)
{
for (double y = 0.; y < 1.01; y += 0.05)
{
for (double z = 0.; z < 1.01; z += 0.05)
{
bool isOccupied = (x + y + z < 1);
ocTree->updateNode(octomap::point3d(x, y, z), isOccupied);
}
}
}
auto ocTreeGeometry = std::make_shared<fcl::OcTreed>(ocTree);
fcl::CollisionObjectd ocTreeCollisionObject(ocTreeGeometry);
const fcl::Transform3d xTranslationMatrix(
Eigen::Translation3d(5., 0., 0.));
ocTreeCollisionObject.setTransform(xTranslationMatrix);
// Part 3: perform a distance request where o1 = OcTree and o2 = BVHModel
const fcl::DistanceRequestd distanceRequest(
/* enable_nearest_points = */ true);
fcl::DistanceResultd distanceResult;
{
const double distance
= fcl::distance(&ocTreeCollisionObject, &bvhCollisionObject, distanceRequest, distanceResult);
std::cout << str(boost::format("distance : %5.2f , ")
% distance
);
std::cout << str(boost::format("p1 : %s , p2 : %s , ")
% format_vector3(distanceResult.nearest_points[0])
% format_vector3(distanceResult.nearest_points[1])
);
std::cout << "\n";
// Output:
// distance : 4.00 , p1 : [ 5.00 , 0.00 , 0.00 ] , p2 : [ 1.00 , 0.00 , 0.00 ] ,
}
// Part 4: perform a distance request swapping o1 and o2
// We could expect that the returned nearest points are also swapped,
// but this is not the case: they are returned in the same order
{
const double distance
= fcl::distance(&bvhCollisionObject, &ocTreeCollisionObject, distanceRequest, distanceResult);
std::cout << str(boost::format("distance : %5.2f , ")
% distance
);
std::cout << str(boost::format("p1 : %s , p2 : %s , ")
% format_vector3(distanceResult.nearest_points[0])
% format_vector3(distanceResult.nearest_points[1])
);
std::cout << "\n";
// Output:
// distance : 4.00 , p1 : [ 5.00 , 0.00 , 0.00 ] , p2 : [ 1.00 , 0.00 , 0.00 ] ,
}
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@jmirabel Sorry in advance for my pedantry. But I believe the relevant function would be:
void OcTreeSolver<NarrowPhaseSolver>::OcTreeMeshDistance( |
(this is about the distance query and not the collision query). However, what I find interesting is that there are two variants: the one indicated above and
void OcTreeSolver<NarrowPhaseSolver>::MeshOcTreeDistance( |
They are ostensibly mirror images of each other to support (Mesh, Octree) or (Octree, Mesh). However, things break down quickly. Both functions call a method called: OcTreeMeshDistanceRecurse
. But it reverses the semantics of the parameters. And I can only find one definition of that method. And unless BVHModel
and OcTree
are interchangeable, the second invocation of this method shouldn't even compile.
I haven't worked with fcl and octrees, and figuring out this particular issue is going to require more of an effort that I believe I can make right now.
I'm going to take @AlbertoMarnetto-Work's example and roll it into a new issue.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@AlbertoMarnetto-Work: As I look at your example, it's suggestive that the distance results are presenting the nearest points in a fixed order. Can you also confirm that the o1
and o1
are always presented in a fixed order (i.e., o1
is always the octree and o2
the bvh)? It seems to me that there is no real problem as long as the nearest points correctly correspond to the returned objects.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
Yes, thanks for the suggestion, I had not thought to check the order of the collision objects in the Indeed Taking this element into account all my issues are solved. Thanks for the very prompt feedback! I post here an amended example which checks the identity of the collision objects. #include <boost/format.hpp>
#include <fcl/math/bv/OBBRSS.h>
#include <fcl/math/triangle.h>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/narrowphase/collision.h>
#include <fcl/narrowphase/distance.h>
#include <Eigen/Core>
#include <vector>
std::string format_vector3(const Eigen::Vector3d& vector)
{
return str(boost::format("[ %5.2f , %5.2f , %5.2f ]")
% vector.x() % vector.y() % vector.z());
}
int main()
{
// Part 1: build a tetrahedron as BVHModel
std::vector<Eigen::Vector3d> vertices
= { { 0, 0, 0 }, { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
std::vector<fcl::Triangle> triangles
= { { 0, 1, 2 }, { 0, 1, 3 }, { 0, 2, 3 }, { 1, 2, 3 } };
typedef fcl::BVHModel<fcl::OBBRSSd> BvhGeometry;
auto bvhGeometry = std::make_shared<BvhGeometry>();
bvhGeometry->beginModel();
bvhGeometry->addSubModel(vertices, triangles);
bvhGeometry->endModel();
const fcl::CollisionObjectd bvhCollisionObject(bvhGeometry);
// Part 2: build a tetrahedron as octree
auto ocTree = std::make_shared<octomap::OcTree>(/* resolution = */ 0.01);
for (double x = 0.; x < 1.01; x += 0.05)
{
for (double y = 0.; y < 1.01; y += 0.05)
{
for (double z = 0.; z < 1.01; z += 0.05)
{
bool isOccupied = (x + y + z < 1);
ocTree->updateNode(octomap::point3d(x, y, z), isOccupied);
}
}
}
auto ocTreeGeometry = std::make_shared<fcl::OcTreed>(ocTree);
fcl::CollisionObjectd ocTreeCollisionObject(ocTreeGeometry);
const fcl::Transform3d xTranslationMatrix(
Eigen::Translation3d(5., 0., 0.));
ocTreeCollisionObject.setTransform(xTranslationMatrix);
// Part 3: perform a distance request where tree1 = OcTree and tree2 = BVHModel
const fcl::DistanceRequestd distanceRequest(
/* enable_nearest_points = */ true);
fcl::DistanceResultd distanceResult;
auto collisionObjectIdentity = [&](const fcl::CollisionGeometryd* o) -> std::string
{
if (o == &*bvhCollisionObject.collisionGeometry())
return "BVH";
else if (o == &*ocTreeCollisionObject.collisionGeometry())
return "OcTree";
else
return "Bad pointer";
};
{
const double distance
= fcl::distance(&ocTreeCollisionObject, &bvhCollisionObject, distanceRequest, distanceResult);
std::cout << str(boost::format("distance : %5.2f , ")
% distance
);
std::cout << str(boost::format("p1 : %s (in %s), p2 : %s (in %s), ")
% format_vector3(distanceResult.nearest_points[0])
% collisionObjectIdentity(distanceResult.o1)
% format_vector3(distanceResult.nearest_points[1])
% collisionObjectIdentity(distanceResult.o2)
);
std::cout << "\n";
// Output:
// distance : 4.00 , p1 : [ 5.00 , 0.00 , 0.00 ] (in OcTree), p2 : [ 1.00 , 0.00 , 0.00 ] (in BVH)
}
// Part 4: perform a distance request swapping tree1 and tree2
{
const double distance
= fcl::distance(&bvhCollisionObject, &ocTreeCollisionObject, distanceRequest, distanceResult);
std::cout << str(boost::format("distance : %5.2f , ")
% distance
);
std::cout << str(boost::format("p1 : %s (in %s), p2 : %s (in %s), ")
% format_vector3(distanceResult.nearest_points[0])
% collisionObjectIdentity(distanceResult.o1)
% format_vector3(distanceResult.nearest_points[1])
% collisionObjectIdentity(distanceResult.o2)
);
std::cout << "\n";
// Output (same as in part 3):
// distance : 4.00 , p1 : [ 5.00 , 0.00 , 0.00 ] (in OcTree), p2 : [ 1.00 , 0.00 , 0.00 ] (in BVH)
}
} |
@AlbertoMarnetto-Work Glad to hear at least your problem is resolved. :) |
I have extended an existing test to check this patch. It can be cherry-picked into @SebastianRiedel 's branch or merged afterwards: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Adding a comparison test for the nearest point result was sufficiently easy, however, not all tested cases seem to behave correct. There are 2 basic test functions, instantiated into 6 different configurations of which 4 pass and 2 fail for non-obvious reasons.
- PASS: compares result of (manager of primitives <-> octomap) against (manager of primitives <-> manager of box primitives from octomap)
- FAIL: compares result of (manager of meshes <-> octomap) against (manager of meshes <-> manager of box meshes from octomap)
- PASS: compares result of (manager of meshes <-> octomap) against (manager of meshes <-> manager of box primitives from octomap)
- FAIL: compares result of (BVH<RSS> <-> octomap) against (BVH<RSS> <-> manager of box primitives from octomap)
- PASS: compares result of (BVH<OBBRSS> <-> octomap) against (BVH<OBBRSS> <-> manager of box primitives from octomap)
- PASS: compares result of (BVH<kIOS> <-> octomap) against (BVH<kIOS> <-> manager of box primitives from octomap)
[----------] Global test environment tear-down
[==========] 6 tests from 1 test case ran. (21267 ms total)
[ PASSED ] 4 tests.
[ FAILED ] 2 tests, listed below:
[ FAILED ] FCL_OCTOMAP.test_octomap_distance_mesh
[ FAILED ] FCL_OCTOMAP.test_octomap_bvh_rss_d_distance_rss
Reviewable status: 1 of 2 files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
I think the failure is due to the ill-posedness of the problem. It suffices to change the geometry by a small amount (e.g. switching from the octomap to its "boxes" approximation) to make a different pair of points the nearest one. For this reason I would suggest a simpler check (s. my commit), which even if weaker suffices to prove that the fix is improving the codebase. |
Any news on this? I think that the combination @SebastianRiedel 's fix with my test (AlbertoMarnetto-Work@eb1e0b2) yields a fully working patch. The test is slightly weaker than Sebastian's one, but it passes and suffices to show that the calculation of the nearest points works. The branch is already available: master...AlbertoMarnetto-Work:fix-octomap-mesh-distance-test If Sebastian agrees I would open a PR for it. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If we can update this PR so that it's test is sufficient to show improvement and pass CI, I'd be content. (Or, if @AlbertoMarnetto-Work's branch includes the fix here plus a test, I'd be content for closing this PR in favor of that PR.) I'm flexible on the mechanism, just the goal of "fix + sufficient test" is my only request.
Reviewable status: 1 of 2 files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reverted the "failed" test attempt of mine and merged Alberto's branch (@AlbertoMarnetto-Work your commit is part of the history). Let's see what CI says. Thanks for following up.
Reviewable status: 1 of 2 files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
The CI failed, but this seems to be unrelated with this patch (segfault in The test involved in this branch ( |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I agree about the CI error.
Reviewed 1 of 1 files at r4.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)
Merged despite erratic test failure in unrelated test. |
I stumbled upon this one while testing distance queries between octrees and other entities. Seems like someone just forgot to add the (already computed) nearest points to the result object in the Octree<->Mesh case. A quick test suggests that the nearest points returned now are correct.
For comparison, the Octree<->[Shape, Octree] distance computation attaches the nearest points to the result, cf.
fcl/include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h
Line 297 in 6aeb7a1
fcl/include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h
Line 787 in 6aeb7a1
This change is