change contact, distance primitive id to intptr_t; add fcl::OcTree API to get info from opaque ids

doc/Doxyfile.in

@@ -2037,7 +2037,7 @@ INCLUDE_FILE_PATTERNS =
 # recursively expanded use the := operator instead of the = operator.
 # This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
 
-PREDEFINED = FCL_DOXYGEN_CXX=1
+PREDEFINED = FCL_DOXYGEN_CXX=1 FCL_HAVE_OCTOMAP
 
 # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
 # tag can be used to specify a list of macro names that should be expanded. The

include/fcl/common/types.h

@@ -60,6 +60,8 @@ using int64 = std::int64_t;
 using uint64 = std::uint64_t;
 using int32 = std::int32_t;
 using uint32 = std::uint32_t;
+using intptr_t = std::intptr_t;
+using uintptr_t = std::uintptr_t;
 
 template <typename S>
 using Vector2 = Eigen::Matrix<S, 2, 1>;

include/fcl/geometry/octree/octree-inl.h

@@ -42,6 +42,8 @@
 
 #include "fcl/config.h"
 
+#include "fcl/geometry/shape/utility.h"
+
 #if FCL_HAVE_OCTOMAP
 
 namespace fcl
@@ -296,6 +298,84 @@ void computeChildBV(const AABB<S>& root_bv, unsigned int i, AABB<S>& child_bv)
  }
 }
 
+//==============================================================================
+template <typename S>
+const typename OcTree<S>::OcTreeNode* OcTree<S>::getNodeByQueryCellId(
+ intptr_t id,
+ const Vector3<S>& point,
+ AABB<S>* aabb,
+ octomap::OcTreeKey* key,
+ unsigned int* depth) const
+{
+ octomap::OcTree::leaf_bbx_iterator it;
+ if (!getOctomapIterator(id, point, &it))
+ {
+ return nullptr;
+ }
+ if (aabb != nullptr)
+ {
+ Vector3<S> center(it.getX(), it.getY(), it.getZ());
+ double half_size = it.getSize() / 2.0;
+ Vector3<S> half_extent(half_size, half_size, half_size);
+ aabb->min_ = center - half_extent;
+ aabb->max_ = center + half_extent;
+ }
+ if (key != nullptr)
+ *key = it.getKey();
+ if (depth != nullptr)
+ *depth = it.getDepth();
+ return &(*it);
+}
+
+//==============================================================================
+template <typename S>
+bool OcTree<S>::getOctomapIterator(
+ intptr_t id,
+ const Vector3<S>& point,
+ octomap::OcTree::leaf_bbx_iterator* out) const
+{
+ assert(out != nullptr);
+ // The octomap tree structure provides no way to find a node from its pointer
+ // short of an exhaustive search. This could take a long time on a large
+ // tree. Instead, require the user to supply the contact point or nearest
+ // point returned by the query that also returned the id. Use the point to
+ // create the bounds to search for the node pointer.
+ const octomap::OcTreeKey point_key = tree->coordToKey(
+ point[0], point[1], point[2]);
+ // Set the min and max keys used for the bbx to the point key plus or minus
+ // one (if not at the limits of the data type) so we are guaranteed to hit
+ // the correct cell even when the point is on a boundary and rounds to the
+ // wrong cell.
+ octomap::OcTreeKey min_key, max_key;
+ for (unsigned int i = 0; i < 3; ++i)
+ {
+ min_key[i] = (point_key[i] > std::numeric_limits<octomap::key_type>::min() ?
+ point_key[i] - 1 : point_key[i]);
+ max_key[i] = (point_key[i] < std::numeric_limits<octomap::key_type>::max() ?
+ point_key[i] + 1 : point_key[i]);
+ }
+ octomap::OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(
+ min_key, max_key);
+ const octomap::OcTree::leaf_bbx_iterator end = tree->end_leafs_bbx();
+ const OcTreeNode* const node = getRoot() + id;
+ // While it may appear like this loop could take forever, in reality it will
+ // only take a few iterations. Octomap iterators use a fixed end iterator
+ // (copied above), and we are guaranteed to get to the end iterator after
+ // incrementing past the bounds of the octomap::OcTree::leaf_bbx_iterator.
+ // Incrementing end will keep the iterator at end as well. This functionality
+ // of octomap iterators is tested extensively in the octomap package tests.
+ while (it != end)
+ {
+ if (node == &(*it))
+ {
+ *out = it;
+ return true;
+ }
+ ++it;
+ }
+ return false;
+}
+
 } // namespace fcl
 
 #endif

