Merge pull request #390 from jcarpent/topic/hppfcl3x

Extend octree API

.git-blame-ignore-revs

@@ -1 +1,2 @@
 0067c8aa66aac548601e2a3fd029aa264cc59f2a
+76b68f785df31b00e153290b45ec290a9c5f7963

.github/workflows/macos-linux-conda.yml

@@ -31,7 +31,7 @@ jobs:
         activate-environment: hpp-fcl
         auto-update-conda: true
         environment-file: .github/workflows/conda/conda-env.yml
-        python-version: 3.7
+        python-version: "3.10"
 
     - name: Install compilers on OSX
       if: contains(matrix.os, 'macos')

.pre-commit-config.yaml

@@ -2,7 +2,7 @@ ci:
     autoupdate_branch: 'devel'
 repos:
 -   repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v15.0.7
+    rev: v16.0.0
     hooks:
     -   id: clang-format
         args: ['--style={BasedOnStyle: Google, SortIncludes: false}']
@@ -11,6 +11,6 @@ repos:
     hooks:
     -   id: trailing-whitespace
 -   repo: https://github.com/psf/black
-    rev: 23.1.0
+    rev: 23.3.0
     hooks:
     -   id: black

CMakeLists.txt

@@ -96,7 +96,7 @@ CMAKE_DEPENDENT_OPTION(GENERATE_PYTHON_STUBS "Generate the Python stubs associat
 ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0")
 
 if(BUILD_PYTHON_INTERFACE)
-  FIND_PACKAGE(eigenpy 2.7.10 REQUIRED)
+  FIND_PACKAGE(eigenpy 2.9.2 REQUIRED)
 endif()
 
 # Required dependencies
@@ -223,7 +223,6 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/BVH/BVH_utility.h
   include/hpp/fcl/collision_object.h
   include/hpp/fcl/collision_utility.h
-  include/hpp/fcl/octree.h
   include/hpp/fcl/hfield.h
   include/hpp/fcl/fwd.hh
   include/hpp/fcl/mesh_loader/assimp.h
@@ -237,7 +236,6 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/internal/traversal_node_bvh_shape.h
   include/hpp/fcl/internal/traversal_node_bvhs.h
   include/hpp/fcl/internal/traversal_node_hfield_shape.h
-  include/hpp/fcl/internal/traversal_node_octree.h
   include/hpp/fcl/internal/traversal_node_setup.h
   include/hpp/fcl/internal/traversal_node_shapes.h
   include/hpp/fcl/internal/traversal_recurse.h
@@ -262,6 +260,14 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/timings.h
   )
 
+IF(HPP_FCL_HAS_OCTOMAP)
+  LIST(APPEND ${PROJECT_NAME}_HEADERS
+    include/hpp/fcl/octree.h
+    include/hpp/fcl/serialization/octree.h
+    include/hpp/fcl/internal/traversal_node_octree.h
+  )
+ENDIF(HPP_FCL_HAS_OCTOMAP)
+
 add_subdirectory(doc)
 add_subdirectory(src)
 if (BUILD_PYTHON_INTERFACE)

include/hpp/fcl/data_types.h

