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

[c-andy-martin]

The distance result brief information stored the brief information
in an int. However, some primatives can only be stored by pointer
(namely octree nodes), and int is not wide enough on 64-bit
platforms to safely store a pointer. So upgrade the brief
information to be intptr_t which is guaranteed to be wide enough.

---

This change is 

[SeanCurtis-TRI]

Reviewable status: 0 of 2 files reviewed, 1 unresolved discussion (waiting on @c-andy-martin)

a discussion (no related file):
Quick disclaimer: I haven't played with or really looked at the octree aspect of FCL. So, this conversation may simply end with some education for me.

The documentation in distance_result.h suggests that the b1 and b2 should be the "id of the cell" if an octree is part of the result. Implicitly that sounds like it should have octree covered. So, I have the following questions:

1. What is the relationship between the "cell" referred to in the doc and the "node" to which you refer?
2. Are there truly no ids for octree cells?
3. Can you point to the code where this field is populated for octrees?

---

[SeanCurtis-TRI]

Reviewable status: 0 of 2 files reviewed, 1 unresolved discussion (waiting on @c-andy-martin)

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Quick disclaimer: I haven't played with or really looked at the octree aspect of FCL. So, this conversation may simply end with some education for me.

The documentation in distance_result.h suggests that the b1 and b2 should be the "id of the cell" if an octree is part of the result. Implicitly that sounds like it should have octree covered. So, I have the following questions:

1. What is the relationship between the "cell" referred to in the doc and the "node" to which you refer?
2. Are there truly no ids for octree cells?
3. Can you point to the code where this field is populated for octrees?

At this call site It appears that the "id" of the node in question is the offset from the octree root to that node.

The same thing happens here and here

I presume from that that the tree is laid out in contiguous memory so the concept of a "node/cell id" seems meaningful. That makes it feel like passing pointers around is not the right thing.

---

[SeanCurtis-TRI]

Reviewable status: 0 of 2 files reviewed, 1 unresolved discussion (waiting on @c-andy-martin)

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

At this call site It appears that the "id" of the node in question is the offset from the octree root to that node.

The same thing happens here and here

I presume from that that the tree is laid out in contiguous memory so the concept of a "node/cell id" seems meaningful. That makes it feel like passing pointers around is not the right thing.

One last thought:

The fcl::OcTree class doesn't seem to have an API that goes from the "id" to the node. The best you can do is OcTreeNode* node = octree.getRoot() + result.b2; I don't see that documented anywhere or, better yet, an API that does that voodoo for you (cross documented in the distance_result.h and contact.h.

In that same vein, when the corresponding object is mesh or point cloud, it is supposed to be the triangle or point id. And the BVHModel also doesn't have an API for using that id. BUt it does expose the underlying data structures so the ith triangle can be extracted directly.

---

[c-andy-martin]

Previously, SeanCurtis-TRI (Sean Curtis) wrote…
At this call site It appears that the "id" of the node in question is the offset from the octree root to that node.

The same thing happens here and here

I presume from that that the tree is laid out in contiguous memory so the concept of a "node/cell id" seems meaningful. That makes it feel like passing pointers around is not the right thing.

No, octomaps are not laid out in contiguous memory. The nodes are allocated with new. Therefore, you can not take the id and turn it back into the node pointer using the root of the tree when it is cast to a 32-bit wide integer, as the upper bits of the address are lost and there is no guarantee that the node will be within a 32-bit distance of the tree's root node. I submitted this PR to fix that issue.

However, your comments bring up a good point that a valid node pointer does not directly give you the actual geometric box that was the closest, as the node pointer stores nothing more than the occupancy information of that cell, and the fcl::Octree does not expose any way of getting at the underlying octree (although the caller who created the octree could keep a pointer around to the queried tree). The only way you could figure out what box of space is being referred to from the id field would be to iterate over a bounding box of the tree, bounded by the returned distance, looking for the matching node pointer (inefficient).

I actually have a local change I run with which returns a shared pointer to the nearest primitives (and the corresponding transforms) but did not submit a PR for this because I only added support to return them for octomap/BVH distance queries. You can see that change here, and then I added returning that for octomap/BVH here. This way I can efficiently make use of the nearest box.

Yet, even though this change doesn't help much, it does correct the bug and does make it possible for the caller to find the correct octree cell, assuming they keep a pointer around to the tree they queried, and assuming they don't mind paying to iterate over the tree to find the correct cell. It does this at the cost of a small storage increase in the distance result and does also break the ABI for distance results, and I'm not sure what your policy is on that.

[SeanCurtis-TRI]

Thanks for the elaboration.

Whew...it's all kind of crazy.

I have to admit to a fair amount of surprise that the individual nodes of an octree would be so far apart in virtual memory. While I acknowledge there's nothing preventing that from happening, i would've thought the temporal locality of allocation would've prevented that. As I scanned the octree code, I found indications in the nodes that the memory for its children are managed by the tree. However, the copy constructor for the data node clearly shows it allocating its own children -- hard to see how the tree enters into that. So, based on that cursory perusal, I'm disinclined to assert anything about how OcTree manages its memory.

With that in mind, you are right. If a node is more than 2 Gb away from the root node in address space, the pointer arithmetic becomes garbage.

I'd propose we take this a step further to repair the underlying defect. In addition to promoting the type to be sufficient for a 64-bit build we should do the following:

• Do the same thing in contact.h - it suffers from the same problem.
• Repair the documentation so "id of the cell" becomes more meaningful. Someone shouldn't have to slog through the code to figure out what is implied there. If there were a single place it could be documented and referenced both in contact.h as well as distance_result.h that'd be nice. :)
  • Some guidance that this can be used to get the OcTreeNode* based on tree.getRoot() + b1.
  • Further guidance that it doesn't provide the tree node's geometry (i.e., the position/dimensions of the "cell").

Reviewable status: 0 of 2 files reviewed, 2 unresolved discussions (waiting on @c-andy-martin)

a discussion (no related file):
Incidentally -- it's a shame that OcTree doesn't have upward pointers. If you knew the parent of a node (and which child the node is), then your "search" for the box for the node would be a simple log(N) search -- up the ancestry to the root where you get the full tree box, and then pop back down the path, splitting the BV as you go.

Since you can't do that, I believe the next best option would be start at the root and use the DistanceResult::nearest_point to simply walk down the tree directly (akin to the OcTreeBaseImpl::Search does; it returns a node, you'd want to return the box of the node). I.e., exploit the geometry of the OcTree rather than treating it like a graph search -- but it's still kind of crappy that it's not part of the public API.

---

[c-andy-martin]

OK. I'll code that up. I'm thinking tho instead of documenting the particulars of the id of the cell to provide an API in fcl::Octree that takes the ID and returns the cell information in various forms. I'll add some test cases to cover it.

[SeanCurtis-TRI]

Two thumbs way up for giving fcl::OcTree an API that can actually make use of the putative API. That's certainly the most robust.

Reviewable status: 0 of 2 files reviewed, 2 unresolved discussions (waiting on @c-andy-martin)

[c-andy-martin]

I have update w/ my implementation. Let me know if you'd like any changes, I'm especially wanting to be sure the public API names are acceptable.

[SeanCurtis-TRI]

Great stuff! I have some thoughts on the API and some style requests.

On the subject of style requests, there's a subjective component. We're pushing towards google styleguide in a very piecemeal manner. So, you've definitely mimicked the code that was here already, I'm inclined to push towards the new API reflecting the preferred style. This leads, of course, to inconsistent style in this file and if that bothers you, I'll withdraw the request -- either way, it leads us with some form of technical debt and I'm not convinced that one form is inherently superior to another.

Reviewed 1 of 2 files at r1, 5 of 5 files at r2.
Reviewable status: all files reviewed, 8 unresolved discussions (waiting on @c-andy-martin)

a discussion (no related file):
Also, could you update the title of this PR to reflect the evolution? It'll help anyone in the future who goes spelunking through the issues/PRs in the future.

---

---

include/fcl/geometry/octree/octree.h, line 136 at r2 (raw file):

  NODE_TYPE getNodeType() const;

  /// @brief Access a node from an opaque cell id

BTW I like the idea of "opaque cell id" as communicating a magic number. I wonder if it wouldn't be more beneficial to call it something that further reflects its expected provenance: query cell id? The id of the cell as reported by the distance and collision queries?

---

include/fcl/geometry/octree/octree.h, line 141 at r2 (raw file):

  /// The id of a cell is the opaque id returned by collision and
  /// distance queries. The node pointer can be used to retrieve any
  /// information stored in the cell, such as occupancy information.

nit: This would definitely benefit from a doxygen group:

/// @name Accessing Cells Using an Opaque 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 the "opaque id" of the corresponding tree cell.
///
/// These methods provide access to properties of the OcTree cell
/// associated with such an opaque id.
//@{

...  // the methods you have below...

//@}

This would allow us to discuss the concept of unique ids in one place that all of the methods would benefit from.

---

include/fcl/geometry/octree/octree.h, line 144 at r2 (raw file):

  const OcTreeNode* getNode(intptr_t id) const;

  /// @brief Return the BV of the given opaque cell id and point on cell

nit: Given this is very explicitly an AABB, let's have that reflected in the method name. (Especially since we typically use BV to represent a template parameter for arbitrary bounding volume types).

---

include/fcl/geometry/octree/octree.h, line 145 at r2 (raw file):

  /// @brief Return the BV of the given opaque cell id and point on cell
  /// @return true if the cell was found and aabb updated

nit: The return value here is ambiguous when reading through the implementation. This may simply be resolved by adding more documentation to the proposed doxygen group above. But, as I see it, in order to return true, the following must be true:

• The point must lie within the domain of the octree.
• The point must end up lying in a cell that either has the given opaque id or is "adjacent" to it (in some weird sense I haven't fully understood yet based on the adaptive nature of the tree and the tree keys).

It's difficult to see how that is captured by "if the cell was found". We'll need to elaborate more.

---

include/fcl/geometry/octree/octree.h, line 146 at r2 (raw file):

  /// @brief Return the BV of the given opaque cell id and point on cell
  /// @return true if the cell was found and aabb updated
  bool getNodeBV(intptr_t id, const Vector3<S>& point, AABB<S>& aabb) const;

nit: We're generally pushing FCL towards a more google style-guide like style. As such, could you change this to AABB<S>* aabb? The documentation would include a @pre that it be non null, and the implementation some kind of enforcement of that precondition (e.g., assert or test-and-throw.)

(Same for any other "output" parameters below.)

---

include/fcl/geometry/octree/octree.h, line 150 at r2 (raw file):

  /// @brief Return the box of the given opaque cell id and point on cell
  /// @return true if the cell was found and box and tf updated
  bool getNodeBox(intptr_t id, const Vector3<S>& point, Box<S>& box, Transform3<S>& box_tf) const;

BTW Do we have a proven need to return both an AABB and a Box? As it is, it's simply sugar for doing what you've done in the implementation: compute the AABB and use a utility to convert to box. Are both of these methods justified?

I ask because FCL has a fair amount of zombie code (which gets discovered and pruned intermittently) and I'd like to defer adding public API without proven need.

(Same applies for the GetNodeKeyAndDepth and getNode methods -- do they have concrete value?)

For the record, a simple, "Yes, I use them" is sufficient for me. I just don't want them to be purely speculative.

---

include/fcl/geometry/octree/octree.h, line 156 at r2 (raw file):

  bool getNodeKeyAndDepth(intptr_t id, const Vector3<S>& point, octomap::OcTreeKey& key, unsigned int& depth) const;

protected:

nit: protected seems highly suspicious. I don't believe this is sub-classed (nor designed to be). So, perhaps it would be better if this were simply private. And if private, replace /// documentation with // so as to not imply that it should appear in doxygen.

---

include/fcl/geometry/octree/octree-inl.h, line 359 at r2 (raw file):

  // point returned by the query that also returned the id. Use the point to
  // create the bounds to search for the node pointer.
  octomap::OcTreeKey point_key = tree->coordToKey(point[0], point[1], point[2]);

nit: make point_key, it, and end all const.

[c-andy-martin]

Reviewable status: 1 of 6 files reviewed, 6 unresolved discussions (waiting on @SeanCurtis-TRI)

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Also, could you update the title of this PR to reflect the evolution? It'll help anyone in the future who goes spelunking through the issues/PRs in the future.

Done.

---

---

include/fcl/geometry/octree/octree.h, line 136 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I like the idea of "opaque cell id" as communicating a magic number. I wonder if it wouldn't be more beneficial to call it something that further reflects its expected provenance: query cell id? The id of the cell as reported by the distance and collision queries?

Done

---

include/fcl/geometry/octree/octree.h, line 141 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This would definitely benefit from a doxygen group:

/// @name Accessing Cells Using an Opaque 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 the "opaque id" of the corresponding tree cell.
///
/// These methods provide access to properties of the OcTree cell
/// associated with such an opaque id.
//@{

...  // the methods you have below...

//@}

This would allow us to discuss the concept of unique ids in one place that all of the methods would benefit from.

I've rewritten the API around one method instead

---

include/fcl/geometry/octree/octree.h, line 144 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: Given this is very explicitly an AABB, let's have that reflected in the method name. (Especially since we typically use BV to represent a template parameter for arbitrary bounding volume types).

this method is now removed

---

include/fcl/geometry/octree/octree.h, line 145 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: The return value here is ambiguous when reading through the implementation. This may simply be resolved by adding more documentation to the proposed doxygen group above. But, as I see it, in order to return true, the following must be true:

• The point must lie within the domain of the octree.
• The point must end up lying in a cell that either has the given opaque id or is "adjacent" to it (in some weird sense I haven't fully understood yet based on the adaptive nature of the tree and the tree keys).

It's difficult to see how that is captured by "if the cell was found". We'll need to elaborate more.

I hope my update to getNode makes the intention plain. The method can fail if given an invalid id or given a point not on or in the cell that corresponds to the id. The reason we have to check up and down is that (without knowing anything about the box) we may be on any boundary of it. Not all the boundaries will round to the correct cell, it may round up or down depending on several factors. Therefore we must check every possible rounding. This is easy to do with the octomap bbx iterator as we can specify an AABB of space to quickly search which must contain our node (given correct inputs). The new getNode returns nullptr on failure and does not update any of the outputs.

---

include/fcl/geometry/octree/octree.h, line 146 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: We're generally pushing FCL towards a more google style-guide like style. As such, could you change this to AABB<S>* aabb? The documentation would include a @pre that it be non null, and the implementation some kind of enforcement of that precondition (e.g., assert or test-and-throw.)

(Same for any other "output" parameters below.)

Oh, excellent. I much prefer this direction. This frees me up to combine all the methods into one getNode by using pointers as optional outputs.

---

include/fcl/geometry/octree/octree.h, line 150 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Do we have a proven need to return both an AABB and a Box? As it is, it's simply sugar for doing what you've done in the implementation: compute the AABB and use a utility to convert to box. Are both of these methods justified?

I ask because FCL has a fair amount of zombie code (which gets discovered and pruned intermittently) and I'd like to defer adding public API without proven need.

(Same applies for the GetNodeKeyAndDepth and getNode methods -- do they have concrete value?)

For the record, a simple, "Yes, I use them" is sufficient for me. I just don't want them to be purely speculative.

This is sugar. I removed it.

---

include/fcl/geometry/octree/octree.h, line 156 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: protected seems highly suspicious. I don't believe this is sub-classed (nor designed to be). So, perhaps it would be better if this were simply private. And if private, replace /// documentation with // so as to not imply that it should appear in doxygen.

Done

---

include/fcl/geometry/octree/octree-inl.h, line 359 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: make point_key, it, and end all const.

Done, except for it as it is mutated in the loop so must be non-const.

[c-andy-martin]

I'm not sure whether reviewable.io left my responses visible to you or not, but I have replied to your comments using reviewable.io and force-pushed a new version which combines all the API's into one getNode, hopefully addressing all your comments.

[SeanCurtis-TRI]

We're in the home stretch. A couple of small cosmetic requests.

CI failure looks like a flake in AppVeyor -- I expect the next push will clear it.

Reviewed 5 of 5 files at r3.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @c-andy-martin)

---

include/fcl/geometry/octree/octree.h, line 155 at r3 (raw file):

  /// or invalid query cell id.
  ///
  /// @param id the query cell id, as given in b1 or b2 in a query result

nit: These capitalization and final periods.

---

include/fcl/geometry/octree/octree.h, line 161 at r3 (raw file):

  /// @param 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* getNode(

nit: It now is apparent to me that this name should be more specific: getNodeFromQueryid (or some such thing).

[c-andy-martin]

---

include/fcl/geometry/octree/octree.h, line 155 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: These capitalization and final periods.

Fixed

[c-andy-martin]

---

include/fcl/geometry/octree/octree.h, line 161 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: It now is apparent to me that this name should be more specific: getNodeFromQueryid (or some such thing).

I chose getNodeByQueryId and updated accordingly.

[c-andy-martin]

OK, I have addressed the comments. Also, in testing the Doxygen documentation to ensure my links from Contact and DistanceResult were working, I noticed that the OcTree documentation was not being generated at all. I have added another commit in the history which corrects this defect as well. (If you'd rather me open a separate PR for that I can).

[SeanCurtis-TRI]

See side discussion below on the whole doxygen question. That discussion aside, I really appreciate how thorough you've been in this whole process.

Reviewed 7 of 7 files at r4.
Reviewable status: all files reviewed, 3 unresolved discussions (waiting on @c-andy-martin)

a discussion (no related file):
I hadn't thought of the doxygen; that's a good call. It's spurred me to think about this and I'd like to have a quick back and forth to solicit your thoughts.

• Are we doing the right thing?
  • What I see it doing is:
    • If FCL is successfully configured to use octomap, then building doxygen in that build environment will include things like fcl::OcTree and reference to that API found in contact.h and distance_result.h will create live links.
    • If FCL is not configured to use octomap (for any reason), the documentation in contact.h and distance_result.h will still reference the class and method, but no longer be a live link.
  • Are we happy with that intermittently dead link? Presumably, we'd expect the de facto documentation to be compiled with all the bells and whistles, so that those links would always be valid.
    • I can imagine if someone builds doxygen locally, that references should be valid regardless of how they configure it.
      • That suggests that we would handle these build-dependent connections differently.

I can imagine an alternative approach:

• Doxygen should render all classes (regardless of what's included in the build).
• Those classes/functions/whatevers that are only conditionally included in the binary should have some doxygen-renderable tag that communicates that they are conditional (with a link to instructions that would satisfy the condition).

If we were to do this, we'd simply define the missing macro FCL_HAVE_OCTOMAP in the Doxyfile.in file and include an alias in Doxyfile.in like @octomap_conditional that would get pushed into the doxygen of the appropriate entities which expands to the appropriate magical text.

This is rendered somewhat muddy because of how FCL_HAVE_OCTOMAP is used. In some cases (e.g. octree.h), it simply introduces/erases whole entities. In other files (e.g., octree_distance_traversal_node.h), the entities still exist, but including the file leads to a compile-time error. This requires some thought to decide if it increases the complexity. (And there are third cases where even include octree.h is guarded by a test on FCL_HAVE_OCTOMAP which is a little mysterious.)

I can imagine three possible outcomes here:

1. Back the doxygen changes out of this PR and simply shrug.
2. Include the doxygen changes included in this revision.
3. Back the doxygen changes out and run up with a different solution.

Thoughts?

---

---

CMakeLists.txt, line 386 at r4 (raw file):

                     COMMAND ${CMAKE_COMMAND} -E echo_append "Building API Documentation..."
                     COMMAND ${DOXYGEN_EXECUTABLE} -u ${DOXYGEN_DOXYFILE}
                     COMMAND env DOXYGEN_INCLUDE_PATH=${DOXYGEN_INCLUDE_PATH} ${DOXYGEN_EXECUTABLE} ${DOXYGEN_DOXYFILE}

nit: I don't believe that this env prefix does anything. You've already configured the include path in the Doxyfile.in. (Amusing that it wasn't previously used.)

---

CMakeLists.txt, line 388 at r4 (raw file):

                     COMMAND env DOXYGEN_INCLUDE_PATH=${DOXYGEN_INCLUDE_PATH} ${DOXYGEN_EXECUTABLE} ${DOXYGEN_DOXYFILE}
                     COMMAND ${CMAKE_COMMAND} -E echo "Done."
                     COMMAND env

nit: This seems like a debugging artifact left over.

[c-andy-martin]

---

CMakeLists.txt, line 388 at r4 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This seems like a debugging artifact left over.

removed

[c-andy-martin]

---

CMakeLists.txt, line 386 at r4 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: I don't believe that this env prefix does anything. You've already configured the include path in the Doxyfile.in. (Amusing that it wasn't previously used.)

removed

[c-andy-martin]

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I hadn't thought of the doxygen; that's a good call. It's spurred me to think about this and I'd like to have a quick back and forth to solicit your thoughts.

• Are we doing the right thing?
  • What I see it doing is:
    • If FCL is successfully configured to use octomap, then building doxygen in that build environment will include things like fcl::OcTree and reference to that API found in contact.h and distance_result.h will create live links.
    • If FCL is not configured to use octomap (for any reason), the documentation in contact.h and distance_result.h will still reference the class and method, but no longer be a live link.
  • Are we happy with that intermittently dead link? Presumably, we'd expect the de facto documentation to be compiled with all the bells and whistles, so that those links would always be valid.
    • I can imagine if someone builds doxygen locally, that references should be valid regardless of how they configure it.
      • That suggests that we would handle these build-dependent connections differently.

I can imagine an alternative approach:

• Doxygen should render all classes (regardless of what's included in the build).
• Those classes/functions/whatevers that are only conditionally included in the binary should have some doxygen-renderable tag that communicates that they are conditional (with a link to instructions that would satisfy the condition).

If we were to do this, we'd simply define the missing macro FCL_HAVE_OCTOMAP in the Doxyfile.in file and include an alias in Doxyfile.in like @octomap_conditional that would get pushed into the doxygen of the appropriate entities which expands to the appropriate magical text.

This is rendered somewhat muddy because of how FCL_HAVE_OCTOMAP is used. In some cases (e.g. octree.h), it simply introduces/erases whole entities. In other files (e.g., octree_distance_traversal_node.h), the entities still exist, but including the file leads to a compile-time error. This requires some thought to decide if it increases the complexity. (And there are third cases where even include octree.h is guarded by a test on FCL_HAVE_OCTOMAP which is a little mysterious.)

I can imagine three possible outcomes here:

1. Back the doxygen changes out of this PR and simply shrug.
2. Include the doxygen changes included in this revision.
3. Back the doxygen changes out and run up with a different solution.

Thoughts?

I agree. So for FCL_HAVE_OCTOMAP, I think the best approach is to declare FCL_HAVE_OCTOMAP in Doxyfile.in. However, it didn't seem necessary for me to need to document the quirk that the OcTree class might not be delcared if octomap couldn't be found at compile time in more than one place, so I simply added a note to OcTree's documentation. I have rewritten the commit to be narrower and only deal w/ this issue for octomap (which seems relevant to address the missing documentation needed to document the new API method)

---

[SeanCurtis-TRI]

@c-andy-martin Thanks for your hard work on this. We don't use octree and so this has been a great opportunity for me to explore that corner of FCL I've previously largely ignored.

+@sherm1 for platform review.
+@SeanCurtis-TRI for feature review (I just realized I'd never actually been assigned.)

Reviewed 3 of 3 files at r5.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @sherm1)

[sherm1]

Checkpoint.

Reviewed 1 of 2 files at r1, 2 of 3 files at r5.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @c-andy-martin)

---

include/fcl/geometry/octree/octree.h, line 61 at r5 (raw file):

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

minor: consider mentioning that the doxygen comments here will be processed regardless of whether octomap was found.

---

include/fcl/geometry/octree/octree.h, line 164 at r5 (raw file):

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

nit: please keep line lengths to <= 80

[c-andy-martin]

Reviewable status: 6 of 7 files reviewed, 2 unresolved discussions (waiting on @SeanCurtis-TRI and @sherm1)

---

include/fcl/geometry/octree/octree.h, line 61 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: consider mentioning that the doxygen comments here will be processed regardless of whether octomap was found.

Fixed

---

include/fcl/geometry/octree/octree.h, line 164 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: please keep line lengths to <= 80

Fixed. I actually made them all <=79.

[SeanCurtis-TRI]

Reviewed 1 of 1 files at r6.
Reviewable status: complete! all files reviewed, all discussions resolved

[sherm1]

Thanks for doing this, Andy! It's really nice to see code with documentation and tests in FCL -- sets a good example for future programmers. Platform pending a few minor things. Please keep line lengths to <= 80 per the style guide we are trying to evolve towards (also makes it much easier to review).

Reviewed 4 of 7 files at r4.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @c-andy-martin)

---

include/fcl/geometry/octree/octree-inl.h, line 352 at r5 (raw file):

  {
    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];

nit: lines <= 80 please. Among other reasons we prefer that, it is hard to do a side-by-side review in Reviewable with long lines.
(Also in a few other places)

---

include/fcl/geometry/octree/octree-inl.h, line 364 at r5 (raw file):

      return true;
    }
    ++it;

minor: there is a potential infinite loop here if something goes wrong. Is it possible to set a reasonable maximum on the number of iterations, add a counter and assert it can't get bigger than the max?

[c-andy-martin]

No problem. I appreciate your thoroughness as maintainers! I have several more PR's I'd like to get merged over the next month or so, I'll keep them coming (there is already another in the pipe). I use FCL and octomap in costmap_3d, which is a ROS package I am writing that is an extension of the ROS costmap_2d to 3d.

Reviewable status: 3 of 7 files reviewed, 2 unresolved discussions (waiting on @SeanCurtis-TRI and @sherm1)

---

include/fcl/geometry/octree/octree-inl.h, line 352 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: lines <= 80 please. Among other reasons we prefer that, it is hard to do a side-by-side review in Reviewable with long lines.
(Also in a few other places)

Fixed, and in the other places I could find in the commits.

---

include/fcl/geometry/octree/octree-inl.h, line 364 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: there is a potential infinite loop here if something goes wrong. Is it possible to set a reasonable maximum on the number of iterations, add a counter and assert it can't get bigger than the max?

The only possible way for this loop to not terminate is if it never becomes end. Since it is unconditionally incremented, the only possible way is if there were a bug in the octomap iterator. The octomap iterator class has its own test cases in octomap. I do not think the added complexity is worth it.

[sherm1]

Just one little comment request then ready to merge.

Reviewed 1 of 1 files at r6, 4 of 4 files at r7.
Reviewable status: complete! all files reviewed, all discussions resolved

---

include/fcl/geometry/octree/octree-inl.h, line 364 at r5 (raw file):

Previously, c-andy-martin (C. Andy Martin) wrote…

The only possible way for this loop to not terminate is if it never becomes end. Since it is unconditionally incremented, the only possible way is if there were a bug in the octomap iterator. The octomap iterator class has its own test cases in octomap. I do not think the added complexity is worth it.

Would be great to have that argument as a comment here in the code. Otherwise the next reader has to puzzle over it like I did to try to get convinced that it can't loop forever. It is a little jarring without some reassurance.

[c-andy-martin]

---

include/fcl/geometry/octree/octree-inl.h, line 364 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Would be great to have that argument as a comment here in the code. Otherwise the next reader has to puzzle over it like I did to try to get convinced that it can't loop forever. It is a little jarring without some reassurance.

Done

[sherm1]

Thanks, @c-andy-martin !

Reviewed 1 of 1 files at r8.
Reviewable status: complete! all files reviewed, all discussions resolved