Skip to content
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

Merged

Conversation

SebastianRiedel
Copy link
Contributor

@SebastianRiedel SebastianRiedel commented Nov 28, 2019

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.

dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult<S>::NONE, closest_p1, closest_p2);
and
dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2);


This change is Reviewable

Copy link
Contributor

@SeanCurtis-TRI SeanCurtis-TRI left a 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).

Copy link
Contributor Author

@SebastianRiedel SebastianRiedel left a 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)

Copy link
Contributor

@SeanCurtis-TRI SeanCurtis-TRI left a 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)

@AlbertoMarnetto-Work
Copy link
Contributor

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 distanceResult.nearest_points array is filled, however I noticed that the order of the points does not always correspond to the order of the collision objects (i.e. sometimes nearest_points[0] belongs to o2 and nearest_points[1] to o1).

After further analysis it seems that nearest_points[0] belongs to the OcTree object and nearest_points[1] to the Mesh, independently of the order of such objects in the call to distance. However I did not yet test extensively enough to check if this is always the case (I can provide my sample test if needed).

Is this an intentional behavior, a bug or there is simply no guarantee about the order of distanceResult.nearest_points?

@jmirabel
Copy link

jmirabel commented Dec 6, 2019

however I noticed that the order of the points does not always correspond to the order of the collision objects (i.e. sometimes nearest_points[0] belongs to o2 and nearest_points[1] to o1).

This is a bug coming from

if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
for distance and from
if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
for collision.

When the test is true, the test that is done is o2, o1 and the result should be inverted. Applying something similar to humanoid-path-planner/hpp-fcl@177fb1f should solve the issue.

Copy link
Contributor

@SeanCurtis-TRI SeanCurtis-TRI left a 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)

@jmirabel
Copy link

jmirabel commented Dec 6, 2019

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?

You are right. For OcTree <-> BVH, the invertion of the result should happen here:

OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),

@AlbertoMarnetto-Work
Copy link
Contributor

@AlbertoMarnetto-Work 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.

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 ] ,
	}
}

Copy link
Contributor

@SeanCurtis-TRI SeanCurtis-TRI left a 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)

Copy link
Contributor

@SeanCurtis-TRI SeanCurtis-TRI left a 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)

@AlbertoMarnetto-Work
Copy link
Contributor

AlbertoMarnetto-Work commented Dec 6, 2019

@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 o2 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.

Yes, thanks for the suggestion, I had not thought to check the order of the collision objects in the DistanceResult.

Indeed o1 is always the OcTree and o2 the BVH, so that nearest_points[0] always corresponds to the returned o1 and nearest_points[1] always corresponds to o2.

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)
	}
}

@SeanCurtis-TRI
Copy link
Contributor

@AlbertoMarnetto-Work Glad to hear at least your problem is resolved. :)

@AlbertoMarnetto-Work
Copy link
Contributor

AlbertoMarnetto-Work commented Dec 9, 2019

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.

I have extended an existing test to check this patch. It can be cherry-picked into @SebastianRiedel 's branch or merged afterwards:

AlbertoMarnetto-Work@eb1e0b2

Copy link
Contributor Author

@SebastianRiedel SebastianRiedel left a 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.

  1. PASS: compares result of (manager of primitives <-> octomap) against (manager of primitives <-> manager of box primitives from octomap)
  2. FAIL: compares result of (manager of meshes <-> octomap) against (manager of meshes <-> manager of box meshes from octomap)
  3. PASS: compares result of (manager of meshes <-> octomap) against (manager of meshes <-> manager of box primitives from octomap)
  4. FAIL: compares result of (BVH<RSS> <-> octomap) against (BVH<RSS> <-> manager of box primitives from octomap)
  5. PASS: compares result of (BVH<OBBRSS> <-> octomap) against (BVH<OBBRSS> <-> manager of box primitives from octomap)
  6. 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)

@AlbertoMarnetto-Work
Copy link
Contributor

AlbertoMarnetto-Work commented Dec 9, 2019

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.

@AlbertoMarnetto-Work
Copy link
Contributor

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.

Copy link
Contributor

@SeanCurtis-TRI SeanCurtis-TRI left a 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)

Copy link
Contributor Author

@SebastianRiedel SebastianRiedel left a 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)

@AlbertoMarnetto
Copy link

The CI failed, but this seems to be unrelated with this patch (segfault in test_fcl_signed_distance, s. #407).

The test involved in this branch (test_fcl_octomap_distance) was instead successful in all builds.

Copy link
Contributor

@SeanCurtis-TRI SeanCurtis-TRI left a 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.

:LGTM:

Reviewed 1 of 1 files at r4.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @SebastianRiedel)

@SeanCurtis-TRI SeanCurtis-TRI merged commit 79b4d4c into flexible-collision-library:master Jan 14, 2020
@SeanCurtis-TRI
Copy link
Contributor

Merged despite erratic test failure in unrelated test.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants