Merge pull request #224 from jcarpent/devel

Add serialization for basic primitives

CMakeLists.txt

@@ -182,6 +182,8 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/serialization/collision_data.h
   include/hpp/fcl/serialization/collision_object.h
   include/hpp/fcl/serialization/eigen.h
+  include/hpp/fcl/serialization/geometric_shapes.h
+  include/hpp/fcl/serialization/memory.h
   include/hpp/fcl/serialization/OBB.h
   include/hpp/fcl/serialization/RSS.h
   include/hpp/fcl/serialization/OBBRSS.h

include/hpp/fcl/serialization/BVH_model.h

@@ -11,6 +11,7 @@
 #include "hpp/fcl/serialization/BV_node.h"
 #include "hpp/fcl/serialization/BV_splitter.h"
 #include "hpp/fcl/serialization/collision_object.h"
+#include "hpp/fcl/serialization/memory.h"
 
 namespace boost
 {
@@ -314,4 +315,22 @@ namespace boost
   }
 }
 
+namespace hpp {
+namespace fcl {
+
+namespace internal
+{
+  template<typename BV>
+  struct memory_footprint_evaluator< ::hpp::fcl::BVHModel<BV> >
+  {
+    static size_t run(const ::hpp::fcl::BVHModel<BV> & bvh_model)
+    {
+      return static_cast<size_t>(bvh_model.memUsage(false));
+    }
+  };
+}
+
+} // namespace fcl
+} // namespace hpp
+
 #endif // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H

include/hpp/fcl/serialization/geometric_shapes.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H
+#define HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H
+
+#include "hpp/fcl/shape/geometric_shapes.h"
+#include "hpp/fcl/serialization/fwd.h"
+
+namespace boost
+{
+  namespace serialization
+  {
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::ShapeBase & shape_base,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::CollisionGeometry>(shape_base));
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::TriangleP & triangle,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(triangle));
+      ar & make_nvp("a",triangle.a);
+      ar & make_nvp("b",triangle.b);
+      ar & make_nvp("c",triangle.c);
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::Box & box,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(box));
+      ar & make_nvp("halfSide",box.halfSide);
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::Sphere & sphere,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(sphere));
+      ar & make_nvp("radius",sphere.radius);
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::Capsule & capsule,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(capsule));
+      ar & make_nvp("radius",capsule.radius);
+      ar & make_nvp("halfLength",capsule.halfLength);
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::Cone & cone,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(cone));
+      ar & make_nvp("radius",cone.radius);
+      ar & make_nvp("halfLength",cone.halfLength);
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::Cylinder & cylinder,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(cylinder));
+      ar & make_nvp("radius",cylinder.radius);
+      ar & make_nvp("halfLength",cylinder.halfLength);
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::Halfspace & half_space,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(half_space));
+      ar & make_nvp("n",half_space.n);
+      ar & make_nvp("d",half_space.d);
+    }
+  
+    template <class Archive>
+    void serialize(Archive & ar,
+                   hpp::fcl::Plane & plane,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("base",boost::serialization::base_object<hpp::fcl::ShapeBase>(plane));
+      ar & make_nvp("n",plane.n);
+      ar & make_nvp("d",plane.d);
+    }
+
+  }
+}
+
+#endif // ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H

include/hpp/fcl/serialization/memory.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef HPP_FCL_SERIALIZATION_MEMORY_H
+#define HPP_FCL_SERIALIZATION_MEMORY_H
+
+namespace hpp {
+namespace fcl {
+
+namespace internal
+{
+  template<typename T>
+  struct memory_footprint_evaluator
+  {
+    static size_t run(const T &) { return sizeof(T); }
+  };
+}
+
+  /// \brief Returns the memory footpring of the input object.
+  /// For POD objects, this function returns the result of sizeof(T)
+  ///
+  /// \param[in] object whose memory footprint needs to be evaluated.
+  ///
+  /// \return the memory footprint of the input object.
+  template<typename T>
+  size_t computeMemoryFootprint(const T & object)
+  {
+    return internal::memory_footprint_evaluator<T>::run(object);
+  }
+
+} // namespace fcl
+} // namespace hpp
+
+#endif // ifndef HPP_FCL_SERIALIZATION_MEMORY_H

python/collision-geometries.cc

 //
 // Software License Agreement (BSD License)
 //
-//  Copyright (c) 2019-2020 CNRS-LAAS INRIA
+//  Copyright (c) 2019-2021 CNRS-LAAS INRIA
 //  Author: Joseph Mirabel
 //  All rights reserved.
 //
@@ -42,6 +42,7 @@
 #include <hpp/fcl/shape/geometric_shapes.h>
 #include <hpp/fcl/shape/convex.h>
 #include <hpp/fcl/BVH/BVH_model.h>