include/fcl/geometry/octree/octree.h

@@ -47,13 +47,19 @@
 
 #include <octomap/octomap.h>
 #include "fcl/math/bv/AABB.h"
+#include "fcl/geometry/shape/box.h"
 #include "fcl/narrowphase/collision_object.h"
 
 namespace fcl
 {
 
 /// @brief Octree is one type of collision geometry which can encode uncertainty
 /// information in the sensor data.
+///
+/// @note OcTree will only be declared if octomap was found when FCL was built.
+/// For any particular FCL install, FCL_HAVE_OCTOMAP will be set to 1 in
+/// fcl/config.h if and only if octomap was found. Doxygen documentation will
+/// be generated whether or not octomap was found.
 template <typename S>
 class FCL_EXPORT OcTree : public CollisionGeometry<S>
 {
@@ -131,6 +137,52 @@ class FCL_EXPORT OcTree : public CollisionGeometry<S>
 
  /// @brief return node type, it is an octree
  NODE_TYPE getNodeType() const;
+
+ /// @brief Access the node and corresponding info by query cell id
+ ///
+ /// The results of distance and collision queries include various pieces of
+ /// information about the objects involved in the query (in DistanceResult
+ /// and Contact, respectively). When an object in the query is an OcTree the
+ /// corresponding member (b1 or b2) contains an opaque identifier refered to
+ /// as the "query cell id" of the corresponding tree cell.
+ ///
+ /// This method returns the node pointer of the given query cell id.
+ /// In order to efficiently find the cell by its query cell id, the contact
+ /// point or nearest point in or on the cell must be given. This method also
+ /// provides optional access to properties of the OcTree cell associated
+ /// with the cell.
+ ///
+ /// If the given cell can not be found, nullptr is returned and no
+ /// information is stored in the output pointer locations. This might happen
+ /// if some other point not in or on the cell is provided or if given a stale
+ /// or invalid query cell id.
+ ///
+ /// @param[in] id The query cell id, as given in b1 or b2 in
+ /// DistanceResult or Contact.
+ /// @param[in] point A point in or on the given cell id, from DistanceResult
+ /// or Contact.
+ /// @param[out] aabb If valid, output pointer for the AABB of the cell.
+ /// @param[out] key If valid, output pointer for the octomap::OcTreeKey of
+ /// the cell.
+ /// @param[out] depth If valid, output pointer for the tree depth of the
+ /// cell.
+ /// @return The node pointer for the given query cell id, or nullptr if not
+ /// found.
+ const OcTreeNode* getNodeByQueryCellId(
+ intptr_t id,
+ const Vector3<S>& point,
+ AABB<S>* aabb = nullptr,
+ octomap::OcTreeKey* key = nullptr,
+ unsigned int* depth = nullptr) const;
+
+private:
+ // Get the leaf_bbx_iterator pointing to the given cell given a point on or
+ // in the cell and the query id.
+ // Returns true if the cell was found, and false otherwise.
+ bool getOctomapIterator(
+ intptr_t id,
+ const Vector3<S>& point,
+ octomap::OcTree::leaf_bbx_iterator* out) const;
 };
 
 using OcTreef = OcTree<float>;

include/fcl/narrowphase/contact.h

@@ -56,14 +56,16 @@ struct FCL_EXPORT Contact
  /// @brief contact primitive in object 1
  /// if object 1 is mesh or point cloud, it is the triangle or point id
  /// if object 1 is geometry shape, it is NONE (-1),
- /// if object 1 is octree, it is the id of the cell
- int b1;
+ /// if object 1 is octree, it is the query cell id (see
+ /// OcTree::getNodeByQueryCellId)
+ intptr_t b1;
 
  /// @brief contact primitive in object 2
  /// if object 2 is mesh or point cloud, it is the triangle or point id
  /// if object 2 is geometry shape, it is NONE (-1),
- /// if object 2 is octree, it is the id of the cell
- int b2;
+ /// if object 2 is octree, it is the query cell id (see
+ /// OcTree::getNodeByQueryCellId)
+ intptr_t b2;
 
  /// @brief contact normal, pointing from o1 to o2
  Vector3<S> normal;

include/fcl/narrowphase/distance_result.h

@@ -77,14 +77,16 @@ struct FCL_EXPORT DistanceResult
  /// @brief information about the nearest point in object 1
  /// if object 1 is mesh or point cloud, it is the triangle or point id
  /// if object 1 is geometry shape, it is NONE (-1),
- /// if object 1 is octree, it is the id of the cell
- int b1;
+ /// if object 1 is octree, it is the query cell id (see
+ /// OcTree::getNodeByQueryCellId)
+ intptr_t b1;
 
  /// @brief information about the nearest point in object 2
  /// if object 2 is mesh or point cloud, it is the triangle or point id
  /// if object 2 is geometry shape, it is NONE (-1),
- /// if object 2 is octree, it is the id of the cell
- int b2;
+ /// if object 2 is octree, it is the query cell id (see
+ /// OcTree::getNodeByQueryCellId)
+ intptr_t b2;
 
  /// @brief invalid contact primitive information
  static const int NONE = -1;

test/test_fcl_octomap_collision.cpp

@@ -61,7 +61,11 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive,
 /// @brief Octomap collision with an environment mesh with 3 * env_size objects, asserting that correct triangle ids
 /// are returned when performing collision tests
 template <typename S>
-void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution = 0.1);
+void octomap_collision_test_contact_primitive_id(
+ S env_scale,
+ std::size_t env_size,
+ std::size_t num_max_contacts,
+ double resolution = 0.1);
 
 template<typename BV>
 void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolution = 0.1);
