serialization.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021-2022 INRIA.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #define BOOST_TEST_MODULE FCL_SERIALIZATION
36 #include <fstream>
37 #include <boost/test/included/unit_test.hpp>
38 
39 #include <hpp/fcl/collision.h>
40 #include <hpp/fcl/distance.h>
41 #include <hpp/fcl/BV/OBBRSS.h>
42 #include <hpp/fcl/BVH/BVH_model.h>
43 
51 
52 #include "utility.h"
53 #include "fcl_resources/config.h"
54 
55 #include <boost/archive/tmpdir.hpp>
56 #include <boost/archive/text_iarchive.hpp>
57 #include <boost/archive/text_oarchive.hpp>
58 #include <boost/archive/xml_iarchive.hpp>
59 #include <boost/archive/xml_oarchive.hpp>
60 #include <boost/archive/binary_iarchive.hpp>
61 #include <boost/archive/binary_oarchive.hpp>
62 #include <boost/filesystem.hpp>
63 
64 #include <boost/asio/streambuf.hpp>
65 
66 namespace utf = boost::unit_test::framework;
67 
68 using namespace hpp::fcl;
69 
70 template <typename T>
71 void saveToBinary(const T& object, boost::asio::streambuf& buffer) {
72  boost::archive::binary_oarchive oa(buffer);
73  oa & object;
74 }
75 
76 template <typename T>
77 inline void loadFromBinary(T& object, boost::asio::streambuf& buffer) {
78  boost::archive::binary_iarchive ia(buffer);
79  ia >> object;
80 }
81 
82 template <typename T>
83 bool check(const T& value, const T& other) {
84  return value == other;
85 }
86 
87 enum SerializationMode { TXT = 1, XML = 2, BIN = 4, STREAM = 8 };
88 
89 template <typename T>
90 void test_serialization(const T& value, T& other_value,
91  const int mode = TXT | XML | BIN | STREAM) {
92  const boost::filesystem::path tmp_path(boost::archive::tmpdir());
93  const boost::filesystem::path txt_path("file.txt");
94  const boost::filesystem::path bin_path("file.bin");
95  const boost::filesystem::path txt_filename(tmp_path / txt_path);
96  const boost::filesystem::path bin_filename(tmp_path / bin_path);
97 
98  // TXT
99  if (mode & 0x1) {
100  {
101  std::ofstream ofs(txt_filename.c_str());
102 
103  boost::archive::text_oarchive oa(ofs);
104  oa << value;
105  }
106  BOOST_CHECK(check(value, value));
107 
108  {
109  std::ifstream ifs(txt_filename.c_str());
110  boost::archive::text_iarchive ia(ifs);
111 
112  ia >> other_value;
113  }
114  BOOST_CHECK(check(value, other_value));
115  }
116 
117  // BIN
118  if (mode & 0x4) {
119  {
120  std::ofstream ofs(bin_filename.c_str(), std::ios::binary);
121  boost::archive::binary_oarchive oa(ofs);
122  oa << value;
123  }
124  BOOST_CHECK(check(value, value));
125 
126  {
127  std::ifstream ifs(bin_filename.c_str(), std::ios::binary);
128  boost::archive::binary_iarchive ia(ifs);
129 
130  ia >> other_value;
131  }
132  BOOST_CHECK(check(value, other_value));
133  }
134 
135  // Stream Buffer
136  if (mode & 0x8) {
137  boost::asio::streambuf buffer;
138  saveToBinary(value, buffer);
139  BOOST_CHECK(check(value, value));
140 
141  loadFromBinary(other_value, buffer);
142  BOOST_CHECK(check(value, other_value));
143  }
144 }
145 
146 template <typename T>
147 void test_serialization(const T& value,
148  const int mode = TXT | XML | BIN | STREAM) {
149  T other_value;
150  test_serialization(value, other_value, mode);
151 }
152 
154  AABB aabb(-Vec3f::Ones(), Vec3f::Ones());
155  test_serialization(aabb);
156 }
157 
158 BOOST_AUTO_TEST_CASE(test_collision_data) {
159  Contact contact(NULL, NULL, 1, 2, Vec3f::Ones(), Vec3f::Zero(), -10.);
160  test_serialization(contact);
161 
162  CollisionRequest collision_request(CONTACT, 10);
163  test_serialization(collision_request);
164 
165  CollisionResult collision_result;
166  collision_result.addContact(contact);
167  collision_result.addContact(contact);
168  collision_result.distance_lower_bound = 0.1;
169  test_serialization(collision_result);
170 
171  DistanceRequest distance_request(true, 1., 2.);
172  test_serialization(distance_request);
173 
174  DistanceResult distance_result;
175  distance_result.normal.setOnes();
176  distance_result.nearest_points[0].setRandom();
177  distance_result.nearest_points[1].setRandom();
178  test_serialization(distance_result);
179 }
180 
181 BOOST_AUTO_TEST_CASE(test_BVHModel) {
182  std::vector<Vec3f> p1, p2;
183  std::vector<Triangle> t1, t2;
184  boost::filesystem::path path(TEST_RESOURCES_DIR);
185 
186  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
187  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
188 
189  BVHModel<OBBRSS> m1, m2;
190 
191  m1.beginModel();
192  m1.addSubModel(p1, t1);
193  m1.endModel();
194  BOOST_CHECK(m1 == m1);
195 
196  m2.beginModel();
197  m2.addSubModel(p2, t2);
198  m2.endModel();
199  BOOST_CHECK(m2 == m2);
200  BOOST_CHECK(m1 != m2);
201 
202  // Test BVHModel
203  {
204  BVHModel<OBBRSS> m1_copy;
205  test_serialization(m1, m1_copy);
206  }
207  {
208  BVHModel<OBBRSS> m1_copy;
209  test_serialization(m1, m1_copy, STREAM);
210  }
211 }
212 
213 #ifdef HPP_FCL_HAS_QHULL
214 BOOST_AUTO_TEST_CASE(test_Convex) {
215  std::vector<Vec3f> p1;
216  std::vector<Triangle> t1;
217  boost::filesystem::path path(TEST_RESOURCES_DIR);
218 
219  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
220 
221  BVHModel<OBBRSS> m1;
222 
223  m1.beginModel();
224  m1.addSubModel(p1, t1);
225  m1.endModel();
226 
227  m1.buildConvexHull(true);
228 
229  Convex<Triangle>& convex = static_cast<Convex<Triangle>&>(*m1.convex.get());
230 
231  // Test Convex
232  {
233  Convex<Triangle> convex_copy;
234  test_serialization(convex, convex_copy);
235  }
236 }
237 #endif
238 
239 BOOST_AUTO_TEST_CASE(test_HeightField) {
240  const FCL_REAL min_altitude = -1.;
241  const FCL_REAL x_dim = 1., y_dim = 2.;
242  const Eigen::DenseIndex nx = 100, ny = 200;
243  const MatrixXf heights = MatrixXf::Random(ny, nx);
244 
245  HeightField<OBBRSS> hfield(x_dim, y_dim, heights, min_altitude);
246 
247  // Test HeightField
248  {
249  HeightField<OBBRSS> hfield_copy;
250  test_serialization(hfield, hfield_copy);
251  }
252  {
253  HeightField<OBBRSS> hfield_copy;
254  test_serialization(hfield, hfield_copy, STREAM);
255  }
256 }
257 
258 BOOST_AUTO_TEST_CASE(test_shapes) {
259  {
261  TriangleP triangle_copy(Vec3f::Random(), Vec3f::Random(), Vec3f::Random());
262  test_serialization(triangle, triangle_copy);
263  }
264 
265  {
266  Box box(Vec3f::UnitX()), box_copy(Vec3f::Random());
267  test_serialization(box, box_copy);
268  }
269 
270  {
271  Sphere sphere(1.), sphere_copy(2.);
272  test_serialization(sphere, sphere_copy);
273  }
274 
275  {
276  Ellipsoid ellipsoid(1., 2., 3.), ellipsoid_copy(0., 0., 0.);
277  test_serialization(ellipsoid, ellipsoid_copy);
278  }
279 
280  {
281  Capsule capsule(1., 2.), capsule_copy(10., 10.);
282  test_serialization(capsule, capsule_copy);
283  }
284 
285  {
286  Cone cone(1., 2.), cone_copy(10., 10.);
287  test_serialization(cone, cone_copy);
288  }
289 
290  {
291  Cylinder cylinder(1., 2.), cylinder_copy(10., 10.);
292  test_serialization(cylinder, cylinder_copy);
293  }
294 
295  {
296  Halfspace hs(Vec3f::Random(), 1.), hs_copy(Vec3f::Zero(), 0.);
297  test_serialization(hs, hs_copy);
298  }
299 
300  {
301  Plane plane(Vec3f::Random(), 1.), plane_copy(Vec3f::Zero(), 0.);
302  test_serialization(plane, plane_copy);
303  }
304 }
305 
306 BOOST_AUTO_TEST_CASE(test_memory_footprint) {
307  Sphere sphere(1.);
308  BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere));
309 
310  std::vector<Vec3f> p1;
311  std::vector<Triangle> t1;
312  boost::filesystem::path path(TEST_RESOURCES_DIR);
313 
314  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
315 
316  BVHModel<OBBRSS> m1;
317 
318  m1.beginModel();
319  m1.addSubModel(p1, t1);
320  m1.endModel();
321 
322  std::cout << "computeMemoryFootprint(m1): " << computeMemoryFootprint(m1)
323  << std::endl;
324  BOOST_CHECK(sizeof(BVHModel<OBBRSS>) < computeMemoryFootprint(m1));
325  BOOST_CHECK(static_cast<size_t>(m1.memUsage(false)) ==
327 }
hpp::fcl::BVHModelBase::buildConvexHull
bool buildConvexHull(bool keepTriangle, const char *qhullCommand=NULL)
Build a convex hull and store it in attribute convex.
Definition: BVH_model.cpp:132
hpp::fcl::UnitX
const Vec3f UnitX
Definition: utility.cpp:87
hpp::fcl::CollisionResult::addContact
void addContact(const Contact &c)
add one contact into result structure
Definition: collision_data.h:330
hpp::fcl::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: utility.cpp:96
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
hpp::fcl::MatrixXf
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, Eigen::Dynamic > MatrixXf
Definition: data_types.h:71
XML
@ XML
Definition: serialization.cpp:87
SerializationMode
SerializationMode
Definition: serialization.cpp:87
collision_data.h
collision_manager.sphere
sphere
Definition: collision_manager.py:4
hpp::fcl::BVHModelBase::beginModel
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model.cpp:170
hpp::fcl::BVHModelBase::convex
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH/BVH_model.h:84
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::CONTACT
@ CONTACT
Definition: collision_data.h:229
hpp::fcl::HeightField
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition: hfield.h:182
test_serialization
void test_serialization(const T &value, T &other_value, const int mode=TXT|XML|BIN|STREAM)
Definition: serialization.cpp:90
utility.h
STREAM
@ STREAM
Definition: serialization.cpp:87
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
loadFromBinary
void loadFromBinary(T &object, boost::asio::streambuf &buffer)
Definition: serialization.cpp:77
hpp::fcl::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:473
hpp::fcl::Convex
Convex polytope.
Definition: shape/convex.h:50
hpp::fcl::Ellipsoid
Ellipsoid centered at point zero.
Definition: shape/geometric_shapes.h:258
distance.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DistanceResult::normal
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:430
hpp::fcl::BVHModel::memUsage
int memUsage(const bool msg) const
Check the number of memory used.
Definition: BVH_model.cpp:797
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
value
float value
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_aabb)
Definition: serialization.cpp:153
geometric_shapes.h
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
TXT
@ TXT
Definition: serialization.cpp:87
saveToBinary
void saveToBinary(const T &object, boost::asio::streambuf &buffer)
Definition: serialization.cpp:71
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: shape/geometric_shapes.h:333
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
collision.path
path
Definition: scripts/collision.py:6
hpp::fcl
Definition: broadphase_bruteforce.h:45
octree.p1
tuple p1
Definition: octree.py:54
AABB.h
BVH_model.h
convex.h
check
bool check(const T &value, const T &other)
Definition: serialization.cpp:83
OBBRSS.h
BIN
@ BIN
Definition: serialization.cpp:87
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: shape/geometric_shapes.h:729
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
BVH_model.h
hpp::fcl::UnitY
const Vec3f UnitY
Definition: utility.cpp:88
hpp::fcl::UnitZ
const Vec3f UnitZ
Definition: utility.cpp:89
hpp::fcl::computeMemoryFootprint
size_t computeMemoryFootprint(const T &object)
Returns the memory footpring of the input object. For POD objects, this function returns the result o...
Definition: memory.h:25
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: shape/geometric_shapes.h:71
hfield.h
hpp::fcl::BVHModelBase::addSubModel
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model.cpp:410
memory.h
hpp::fcl::CollisionResult::distance_lower_bound
FCL_REAL distance_lower_bound
Definition: collision_data.h:312
hpp::fcl::Contact
Contact information returned by collision.
Definition: collision_data.h:54
collision.h
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::Plane
Infinite plane.
Definition: shape/geometric_shapes.h:810


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:15