+#include <hpp/fcl/serialization/memory.h>
 
 #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
 // FIXME for a reason I do not understand, doxygen fails to understand that
@@ -156,6 +157,29 @@ struct ConvexWrapper
   }
 };
 
+
+template<typename T>
+void defComputeMemoryFootprint()
+{
+  doxygen::def("computeMemoryFootprint",&computeMemoryFootprint<T>);
+}
+
+void exposeComputeMemoryFootprint()
+{
+  defComputeMemoryFootprint<Sphere>();
+  defComputeMemoryFootprint<Cone>();
+  defComputeMemoryFootprint<Capsule>();
+  defComputeMemoryFootprint<Cylinder>();
+  defComputeMemoryFootprint<Box>();
+  defComputeMemoryFootprint<Plane>();
+  defComputeMemoryFootprint<Halfspace>();
+  defComputeMemoryFootprint<TriangleP>();
+  
+  defComputeMemoryFootprint< BVHModel<OBB> >();
+  defComputeMemoryFootprint< BVHModel<RSS> >();
+  defComputeMemoryFootprint< BVHModel<OBBRSS> >();
+}
+
 void exposeShapes ()
 {
   class_ <ShapeBase, bases<CollisionGeometry>, shared_ptr<ShapeBase>, noncopyable>
@@ -470,6 +494,7 @@ void exposeCollisionGeometries ()
     ;
   exposeBVHModel<OBB    >("OBB"    );
   exposeBVHModel<OBBRSS >("OBBRSS" );
+  exposeComputeMemoryFootprint();
 }
 
 void exposeCollisionObject ()

src/BVH/BVH_model.cpp

@@ -826,7 +826,7 @@ int BVHModel<BV>::memUsage(const bool msg) const
     std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl;
   }
 
-  return BVH_OK;
+  return total_mem;
 }
 
 template<typename BV>

test/serialization.cpp

@@ -45,6 +45,8 @@
 #include <hpp/fcl/serialization/collision_data.h>
 #include <hpp/fcl/serialization/AABB.h>
 #include <hpp/fcl/serialization/BVH_model.h>
+#include <hpp/fcl/serialization/geometric_shapes.h>
+#include <hpp/fcl/serialization/memory.h>
 
 #include "utility.h"
 #include "fcl_resources/config.h"
@@ -239,3 +241,72 @@ BOOST_AUTO_TEST_CASE(test_BVHModel)
     test_serialization(m1,m1_copy,STREAM);
   }
 }
+
+BOOST_AUTO_TEST_CASE(test_shapes)
+{
+  
+  {
+    TriangleP triangle(Vec3f::UnitX(),
+                       Vec3f::UnitY(),
+                       Vec3f::UnitZ());
+    TriangleP triangle_copy(Vec3f::Random(),Vec3f::Random(),Vec3f::Random());
+    test_serialization(triangle,triangle_copy);
+  }
+  
+  {
+    Box box(Vec3f::UnitX()), box_copy(Vec3f::Random());
+    test_serialization(box,box_copy);
+  }
+  
+  {
+    Sphere sphere(1.), sphere_copy(2.);
+    test_serialization(sphere,sphere_copy);
+  }
+  
+  {
+    Capsule capsule(1.,2.), capsule_copy(10.,10.);
+    test_serialization(capsule,capsule_copy);
+  }
+  
+  {
+    Cone cone(1.,2.), cone_copy(10.,10.);
+    test_serialization(cone,cone_copy);
+  }
+  
+  {
+    Cylinder cylinder(1.,2.), cylinder_copy(10.,10.);
+    test_serialization(cylinder,cylinder_copy);
+  }
+  
+  {
+    Halfspace hs(Vec3f::Random(),1.), hs_copy(Vec3f::Zero(),0.);
+    test_serialization(hs,hs_copy);
+  }
+  
+  {
+    Plane plane(Vec3f::Random(),1.), plane_copy(Vec3f::Zero(),0.);
+    test_serialization(plane,plane_copy);
+  }
+}
+
+BOOST_AUTO_TEST_CASE(test_memory_footprint)
+{
+  Sphere sphere(1.);
+  BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere));
+  
+  std::vector<Vec3f> p1;
+  std::vector<Triangle> t1;
+  boost::filesystem::path path(TEST_RESOURCES_DIR);
+  
+  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
+
+  BVHModel<OBBRSS> m1;
+
+  m1.beginModel();
+  m1.addSubModel(p1, t1);
+  m1.endModel();
+  
+  std::cout << "computeMemoryFootprint(m1): " << computeMemoryFootprint(m1) << std::endl;
+  BOOST_CHECK(sizeof(BVHModel<OBBRSS>) < computeMemoryFootprint(m1));
+  BOOST_CHECK(m1.memUsage(false) == computeMemoryFootprint(m1));
+}