@@ -109,19 +113,19 @@ GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh)
 }
 
 template <typename S>
-void test_octomap_collision_mesh_triangle_id()
+void test_octomap_collision_contact_primitive_id()
 {
 #ifdef NDEBUG
- octomap_collision_test_mesh_triangle_id<S>(1, 30, 100000);
+ octomap_collision_test_contact_primitive_id<S>(1, 30, 100000);
 #else
- octomap_collision_test_mesh_triangle_id<S>(1, 10, 10000, 1.0);
+ octomap_collision_test_contact_primitive_id<S>(1, 10, 10000, 1.0);
 #endif
 }
 
-GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh_triangle_id)
+GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_contact_primitive_id)
 {
-// test_octomap_collision_mesh_triangle_id<float>();
- test_octomap_collision_mesh_triangle_id<double>();
+// test_octomap_collision_contact_primitive_id<float>();
+ test_octomap_collision_contact_primitive_id<double>();
 }
 
 template <typename S>
@@ -329,12 +333,18 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive,
 }
 
 template <typename S>
-void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution)
+void octomap_collision_test_contact_primitive_id(
+ S env_scale,
+ std::size_t env_size,
+ std::size_t num_max_contacts,
+ double resolution)
 {
  std::vector<CollisionObject<S>*> env;
  test::generateEnvironmentsMesh(env, env_scale, env_size);
 
- OcTree<S>* tree = new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution)));
+ std::shared_ptr<const octomap::OcTree> octree(
+ test::generateOcTree(resolution));
+ OcTree<S>* tree = new OcTree<S>(octree);
  CollisionObject<S> tree_obj((std::shared_ptr<CollisionGeometry<S>>(tree)));
 
  std::vector<CollisionObject<S>*> boxes;
@@ -348,6 +358,31 @@ void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size,
  for(std::size_t index=0; index<cResult.numContacts(); ++index)
  {
  const Contact<S>& contact = cResult.getContact(index);
+
+ const fcl::OcTree<S>* contact_tree = static_cast<const fcl::OcTree<S>*>(
+ contact.o1);
+ fcl::AABB<S> aabb;
+ octomap::OcTreeKey key;
+ unsigned int depth;
+ auto get_node_rv = contact_tree->getNodeByQueryCellId(
+ contact.b1,
+ contact.pos,
+ &aabb,
+ &key,
+ &depth);
+ EXPECT_TRUE(get_node_rv != nullptr);
+ auto center_octomap_point = octree->keyToCoord(key);
+ double cell_size = octree->getNodeSize(depth);
+ for (unsigned i = 0; i < 3; ++i)
+ {
+ EXPECT_FLOAT_EQ(
+ aabb.min_[i], center_octomap_point(i) - cell_size / 2.0);
+ EXPECT_FLOAT_EQ(
+ aabb.max_[i], center_octomap_point(i) + cell_size / 2.0);
+ }
+ auto octree_node = octree->search(key, depth);
+ EXPECT_TRUE(octree_node == get_node_rv);
+
  const fcl::BVHModel<fcl::OBBRSS<S>>* surface = static_cast<const fcl::BVHModel<fcl::OBBRSS<S>>*> (contact.o2);
  EXPECT_TRUE(surface->num_tris > contact.b2);
  }