benchmark.cpp
Go to the documentation of this file.
1 // Copyright (c) 2016, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-fcl.
5 // hpp-fcl is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-fcl is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
16 
17 #include <boost/filesystem.hpp>
18 
21 #include "../src/collision_node.h"
23 
24 #include "utility.h"
25 #include "fcl_resources/config.h"
26 
27 #define RUN_CASE(BV, tf, models, split) \
28  run<BV>(tf, models, split, #BV " - " #split ":\t")
29 
30 using namespace hpp::fcl;
31 
32 bool verbose = false;
33 FCL_REAL DELTA = 0.001;
34 
35 template <typename BV>
36 void makeModel(const std::vector<Vec3f>& vertices,
37  const std::vector<Triangle>& triangles,
38  SplitMethodType split_method, BVHModel<BV>& model);
39 
40 template <typename BV, typename TraversalNode>
41 double distance(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
42  const BVHModel<BV>& m2, bool verbose);
43 
44 template <typename BV, typename TraversalNode>
45 double collide(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
46  const BVHModel<BV>& m2, bool verbose);
47 
48 template <typename BV>
49 double run(const std::vector<Transform3f>& tf,
50  const BVHModel<BV> (&models)[2][3], int split_method,
51  const char* sm_name);
52 
53 template <typename BV>
54 struct traits {};
55 
56 template <>
57 struct traits<RSS> {
58  typedef MeshCollisionTraversalNodeRSS CollisionTraversalNode;
59  typedef MeshDistanceTraversalNodeRSS DistanceTraversalNode;
60 };
61 
62 template <>
63 struct traits<kIOS> {
64  typedef MeshCollisionTraversalNodekIOS CollisionTraversalNode;
65  typedef MeshDistanceTraversalNodekIOS DistanceTraversalNode;
66 };
67 
68 template <>
69 struct traits<OBB> {
70  typedef MeshCollisionTraversalNodeOBB CollisionTraversalNode;
71  // typedef MeshDistanceTraversalNodeOBB DistanceTraversalNode;
72 };
73 
74 template <>
75 struct traits<OBBRSS> {
76  typedef MeshCollisionTraversalNodeOBBRSS CollisionTraversalNode;
77  typedef MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode;
78 };
79 
80 template <typename BV>
81 void makeModel(const std::vector<Vec3f>& vertices,
82  const std::vector<Triangle>& triangles,
83  SplitMethodType split_method, BVHModel<BV>& model) {
84  model.bv_splitter.reset(new BVSplitter<BV>(split_method));
85  model.bv_splitter.reset(new BVSplitter<BV>(split_method));
86 
87  model.beginModel();
88  model.addSubModel(vertices, triangles);
89  model.endModel();
90 }
91 
92 template <typename BV, typename TraversalNode>
93 double distance(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
94  const BVHModel<BV>& m2, bool verbose) {
95  Transform3f pose2;
96 
97  DistanceResult local_result;
98  DistanceRequest request(true);
99  TraversalNode node;
100 
101  node.enable_statistics = verbose;
102 
103  BenchTimer timer;
104  timer.start();
105 
106  for (std::size_t i = 0; i < tf.size(); ++i) {
107  if (!initialize(node, m1, tf[i], m2, pose2, request, local_result))
108  std::cout << "initialize error" << std::endl;
109 
110  distance(&node, NULL);
111  }
112  timer.stop();
113  return timer.getElapsedTimeInMicroSec();
114 }
115 
116 template <typename BV, typename TraversalNode>
117 double collide(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
118  const BVHModel<BV>& m2, bool verbose) {
119  Transform3f pose2;
120 
121  CollisionResult local_result;
122  CollisionRequest request;
123  TraversalNode node(request);
124 
125  node.enable_statistics = verbose;
126 
127  BenchTimer timer;
128  timer.start();
129 
130  for (std::size_t i = 0; i < tf.size(); ++i) {
131  bool success(initialize(node, m1, tf[i], m2, pose2, local_result));
132  (void)success;
133  assert(success);
134 
135  CollisionResult result;
136  collide(&node, request, result);
137  }
138 
139  timer.stop();
140  return timer.getElapsedTimeInMicroSec();
141 }
142 
143 template <typename BV>
144 double run(const std::vector<Transform3f>& tf,
145  const BVHModel<BV> (&models)[2][3], int split_method,
146  const char* prefix) {
147  double col = collide<BV, typename traits<BV>::CollisionTraversalNode>(
148  tf, models[0][split_method], models[1][split_method], verbose);
149  double dist = distance<BV, typename traits<BV>::DistanceTraversalNode>(
150  tf, models[0][split_method], models[1][split_method], verbose);
151 
152  std::cout << prefix << " (" << col << ", " << dist << ")\n";
153  return col + dist;
154 }
155 
156 template <>
157 double run<OBB>(const std::vector<Transform3f>& tf,
158  const BVHModel<OBB> (&models)[2][3], int split_method,
159  const char* prefix) {
160  double col = collide<OBB, traits<OBB>::CollisionTraversalNode>(
161  tf, models[0][split_method], models[1][split_method], verbose);
162  double dist = 0;
163 
164  std::cout << prefix << " (\t" << col << ", \tNaN)\n";
165  return col + dist;
166 }
167 
168 int main(int, char*[]) {
169  std::vector<Vec3f> p1, p2;
170  std::vector<Triangle> t1, t2;
171  boost::filesystem::path path(TEST_RESOURCES_DIR);
172  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
173  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
174 
175  // Make models
176  BVHModel<RSS> ms_rss[2][3];
180  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_rss[1][SPLIT_METHOD_MEAN]);
183 
184  BVHModel<kIOS> ms_kios[2][3];
185  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_kios[0][SPLIT_METHOD_MEAN]);
188  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_kios[1][SPLIT_METHOD_MEAN]);
190  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_kios[1][SPLIT_METHOD_MEDIAN]);
191 
192  BVHModel<OBB> ms_obb[2][3];
196  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obb[1][SPLIT_METHOD_MEAN]);
199 
200  BVHModel<OBBRSS> ms_obbrss[2][3];
201  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obbrss[0][SPLIT_METHOD_MEAN]);
203  ms_obbrss[0][SPLIT_METHOD_BV_CENTER]);
205  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obbrss[1][SPLIT_METHOD_MEAN]);
207  ms_obbrss[1][SPLIT_METHOD_BV_CENTER]);
208  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]);
209 
210  std::vector<Transform3f> transforms; // t0
211  FCL_REAL extents[] = {-3000, -3000, -3000, 3000, 3000, 3000};
212  std::size_t n = 10000;
213 
214  generateRandomTransforms(extents, transforms, n);
215  double total_time = 0;
216 
217  total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEAN);
218  total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_BV_CENTER);
219  total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEDIAN);
220 
221  total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEAN);
222  total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_BV_CENTER);
223  total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEDIAN);
224 
225  total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEAN);
226  total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_BV_CENTER);
227  total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEDIAN);
228 
229  total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEAN);
230  total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_BV_CENTER);
231  total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEDIAN);
232 
233  std::cout << "\n\nTotal time: " << total_time << std::endl;
234 }
verbose
bool verbose
Definition: benchmark.cpp:32
traits< OBBRSS >::CollisionTraversalNode
MeshCollisionTraversalNodeOBBRSS CollisionTraversalNode
Definition: benchmark.cpp:76
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
run
double run(const std::vector< Transform3f > &tf, const BVHModel< BV >(&models)[2][3], int split_method, const char *sm_name)
Definition: benchmark.cpp:144
traits< kIOS >::CollisionTraversalNode
MeshCollisionTraversalNodekIOS CollisionTraversalNode
Definition: benchmark.cpp:64
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::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
initialize
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Definition: traversal_node_setup.h:423
traits< RSS >::DistanceTraversalNode
MeshDistanceTraversalNodeRSS DistanceTraversalNode
Definition: benchmark.cpp:59
utility.h
run< OBB >
double run< OBB >(const std::vector< Transform3f > &tf, const BVHModel< OBB >(&models)[2][3], int split_method, const char *prefix)
Definition: benchmark.cpp:157
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
DELTA
FCL_REAL DELTA
Definition: benchmark.cpp:33
hpp::fcl::SPLIT_METHOD_MEDIAN
@ SPLIT_METHOD_MEDIAN
Definition: internal/BV_splitter.h:53
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
hpp::fcl::BVHModel::bv_splitter
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH/BVH_model.h:278
hpp::fcl::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:473
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts,...
Definition: src/collision.cpp:63
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::BenchTimer::stop
void stop()
stop the timer
Definition: utility.cpp:44
traits< RSS >::CollisionTraversalNode
MeshCollisionTraversalNodeRSS CollisionTraversalNode
Definition: benchmark.cpp:58
hpp::fcl::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: internal/BV_splitter.h:52
hpp::fcl::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: internal/BV_splitter.h:51
traits
Definition: benchmark.cpp:54
makeModel
void makeModel(const std::vector< Vec3f > &vertices, const std::vector< Triangle > &triangles, SplitMethodType split_method, BVHModel< BV > &model)
Definition: benchmark.cpp:81
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
traits< OBB >::CollisionTraversalNode
MeshCollisionTraversalNodeOBB CollisionTraversalNode
Definition: benchmark.cpp:70
hpp::fcl::OBB
Oriented bounding box class.
Definition: include/hpp/fcl/BV/OBB.h:52
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
hpp::fcl::SPLIT_METHOD_BV_CENTER
@ SPLIT_METHOD_BV_CENTER
Definition: internal/BV_splitter.h:54
hpp::fcl::generateRandomTransforms
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: utility.cpp:224
collision.path
path
Definition: scripts/collision.py:6
hpp::fcl
Definition: broadphase_bruteforce.h:45
octree.p1
tuple p1
Definition: octree.py:54
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
hpp::fcl::BenchTimer
Definition: utility.h:82
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
RUN_CASE
#define RUN_CASE(BV, tf, models, split)
Definition: benchmark.cpp:27
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::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BVH/BVH_model.h:59
traversal_node_bvhs.h
BV_splitter.h
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
traits< OBBRSS >::DistanceTraversalNode
MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode
Definition: benchmark.cpp:77
traversal_node_setup.h
main
int main(int, char *[])
Definition: benchmark.cpp:168
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
traits< kIOS >::DistanceTraversalNode
MeshDistanceTraversalNodekIOS DistanceTraversalNode
Definition: benchmark.cpp:65
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
hpp::fcl::BenchTimer::getElapsedTimeInMicroSec
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
Definition: utility.cpp:54
hpp::fcl::BenchTimer::start
void start()
start timer
Definition: utility.cpp:35


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