profiling.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017, 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/fwd.hh>
20 #include <hpp/fcl/collision.h>
21 #include <hpp/fcl/BVH/BVH_model.h>
27 #include "utility.h"
28 #include "fcl_resources/config.h"
29 
30 using namespace hpp::fcl;
31 
34  OBJECT_TYPE object_type1 = o1->getObjectType();
35  OBJECT_TYPE object_type2 = o2->getObjectType();
36  NODE_TYPE node_type1 = o1->getNodeType();
37  NODE_TYPE node_type2 = o2->getNodeType();
38 
39  if (object_type1 == OT_GEOM && object_type2 == OT_BVH)
40  return (lookupTable.collision_matrix[node_type2][node_type1] != NULL);
41  else
42  return (lookupTable.collision_matrix[node_type1][node_type2] != NULL);
43 }
44 
45 template <typename BV /*, SplitMethodType split_method*/>
47  std::vector<Vec3f> pt;
48  std::vector<Triangle> tri;
49  loadOBJFile(filename.c_str(), pt, tri);
50 
51  BVHModel<BV>* model(new BVHModel<BV>());
52  // model->bv_splitter.reset(new BVSplitter<BV>(split_method));
53 
54  model->beginModel();
55  model->addSubModel(pt, tri);
56  model->endModel();
57 
58  return CollisionGeometryPtr_t(model);
59 }
60 
61 template <typename BV /*, SplitMethodType split_method*/>
63  const Vec3f& scale = Vec3f(1, 1, 1)) {
64  shared_ptr<BVHModel<BV> > model(new BVHModel<BV>());
65  loadPolyhedronFromResource(filename, scale, model);
66  return model;
67 }
68 
69 struct Geometry {
70  std::string type;
72 
73  Geometry(const std::string& t, CollisionGeometry* ob) : type(t), o(ob) {}
74  Geometry(const std::string& t, const CollisionGeometryPtr_t& ob)
75  : type(t), o(ob) {}
76 };
77 
78 struct Results {
79  std::vector<CollisionResult> rs;
80  Eigen::VectorXd times; // micro-seconds
81 
82  Results(size_t i) : rs(i), times((Eigen::DenseIndex)i) {}
83 };
84 
85 void collide(const std::vector<Transform3f>& tf, const CollisionGeometry* o1,
86  const CollisionGeometry* o2, const CollisionRequest& request,
87  Results& results) {
88  Transform3f Id;
89  BenchTimer timer;
90  for (std::size_t i = 0; i < tf.size(); ++i) {
91  timer.start();
92  /* int num_contact = */
93  collide(o1, tf[i], o2, Id, request, results.rs[i]);
94  timer.stop();
95  results.times[(Eigen::DenseIndex)i] = timer.getElapsedTimeInMicroSec();
96  }
97 }
98 
99 const char* sep = ", ";
100 
102  std::cout << "Type 1" << sep << "Type 2" << sep << "mean time" << sep
103  << "time std dev" << sep << "min time" << sep << "max time"
104  << std::endl;
105 }
106 
107 void printResults(const Geometry& g1, const Geometry& g2, const Results& rs) {
108  double mean = rs.times.mean();
109  double var = rs.times.cwiseAbs2().mean() - mean * mean;
110  std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var)
111  << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff()
112  << std::endl;
113 }
114 
115 #ifndef NDEBUG // if debug mode
116 size_t Ntransform = 1;
117 #else
118 size_t Ntransform = 100;
119 #endif
121 bool verbose = false;
122 
123 #define OUT(x) \
124  if (verbose) std::cout << x << std::endl
125 #define CHECK_PARAM_NB(NB, NAME) \
126  if (iarg + NB >= argc) \
127  throw std::invalid_argument(#NAME " requires " #NB " numbers")
128 void handleParam(int& iarg, const int& argc, char** argv,
129  CollisionRequest& request) {
130  while (iarg < argc) {
131  std::string a(argv[iarg]);
132  if (a == "-nb_transform") {
133  CHECK_PARAM_NB(1, nb_transform);
134  Ntransform = (size_t)atoi(argv[iarg + 1]);
135  OUT("nb_transform = " << Ntransform);
136  iarg += 2;
137  } else if (a == "-enable_distance_lower_bound") {
138  CHECK_PARAM_NB(1, enable_distance_lower_bound);
139  request.enable_distance_lower_bound = bool(atoi(argv[iarg + 1]));
140  iarg += 2;
141  } else if (a == "-limit") {
142  CHECK_PARAM_NB(1, limit);
143  limit = atof(argv[iarg + 1]);
144  iarg += 2;
145  } else if (a == "-verbose") {
146  verbose = true;
147  iarg += 1;
148  } else {
149  break;
150  }
151  }
152 }
153 #define CREATE_SHAPE_2(var, Name) \
154  CHECK_PARAM_NB(2, Name); \
155  var.reset(new Name(atof(argv[iarg + 1]), atof(argv[iarg + 2]))); \
156  iarg += 3;
157 Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) {
158  if (iarg >= argc) throw std::invalid_argument("An argument is required.");
159  std::string a(argv[iarg]);
160  std::string type;
162  if (a == "-box") {
163  CHECK_PARAM_NB(3, Box);
164  o.reset(new Box(atof(argv[iarg + 1]), atof(argv[iarg + 2]),
165  atof(argv[iarg + 3])));
166  iarg += 4;
167  type = "box";
168  } else if (a == "-sphere") {
170  o.reset(new Sphere(atof(argv[iarg + 1])));
171  iarg += 2;
172  type = "sphere";
173  } else if (a == "-mesh") {
174  CHECK_PARAM_NB(2, Mesh);
175  OUT("Loading " << argv[iarg + 2] << " as BVHModel<" << argv[iarg + 1]
176  << ">...");
177  if (strcmp(argv[iarg + 1], "obb") == 0) {
178  o = meshToGeom<OBB>(argv[iarg + 2]);
179  OUT("Mesh has " << dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris
180  << " triangles");
181  type = "mesh_obb";
182  } else if (strcmp(argv[iarg + 1], "obbrss") == 0) {
183  o = meshToGeom<OBBRSS>(argv[iarg + 2]);
184  OUT("Mesh has " << dynamic_pointer_cast<BVHModel<OBBRSS> >(o)->num_tris
185  << " triangles");
186  type = "mesh_obbrss";
187  } else
188  throw std::invalid_argument("BV type must be obb or obbrss");
189  OUT("done.");
190  iarg += 3;
191  if (iarg < argc && strcmp(argv[iarg], "crop") == 0) {
192  CHECK_PARAM_NB(6, Crop);
193  hpp::fcl::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]),
194  atof(argv[iarg + 3])),
195  Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]),
196  atof(argv[iarg + 6])));
197  OUT("Cropping " << aabb.min_.transpose() << " ---- "
198  << aabb.max_.transpose() << " ...");
199  o->computeLocalAABB();
200  OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- "
201  << o->aabb_local.max_.transpose() << " ...");
202  o.reset(extract(o.get(), Transform3f(), aabb));
203  if (!o) throw std::invalid_argument("Failed to crop.");
204  OUT("Crop has " << dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris
205  << " triangles");
206  iarg += 7;
207  }
208  } else if (a == "-capsule") {
210  type = "capsule";
211  } else if (a == "-cone") {
212  CREATE_SHAPE_2(o, Cone);
213  type = "cone";
214  } else if (a == "-cylinder") {
216  type = "cylinder";
217  } else {
218  throw std::invalid_argument(std::string("Unknown argument: ") + a);
219  }
220  return Geometry(type, o);
221 }
222 
223 int main(int argc, char** argv) {
224  std::vector<Transform3f> transforms;
225 
226  CollisionRequest request;
227 
228  if (argc > 1) {
229  int iarg = 1;
230  handleParam(iarg, argc, argv, request);
231  Geometry first = makeGeomFromParam(iarg, argc, argv);
232  Geometry second = makeGeomFromParam(iarg, argc, argv);
233 
237  Results results(Ntransform);
238  collide(transforms, first.o.get(), second.o.get(), request, results);
239  printResults(first, second, results);
240  } else {
243  boost::filesystem::path path(TEST_RESOURCES_DIR);
244 
245  std::vector<Geometry> geoms;
246  geoms.push_back(Geometry("Box", new Box(1, 2, 3)));
247  geoms.push_back(Geometry("Sphere", new Sphere(2)));
248  geoms.push_back(Geometry("Capsule", new Capsule(2, 1)));
249  geoms.push_back(Geometry("Cone", new Cone(2, 1)));
250  geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1)));
251  // geoms.push_back(Geometry ("Plane" , new Plane
252  // (Vec3f(1,1,1).normalized(), 0) ));
253  // geoms.push_back(Geometry ("Halfspace" , new Halfspace
254  // (Vec3f(1,1,1).normalized(), 0) ));
255  // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP
256  // (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0)) ));
257 
258  geoms.push_back(Geometry("rob BVHModel<OBB>",
259  objToGeom<OBB>((path / "rob.obj").string())));
260  // geoms.push_back(Geometry ("rob BVHModel<RSS>" , objToGeom<RSS> ((path
261  // / "rob.obj").string()))); geoms.push_back(Geometry ("rob BVHModel<kIOS>"
262  // , objToGeom<kIOS> ((path / "rob.obj").string())));
263  geoms.push_back(Geometry("rob BVHModel<OBBRSS>",
264  objToGeom<OBBRSS>((path / "rob.obj").string())));
265 
266  geoms.push_back(Geometry("env BVHModel<OBB>",
267  objToGeom<OBB>((path / "env.obj").string())));
268  // geoms.push_back(Geometry ("env BVHModel<RSS>" , objToGeom<RSS> ((path
269  // / "env.obj").string()))); geoms.push_back(Geometry ("env BVHModel<kIOS>"
270  // , objToGeom<kIOS> ((path / "env.obj").string())));
271  geoms.push_back(Geometry("env BVHModel<OBBRSS>",
272  objToGeom<OBBRSS>((path / "env.obj").string())));
273 
275 
276  // collision
277  for (std::size_t i = 0; i < geoms.size(); ++i) {
278  for (std::size_t j = i; j < geoms.size(); ++j) {
279  if (!supportedPair(geoms[i].o.get(), geoms[j].o.get())) continue;
280  Results results(Ntransform);
281  collide(transforms, geoms[i].o.get(), geoms[j].o.get(), request,
282  results);
283  printResults(geoms[i], geoms[j], results);
284  }
285  }
286  }
287 
288  return 0;
289 }
Ntransform
size_t Ntransform
Definition: profiling.cpp:116
Geometry::o
CollisionGeometryPtr_t o
Definition: profiling.cpp:71
collision_func_matrix.h
supportedPair
bool supportedPair(const CollisionGeometry *o1, const CollisionGeometry *o2)
Definition: profiling.cpp:33
Eigen
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
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
doxygen_xml_parser.type
type
Definition: doxygen_xml_parser.py:882
hpp::fcl::CollisionFunctionMatrix
collision matrix stores the functions for collision between different types of objects and provides a...
Definition: collision_func_matrix.h:51
CREATE_SHAPE_2
#define CREATE_SHAPE_2(var, Name)
Definition: profiling.cpp:153
hpp::fcl::loadPolyhedronFromResource
void loadPolyhedronFromResource(const std::string &resource_path, const fcl::Vec3f &scale, const shared_ptr< BVHModel< BoundingVolume > > &polyhedron)
Read a mesh file and convert it to a polyhedral mesh.
Definition: assimp.h:118
hpp::fcl::extract
HPP_FCL_DLLAPI CollisionGeometry * extract(const CollisionGeometry *model, const Transform3f &pose, const AABB &aabb)
Definition: collision_utility.cpp:68
hpp::fcl::CollisionRequest::enable_distance_lower_bound
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
Definition: collision_data.h:244
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::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::AABB::min_
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
meshToGeom
CollisionGeometryPtr_t meshToGeom(const std::string &filename, const Vec3f &scale=Vec3f(1, 1, 1))
Definition: profiling.cpp:62
limit
FCL_REAL limit
Definition: profiling.cpp:120
utility.h
hpp::fcl::AABB::max_
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
hpp::fcl::CollisionGeometry::getObjectType
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:128
Geometry::Geometry
Geometry(const std::string &t, const CollisionGeometryPtr_t &ob)
Definition: profiling.cpp:74
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
sep
const char * sep
Definition: profiling.cpp:99
hpp::fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
hpp::fcl::OT_BVH
@ OT_BVH
Definition: collision_object.h:55
a
list a
narrowphase.h
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::OT_GEOM
@ OT_GEOM
Definition: collision_object.h:56
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
Results
Definition: profiling.cpp:78
hpp::fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
hpp::fcl::BenchTimer::stop
void stop()
stop the timer
Definition: utility.cpp:44
collision-bench.filename
filename
Definition: collision-bench.py:6
CHECK_PARAM_NB
#define CHECK_PARAM_NB(NB, NAME)
Definition: profiling.cpp:125
objToGeom
CollisionGeometryPtr_t objToGeom(const std::string &filename)
Definition: profiling.cpp:46
handleParam
void handleParam(int &iarg, const int &argc, char **argv, CollisionRequest &request)
Definition: profiling.cpp:128
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::CollisionFunctionMatrix::collision_matrix
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]
each item in the collision matrix is a function to handle collision between objects of type1 and type...
Definition: collision_func_matrix.h:71
Geometry::type
std::string type
Definition: profiling.cpp:70
OUT
#define OUT(x)
Definition: profiling.cpp:123
t
tuple t
makeGeomFromParam
Geometry makeGeomFromParam(int &iarg, const int &argc, char **argv)
Definition: profiling.cpp:157
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
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
printResultHeaders
void printResultHeaders()
Definition: profiling.cpp:101
hpp::fcl::BenchTimer
Definition: utility.h:82
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
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
Results::Results
Results(size_t i)
Definition: profiling.cpp:82
Geometry
Definition: profiling.cpp:69
BVH_model.h
hpp::fcl::CollisionGeometry::getNodeType
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:131
assimp.h
fwd.hh
Results::times
Eigen::VectorXd times
Definition: profiling.cpp:80
Results::rs
std::vector< CollisionResult > rs
Definition: profiling.cpp:79
verbose
bool verbose
Definition: profiling.cpp:121
geometric_shapes.h
Geometry::Geometry
Geometry(const std::string &t, CollisionGeometry *ob)
Definition: profiling.cpp:73
lookupTable
CollisionFunctionMatrix lookupTable
Definition: profiling.cpp:32
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
collision_utility.h
hpp::fcl::BenchTimer::start
void start()
start timer
Definition: utility.cpp:35
collision.h
main
int main(int argc, char **argv)
Definition: profiling.cpp:223
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
printResults
void printResults(const Geometry &g1, const Geometry &g2, const Results &rs)
Definition: profiling.cpp:107


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