@@ -3,6 +3,7 @@
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
  *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2023, Inria
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -64,6 +65,7 @@ namespace hpp {
 namespace fcl {
 typedef double FCL_REAL;
 typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
+typedef Eigen::Matrix<FCL_REAL, 6, 1> Vec6f;
 typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf;
 typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
 typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> Matrixx3f;

include/hpp/fcl/octree.h

@@ -3,6 +3,7 @@
  *
  *  Copyright (c) 2011-2014, Willow Garage, Inc.
  *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  Copyright (c) 2022-2023, Inria
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -38,7 +39,7 @@
 #ifndef HPP_FCL_OCTREE_H
 #define HPP_FCL_OCTREE_H
 
-#include <boost/array.hpp>
+#include <algorithm>
 
 #include <octomap/octomap.h>
 #include <hpp/fcl/fwd.hh>
@@ -51,7 +52,7 @@ namespace fcl {
 /// @brief Octree is one type of collision geometry which can encode uncertainty
 /// information in the sensor data.
 class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
- private:
+ protected:
   shared_ptr<const octomap::OcTree> tree;
 
   FCL_REAL default_occupancy;
@@ -85,20 +86,46 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
     free_threshold = 0;
   }
 
+  ///  \brief Copy constructor
   OcTree(const OcTree& other)
       : CollisionGeometry(other),
         tree(other.tree),
         default_occupancy(other.default_occupancy),
         occupancy_threshold(other.occupancy_threshold),
         free_threshold(other.free_threshold) {}
 
+  /// \brief Clone *this into a new Octree
   OcTree* clone() const { return new OcTree(*this); }
 
+  /// \brief Returns the tree associated to the underlying octomap OcTree.
+  shared_ptr<const octomap::OcTree> getTree() const { return tree; }
+
   void exportAsObjFile(const std::string& filename) const;
 
   /// @brief compute the AABB for the octree in its local coordinate system
   void computeLocalAABB() {
-    aabb_local = getRootBV();
+    typedef Eigen::Matrix<float, 3, 1> Vec3float;
+    Vec3float max_extent, min_extent;
+
+    octomap::OcTree::iterator it =
+        tree->begin((unsigned char)tree->getTreeDepth());
+    octomap::OcTree::iterator end = tree->end();
+
+    if (it == end) return;
+
+    max_extent = min_extent = Eigen::Map<Vec3float>(&it.getCoordinate().x());
+    for (++it; it != end; ++it) {
+      Eigen::Map<Vec3float> pos(&it.getCoordinate().x());
+      max_extent = max_extent.array().max(pos.array());
+      min_extent = min_extent.array().min(pos.array());
+    }
+
+    // Account for the size of the boxes.
+    const FCL_REAL resolution = tree->getResolution();
+    max_extent.array() += float(resolution / 2.);
+    min_extent.array() -= float(resolution / 2.);
+
+    aabb_local = AABB(min_extent.cast<FCL_REAL>(), max_extent.cast<FCL_REAL>());
     aabb_center = aabb_local.center();
     aabb_radius = (aabb_local.min_ - aabb_center).norm();
   }
@@ -111,9 +138,15 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
     return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
   }
 
-  /// @brief Returns the depth of octree
+  /// @brief Returns the depth of the octree
   unsigned int getTreeDepth() const { return tree->getTreeDepth(); }
 
+  /// @brief Returns the size of the octree
+  unsigned long size() const { return tree->size(); }
+
+  /// @brief Returns the resolution of the octree
+  FCL_REAL getResolution() const { return tree->getResolution(); }
+
   /// @brief get the root node of the octree
   OcTreeNode* getRoot() const { return tree->getRoot(); }
 
@@ -137,29 +170,51 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
   /// @brief transform the octree into a bunch of boxes; uncertainty information
   /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the
   /// boxes whose occupied probability is higher enough).
-  std::vector<boost::array<FCL_REAL, 6> > toBoxes() const {
-    std::vector<boost::array<FCL_REAL, 6> > boxes;
+  std::vector<Vec6f> toBoxes() const {
+    std::vector<Vec6f> boxes;
     boxes.reserve(tree->size() / 2);
     for (octomap::OcTree::iterator
              it = tree->begin((unsigned char)tree->getTreeDepth()),
              end = tree->end();
          it != end; ++it) {
       // if(tree->isNodeOccupied(*it))
       if (isNodeOccupied(&*it)) {
-        FCL_REAL size = it.getSize();
         FCL_REAL x = it.getX();
         FCL_REAL y = it.getY();
         FCL_REAL z = it.getZ();
+        FCL_REAL size = it.getSize();
         FCL_REAL c = (*it).getOccupancy();
         FCL_REAL t = tree->getOccupancyThres();
 
-        boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
+        Vec6f box;
+        box << x, y, z, size, c, t;
         boxes.push_back(box);
       }
     }
     return boxes;
   }
 
+  /// \brief Returns a byte description of *this
+  std::vector<uint8_t> tobytes() const {
+    typedef Eigen::Matrix<float, 3, 1> Vec3float;
+    const size_t total_size = (tree->size() * sizeof(FCL_REAL) * 3) / 2;
+    std::vector<uint8_t> bytes;
+    bytes.reserve(total_size);
+
+    for (octomap::OcTree::iterator
+             it = tree->begin((unsigned char)tree->getTreeDepth()),
+             end = tree->end();
+         it != end; ++it) {
+      const Vec3f box_pos =
+          Eigen::Map<Vec3float>(&it.getCoordinate().x()).cast<FCL_REAL>();
+      if (isNodeOccupied(&*it))
+        std::copy(box_pos.data(), box_pos.data() + sizeof(FCL_REAL) * 3,
+                  std::back_inserter(bytes));
+    }
+
+    return bytes;
+  }
+
   /// @brief the threshold used to decide whether one node is occupied, this is
   /// NOT the octree occupied_thresold
   FCL_REAL getOccupancyThres() const { return occupancy_threshold; }
@@ -225,7 +280,7 @@ class HPP_FCL_DLLAPI OcTree : public CollisionGeometry {
     if (other_ptr == nullptr) return false;
     const OcTree& other = *other_ptr;
 
-    return tree.get() == other.tree.get() &&
+    return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) &&
            default_occupancy == other.default_occupancy &&
            occupancy_threshold == other.occupancy_threshold &&
            free_threshold == other.free_threshold;

include/hpp/fcl/serialization/octree.h

@@ -0,0 +1,93 @@
+//
+// Copyright (c) 2023 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_OCTREE_H
+#define HPP_FCL_SERIALIZATION_OCTREE_H
+
+#include <sstream>
+#include <iostream>
+
+#include <boost/serialization/string.hpp>
+
+#include "hpp/fcl/octree.h"
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost {
+namespace serialization {
+
+namespace internal {
+struct OcTreeAccessor : hpp::fcl::OcTree {
+  typedef hpp::fcl::OcTree Base;
+  using Base::default_occupancy;
+  using Base::free_threshold;
+  using Base::occupancy_threshold;
+  using Base::tree;
+};
+}  // namespace internal
+
+template <class Archive>
+void save(Archive &ar, const hpp::fcl::OcTree &octree,
+          const unsigned int /*version*/) {
+  typedef internal::OcTreeAccessor Accessor;
+  const Accessor &access = reinterpret_cast<const Accessor &>(octree);
+
+  ar << make_nvp("base", base_object<hpp::fcl::CollisionGeometry>(octree));
+  ar << make_nvp("default_occupancy", access.default_occupancy);
+  ar << make_nvp("occupancy_threshold", access.occupancy_threshold);
+  ar << make_nvp("free_threshold", access.free_threshold);
+
+  const double resolution = octree.getResolution();
+  ar << make_nvp("resolution", resolution);
+
+  std::ostringstream stream;
+  access.tree->write(stream);
+  //  std::cout << "stream:" << std::endl;
+  //  std::cout << stream.str() << std::endl;
+  //  stream.seekg(0, std::ios::end);
+  //  const int stream_size = stream.tellg();
+  //  ar << make_nvp("tree_size",stream_size);
+  const std::string stream_str = stream.str();
+  ar << make_nvp("tree_data", stream_str);
+}
+
+template <class Archive>
+void load(Archive &ar, hpp::fcl::OcTree &octree,
+          const unsigned int /*version*/) {
+  typedef internal::OcTreeAccessor Accessor;
+  Accessor &access = reinterpret_cast<Accessor &>(octree);
+
+  ar >> make_nvp("base", base_object<hpp::fcl::CollisionGeometry>(octree));
+  ar >> make_nvp("default_occupancy", access.default_occupancy);
+  ar >> make_nvp("occupancy_threshold", access.occupancy_threshold);
+  ar >> make_nvp("free_threshold", access.free_threshold);
+  double resolution;
+  ar >> make_nvp("resolution", resolution);
+
+  std::string stream_str;
+  ar >> make_nvp("tree_data", stream_str);
+  std::istringstream stream(stream_str);
+
+  //  std::shared_ptr<const octomap::OcTree> new_octree(new
+  //  octomap::OcTree(resolution));
+
+  octomap::AbstractOcTree *new_tree = octomap::AbstractOcTree::read(stream);
+  //  const_cast<octomap::OcTree &>(*new_octree).readBinaryData(stream);
+  access.tree = std::shared_ptr<const octomap::OcTree>(
+      dynamic_cast<octomap::OcTree *>(new_tree));
+  //  const_cast<octomap::OcTree &>(*access.tree) = new_octree.get();
+
+  //  const_cast<octomap::OcTree &>(*access.tree).clear();
+  //  const_cast<octomap::OcTree &>(*access.tree).readBinaryData(stream);
+}
+
+template <class Archive>
+void serialize(Archive &ar, hpp::fcl::OcTree &octree,
+               const unsigned int version) {
+  split_free(ar, octree, version);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+#endif  // ifndef HPP_FCL_SERIALIZATION_OCTREE_H

python/CMakeLists.txt

@@ -140,24 +140,24 @@ SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${LIBRARY_NAME}"
   )
 
-IF(UNIX AND NOT APPLE)
-  SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES INSTALL_RPATH "\$ORIGIN/../../..")
-ENDIF()
-
 IF(IS_ABSOLUTE ${PYTHON_SITELIB})
   SET(ABSOLUTE_PYTHON_SITELIB ${PYTHON_SITELIB})
 ELSE()
   SET(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB})
 ENDIF()
 SET(${LIBRARY_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${LIBRARY_NAME})
+IF(UNIX)
+  GET_RELATIVE_RPATH(${${LIBRARY_NAME}_INSTALL_DIR} ${LIBRARY_NAME}_INSTALL_RPATH)
+  SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES INSTALL_RPATH "${${LIBRARY_NAME}_INSTALL_RPATH}")
+ENDIF()
 
 INSTALL(TARGETS ${LIBRARY_NAME}
   DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR})
 
 # --- GENERATE STUBS
 IF(GENERATE_PYTHON_STUBS)
   LOAD_STUBGEN()
-  GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${LIBRARY_NAME} ${ABSOLUTE_PYTHON_SITELIB})
+  GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${LIBRARY_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${LIBRARY_NAME})
 ENDIF(GENERATE_PYTHON_STUBS)
 
 # --- INSTALL SCRIPTS

