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 
19 #include <hpp/fcl/internal/traversal_node_setup.h>
20 #include <hpp/fcl/internal/traversal_node_bvhs.h>
21 #include "../src/collision_node.h"
22 #include <hpp/fcl/internal/BV_splitter.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];
177  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_rss[0][SPLIT_METHOD_MEAN]);
178  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_rss[0][SPLIT_METHOD_BV_CENTER]);
179  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_rss[0][SPLIT_METHOD_MEDIAN]);
180  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_rss[1][SPLIT_METHOD_MEAN]);
181  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_rss[1][SPLIT_METHOD_BV_CENTER]);
182  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_rss[1][SPLIT_METHOD_MEDIAN]);
183 
184  BVHModel<kIOS> ms_kios[2][3];
185  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_kios[0][SPLIT_METHOD_MEAN]);
186  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_kios[0][SPLIT_METHOD_BV_CENTER]);
187  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_kios[0][SPLIT_METHOD_MEDIAN]);
188  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_kios[1][SPLIT_METHOD_MEAN]);
189  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_kios[1][SPLIT_METHOD_BV_CENTER]);
190  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_kios[1][SPLIT_METHOD_MEDIAN]);
191 
192  BVHModel<OBB> ms_obb[2][3];
193  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obb[0][SPLIT_METHOD_MEAN]);
194  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_obb[0][SPLIT_METHOD_BV_CENTER]);
195  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obb[0][SPLIT_METHOD_MEDIAN]);
196  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obb[1][SPLIT_METHOD_MEAN]);
197  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_obb[1][SPLIT_METHOD_BV_CENTER]);
198  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obb[1][SPLIT_METHOD_MEDIAN]);
199 
200  BVHModel<OBBRSS> ms_obbrss[2][3];
201  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obbrss[0][SPLIT_METHOD_MEAN]);
202  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER,
203  ms_obbrss[0][SPLIT_METHOD_BV_CENTER]);
204  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obbrss[0][SPLIT_METHOD_MEDIAN]);
205  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obbrss[1][SPLIT_METHOD_MEAN]);
206  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER,
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 }
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
traversal_node_setup.h
DELTA
FCL_REAL DELTA
Definition: benchmark.cpp:33
traits< OBBRSS >::CollisionTraversalNode
MeshCollisionTraversalNodeOBBRSS CollisionTraversalNode
Definition: benchmark.cpp:76
octree.p1
tuple p1
Definition: octree.py:55
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
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
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::stop
void stop()
stop the timer
Definition: utility.cpp:44
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
traits< RSS >::DistanceTraversalNode
MeshDistanceTraversalNodeRSS DistanceTraversalNode
Definition: benchmark.cpp:59
traits< kIOS >::DistanceTraversalNode
MeshDistanceTraversalNodekIOS DistanceTraversalNode
Definition: benchmark.cpp:65
BV_splitter.h
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
traits< RSS >::CollisionTraversalNode
MeshCollisionTraversalNodeRSS CollisionTraversalNode
Definition: benchmark.cpp:58
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
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
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
traversal_node_bvhs.h
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
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
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
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
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
traits< OBB >::CollisionTraversalNode
MeshCollisionTraversalNodeOBB CollisionTraversalNode
Definition: benchmark.cpp:70
hpp::fcl::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: internal/BV_splitter.h:51
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, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
Definition: src/collision.cpp:63
hpp::fcl::BenchTimer::start
void start()
start timer
Definition: utility.cpp:35
hpp::fcl::BenchTimer::getElapsedTimeInMicroSec
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
Definition: utility.cpp:54
main
int main(int, char *[])
Definition: benchmark.cpp:168
traits< OBBRSS >::DistanceTraversalNode
MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode
Definition: benchmark.cpp:77
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
RUN_CASE
#define RUN_CASE(BV, tf, models, split)
Definition: benchmark.cpp:27
hpp::fcl::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BVH/BVH_model.h:59
verbose
bool verbose
Definition: benchmark.cpp:32
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
traits< kIOS >::CollisionTraversalNode
MeshCollisionTraversalNodekIOS CollisionTraversalNode
Definition: benchmark.cpp:64
hpp::fcl::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:473
makeModel
void makeModel(const std::vector< Vec3f > &vertices, const std::vector< Triangle > &triangles, SplitMethodType split_method, BVHModel< BV > &model)
Definition: benchmark.cpp:81
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::OBB
Oriented bounding box class.
Definition: include/hpp/fcl/BV/OBB.h:52

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00