test_fcl_octomap_distance.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #include <gtest/gtest.h>
39 
40 #include "fcl/config.h"
52 
53 #include "test_fcl_utility.h"
54 
55 #include "fcl_resources/config.h"
56 
57 using namespace fcl;
58 
60 template <typename S>
61 void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1);
62 
63 template<typename BV>
64 void octomap_distance_test_BVH(std::size_t n, double resolution = 0.1);
65 
66 template <typename S>
68 {
69 #ifdef NDEBUG
70  octomap_distance_test<S>(200, 100, false, false);
71  octomap_distance_test<S>(200, 1000, false, false);
72 #else
73  octomap_distance_test<S>(200, 2, false, false, 1.0);
74  octomap_distance_test<S>(200, 10, false, false, 1.0);
75 #endif
76 }
77 
79 {
80 // test_octomap_distance<float>();
81  test_octomap_distance<double>();
82 }
83 
84 template <typename S>
86 {
87 #ifdef NDEBUG
88  octomap_distance_test<S>(200, 100, true, true);
89  octomap_distance_test<S>(200, 1000, true, true);
90 #else
91  octomap_distance_test<S>(200, 2, true, true, 1.0);
92  octomap_distance_test<S>(200, 5, true, true, 1.0);
93 #endif
94 }
95 
97 {
98 // test_octomap_distance_mesh<float>();
99  test_octomap_distance_mesh<double>();
100 }
101 
102 template <typename S>
104 {
105 #ifdef NDEBUG
106  octomap_distance_test<S>(200, 100, true, false);
107  octomap_distance_test<S>(200, 1000, true, false);
108 #else
109  octomap_distance_test<S>(200, 2, true, false, 1.0);
110  octomap_distance_test<S>(200, 5, true, false, 1.0);
111 #endif
112 }
113 
115 {
116 // test_octomap_distance_mesh_octomap_box<float>();
117  test_octomap_distance_mesh_octomap_box<double>();
118 }
119 
120 template <typename S>
122 {
123 #ifdef NDEBUG
124  octomap_distance_test_BVH<RSS<S>>(5);
125 #else
126  octomap_distance_test_BVH<RSS<S>>(5, 1.0);
127 #endif
128 }
129 
131 {
132 // test_octomap_bvh_rss_d_distance_rss<float>();
133  test_octomap_bvh_rss_d_distance_rss<double>();
134 }
135 
136 template <typename S>
138 {
139 #ifdef NDEBUG
140  octomap_distance_test_BVH<OBBRSS<S>>(5);
141 #else
142  octomap_distance_test_BVH<OBBRSS<S>>(5, 1.0);
143 #endif
144 }
145 
147 {
148 // test_octomap_bvh_obb_d_distance_obb<float>();
149  test_octomap_bvh_obb_d_distance_obb<double>();
150 }
151 
152 template <typename S>
154 {
155 #ifdef NDEBUG
156  octomap_distance_test_BVH<kIOS<S>>(5);
157 #else
158  octomap_distance_test_BVH<kIOS<S>>(5, 1.0);
159 #endif
160 }
161 
163 {
164 // test_octomap_bvh_kios_d_distance_kios<float>();
165  test_octomap_bvh_kios_d_distance_kios<double>();
166 }
167 
168 template<typename BV>
169 void octomap_distance_test_BVH(std::size_t n, double resolution)
170 {
171  using S = typename BV::S;
172 
173  std::vector<Vector3<S>> p1;
174  std::vector<Triangle> t1;
175 
176  test::loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1);
177 
178  BVHModel<BV>* m1 = new BVHModel<BV>();
179  std::shared_ptr<CollisionGeometry<S>> m1_ptr(m1);
180 
181  m1->beginModel();
182  m1->addSubModel(p1, t1);
183  m1->endModel();
184 
185  OcTree<S>* tree = new OcTree<S>(
186  std::shared_ptr<octomap::OcTree>(test::generateOcTree(resolution)));
187  std::shared_ptr<CollisionGeometry<S>> tree_ptr(tree);
188 
189  aligned_vector<Transform3<S>> transforms;
190  S extents[] = {-5, -5, -5, 5, 5, 5};
191 
192  test::generateRandomTransforms(extents, transforms, n);
193 
194  for(std::size_t i = 0; i < n; ++i)
195  {
196  Transform3<S> tf1(transforms[i]);
197  Transform3<S> tf2(transforms[n-1-i]);
198 
199  CollisionObject<S> obj1(m1_ptr, tf1);
200  CollisionObject<S> obj2(tree_ptr, tf2);
202  DefaultDistanceData<S> cdata1b;
203  cdata.request.enable_nearest_points = true;
204  cdata1b.request.enable_nearest_points = true;
205  S dist1 = std::numeric_limits<S>::max();
206  S dist1b = std::numeric_limits<S>::max();
207  // Verify that the order of geometry objects does not matter
208  DefaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
209  DefaultDistanceFunction(&obj2, &obj1, &cdata1b, dist1b);
210  EXPECT_NEAR(dist1, dist1b, constants<S>::eps());
211 
212 
213  std::vector<CollisionObject<S>*> boxes;
214  test::generateBoxesFromOctomap(boxes, *tree);
215  for(std::size_t j = 0; j < boxes.size(); ++j)
216  boxes[j]->setTransform(tf2 * boxes[j]->getTransform());
217 
220  manager->registerObjects(boxes);
221  manager->setup();
222 
223  DefaultDistanceData<S> cdata2;
224  manager->distance(&obj1, &cdata2, DefaultDistanceFunction);
225  S dist2 = cdata2.result.min_distance;
226 
227  for(std::size_t j = 0; j < boxes.size(); ++j)
228  delete boxes[j];
229  delete manager;
230 
231  EXPECT_NEAR(dist1, dist2, 0.001);
232 
233  // Check that the nearest points are consistent with the distance
234  // Note that we cannot compare the result with the "boxes" approximation,
235  // since the problem is ill-posed (i.e. the nearest points may differ widely
236  // for slightly different geometries)
237  Vector3<S> nearestPointDistance =
238  cdata.result.nearest_points[0] - cdata.result.nearest_points[1];
239  // Only check the nearest point distance for a non-collision.
240  // For a collision, the nearest points may be tangential and not equal to
241  // the (potentially fake) signed distance returned by the distance check.
242  if (dist1 > 0.0)
243  {
244  EXPECT_NEAR(nearestPointDistance.norm(), dist1, 0.001);
245  }
246  }
247 }
248 
249 template <typename S>
250 void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution)
251 {
252  // srand(1);
253  std::vector<CollisionObject<S>*> env;
254  if(use_mesh)
255  test::generateEnvironmentsMesh(env, env_scale, env_size);
256  else
257  test::generateEnvironments(env, env_scale, env_size);
258 
259  OcTree<S>* tree = new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution)));
260  CollisionObject<S> tree_obj((std::shared_ptr<CollisionGeometry<S>>(tree)));
261 
263  manager->registerObjects(env);
264  manager->setup();
265 
267  test::TStruct t1;
268  test::Timer timer1;
269  timer1.start();
270  manager->octree_as_geometry_collide = false;
271  manager->octree_as_geometry_distance = false;
272  manager->distance(&tree_obj, &cdata, DefaultDistanceFunction);
273  timer1.stop();
274  t1.push_back(timer1.getElapsedTime());
275 
276 
277  DefaultDistanceData<S> cdata3;
278  test::TStruct t3;
279  test::Timer timer3;
280  timer3.start();
281  manager->octree_as_geometry_collide = true;
282  manager->octree_as_geometry_distance = true;
283  manager->distance(&tree_obj, &cdata3, DefaultDistanceFunction);
284  timer3.stop();
285  t3.push_back(timer3.getElapsedTime());
286 
287 
288  test::TStruct t2;
289  test::Timer timer2;
290  timer2.start();
291  std::vector<CollisionObject<S>*> boxes;
292  if(use_mesh_octomap)
293  test::generateBoxesFromOctomapMesh(boxes, *tree);
294  else
295  test::generateBoxesFromOctomap(boxes, *tree);
296  timer2.stop();
297  t2.push_back(timer2.getElapsedTime());
298 
299  timer2.start();
301  manager2->registerObjects(boxes);
302  manager2->setup();
303  timer2.stop();
304  t2.push_back(timer2.getElapsedTime());
305 
306 
307  DefaultDistanceData<S> cdata2;
308 
309  timer2.start();
310  manager->distance(manager2, &cdata2, DefaultDistanceFunction);
311  timer2.stop();
312  t2.push_back(timer2.getElapsedTime());
313 
314  std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl;
315 
316  if(cdata.result.min_distance < 0)
317  EXPECT_LE(cdata2.result.min_distance, 0);
318  else
319  EXPECT_NEAR(cdata.result.min_distance, cdata2.result.min_distance, 1e-3);
320 
321  delete manager;
322  delete manager2;
323  for(size_t i = 0; i < boxes.size(); ++i)
324  delete boxes[i];
325 
326 
327  std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
328  std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
329  std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
330  std::cout << " a) to boxes: " << t2.records[0] << std::endl;
331  std::cout << " b) structure init: " << t2.records[1] << std::endl;
332  std::cout << " c) distance: " << t2.records[2] << std::endl;
333  std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
334 }
335 
336 //==============================================================================
337 int main(int argc, char* argv[])
338 {
339  ::testing::InitGoogleTest(&argc, argv);
340  return RUN_ALL_TESTS();
341 }
fcl::DynamicAABBTreeCollisionManager::setup
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_dynamic_AABB_tree-inl.h:800
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::test::TStruct::push_back
void push_back(double t)
Definition: test_fcl_utility.h:111
fcl::DynamicAABBTreeCollisionManager::registerObjects
void registerObjects(const std::vector< CollisionObject< S > * > &other_objs)
add objects to the manager
Definition: broadphase_dynamic_AABB_tree-inl.h:748
fcl::test::Timer
Definition: test_fcl_utility.h:77
fcl::DefaultDistanceFunction
bool DefaultDistanceFunction(CollisionObject< S > *o1, CollisionObject< S > *o2, void *data, S &dist)
Provides a simple callback for the distance query in the BroadPhaseCollisionManager....
Definition: default_broadphase_callbacks.h:196
fcl::test::TStruct::overall_time
double overall_time
Definition: test_fcl_utility.h:107
obj2
CollisionObject< S > * obj2
Definition: broadphase_SaP.h:211
GTEST_TEST
GTEST_TEST(FCL_OCTOMAP, test_octomap_distance)
Definition: test_fcl_octomap_distance.cpp:78
fcl::DynamicAABBTreeCollisionManager
Definition: broadphase_dynamic_AABB_tree.h:54
fcl::test::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vector3< S >> &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: test_fcl_utility.h:194
test_octomap_bvh_rss_d_distance_rss
void test_octomap_bvh_rss_d_distance_rss()
Definition: test_fcl_octomap_distance.cpp:121
fcl::test::Timer::getElapsedTime
double getElapsedTime()
get elapsed time in milli-second
Definition: test_fcl_utility.cpp:129
fcl::test::TStruct::records
std::vector< double > records
Definition: test_fcl_utility.h:106
default_broadphase_callbacks.h
fcl::test::Timer::start
void start()
start timer
Definition: test_fcl_utility.cpp:75
obj1
CollisionObject< S > * obj1
Definition: broadphase_SaP.h:210
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::BVHModel::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model-inl.h:450
fcl::DefaultDistanceData::request
DistanceRequest< S > request
Definition: default_broadphase_callbacks.h:166
broadphase_spatialhash.h
test_fcl_utility.h
fcl::DynamicAABBTreeCollisionManager::octree_as_geometry_collide
bool octree_as_geometry_collide
Definition: broadphase_dynamic_AABB_tree.h:67
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::test::TStruct
Definition: test_fcl_utility.h:104
octomap_distance_test_BVH
void octomap_distance_test_BVH(std::size_t n, double resolution=0.1)
Definition: test_fcl_octomap_distance.cpp:169
fcl::DefaultDistanceData::result
DistanceResult< S > result
Definition: default_broadphase_callbacks.h:167
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
fcl::DefaultDistanceData
Distance data for use with the DefaultDistanceFunction. It stores the distance request and the result...
Definition: default_broadphase_callbacks.h:165
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
octomap_distance_test
void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution=0.1)
Octomap distance with an environment with 3 * env_size objects.
Definition: test_fcl_octomap_distance.cpp:250
broadphase_dynamic_AABB_tree_array.h
broadphase_SaP.h
broadphase_interval_tree.h
fcl::BVHModel::beginModel
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model-inl.h:207
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::test::generateRandomTransforms
void generateRandomTransforms(S extents[6], aligned_vector< Transform3< S >> &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: test_fcl_utility.h:349
fcl::test::Timer::stop
void stop()
stop the timer
Definition: test_fcl_utility.cpp:86
test_octomap_distance
void test_octomap_distance()
Definition: test_fcl_octomap_distance.cpp:67
broadphase_dynamic_AABB_tree.h
broadphase_SSaP.h
fcl::test::generateEnvironments
void generateEnvironments(std::vector< CollisionObject< S > * > &env, S env_scale, std::size_t n)
Generate environment with 3 * n objects: n boxes, n spheres and n cylinders.
Definition: test_fcl_utility.h:476
test_octomap_bvh_obb_d_distance_obb
void test_octomap_bvh_obb_d_distance_obb()
Definition: test_fcl_octomap_distance.cpp:137
fcl::DynamicAABBTreeCollisionManager::distance
void distance(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback) const
perform distance computation between one object and all the objects belonging to the manager
Definition: broadphase_dynamic_AABB_tree-inl.h:922
geometric_shape_to_BVH_model.h
test_octomap_bvh_kios_d_distance_kios
void test_octomap_bvh_kios_d_distance_kios()
Definition: test_fcl_octomap_distance.cpp:153
test_octomap_distance_mesh_octomap_box
void test_octomap_distance_mesh_octomap_box()
Definition: test_fcl_octomap_distance.cpp:103
fcl::constants
Definition: constants.h:129
test_octomap_distance_mesh
void test_octomap_distance_mesh()
Definition: test_fcl_octomap_distance.cpp:85
fcl::BVHModel::addSubModel
int addSubModel(const std::vector< Vector3< S >> &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model-inl.h:383
main
int main(int argc, char *argv[])
Definition: test_fcl_octomap_distance.cpp:337
fcl::DynamicAABBTreeCollisionManager::octree_as_geometry_distance
bool octree_as_geometry_distance
Definition: broadphase_dynamic_AABB_tree.h:68
fcl::test::generateEnvironmentsMesh
void generateEnvironmentsMesh(std::vector< CollisionObject< S > * > &env, S env_scale, std::size_t n)
Generate environment with 3 * n objects, but all in meshes.
Definition: test_fcl_utility.h:505
broadphase_bruteforce.h
extents
std::array< S, 6 > & extents()
Definition: test_fcl_geometric_shapes.cpp:63
octree.h
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::aligned_vector
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: types.h:122
collision.h


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49