python/broadphase/broadphase_callbacks.hh

@@ -100,8 +100,8 @@ struct DistanceCallBackBaseWrapper : DistanceCallBackBase,
         .def("distance",
              bp::pure_virtual(
                  static_cast<bool (Self::*)(
-                     CollisionObject * o1, CollisionObject * o2,
-                     Eigen::Matrix<double, 1, 1> & dist)>(&Self::distance)),
+                     CollisionObject* o1, CollisionObject* o2,
+                     Eigen::Matrix<double, 1, 1>& dist)>(&Self::distance)),
              doxygen::member_func_doc(&Base::distance))
         .def("__call__", &Base::operator(),
              doxygen::member_func_doc(&Base::operator()));

python/math.cc

@@ -53,12 +53,12 @@ struct TriangleWrapper {
   static Triangle::index_type getitem(const Triangle& t, int i) {
     if (i >= 3 || i <= -3)
       PyErr_SetString(PyExc_IndexError, "Index out of range");
-    return t[(i % 3)];
+    return t[size_t(i % 3)];
   }
   static void setitem(Triangle& t, int i, Triangle::index_type v) {
     if (i >= 3 || i <= -3)
       PyErr_SetString(PyExc_IndexError, "Index out of range");
-    t[(i % 3)] = v;
+    t[size_t(i % 3)] = v;
   }
 };