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 
234  FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit};
235  generateRandomTransforms(extents, transforms, Ntransform);
237  Results results(Ntransform);
238  collide(transforms, first.o.get(), second.o.get(), request, results);
239  printResults(first, second, results);
240  } else {
241  FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit};
242  generateRandomTransforms(extents, transforms, Ntransform);
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 }
HPP_FCL_DLLAPI CollisionGeometry * extract(const CollisionGeometry *model, const Transform3f &pose, const AABB &aabb)
void printResultHeaders()
Definition: profiling.cpp:101
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
bool verbose
Definition: profiling.cpp:121
Eigen::VectorXd times
Definition: profiling.cpp:80
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
size_t Ntransform
Definition: profiling.cpp:116
FCL_REAL limit
Definition: profiling.cpp:120
CollisionGeometryPtr_t meshToGeom(const std::string &filename, const Vec3f &scale=Vec3f(1, 1, 1))
Definition: profiling.cpp:62
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Cylinder along Z axis. The cylinder is defined at its centroid.
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: utility.cpp:96
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...
void stop()
stop the timer
Definition: utility.cpp:44
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
void handleParam(int &iarg, const int &argc, char **argv, CollisionRequest &request)
Definition: profiling.cpp:128
Results(size_t i)
Definition: profiling.cpp:82
std::vector< CollisionResult > rs
Definition: profiling.cpp:79
virtual NODE_TYPE getNodeType() const
get the node type
Geometry(const std::string &t, const CollisionGeometryPtr_t &ob)
Definition: profiling.cpp:74
request to the collision algorithm
double FCL_REAL
Definition: data_types.h:65
Center at zero point, axis aligned box.
virtual OBJECT_TYPE getObjectType() const
get the type of the object
FCL_REAL extents[6]
CollisionGeometryPtr_t objToGeom(const std::string &filename)
Definition: profiling.cpp:46
CollisionGeometryPtr_t o
Definition: profiling.cpp:71
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Cone The base of the cone is at and the top is at .
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
#define CHECK_PARAM_NB(NB, NAME)
Definition: profiling.cpp:125
void printResults(const Geometry &g1, const Geometry &g2, const Results &rs)
Definition: profiling.cpp:107
Center at zero point sphere.
CollisionFunctionMatrix lookupTable
Definition: profiling.cpp:32
Capsule It is where is the distance between the point x and the capsule segment AB...
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
#define CREATE_SHAPE_2(var, Name)
Definition: profiling.cpp:153
std::string type
Definition: profiling.cpp:70
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
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.
int main(int argc, char **argv)
Definition: profiling.cpp:223
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
void start()
start timer
Definition: utility.cpp:35
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
Definition: utility.cpp:54
Geometry makeGeomFromParam(int &iarg, const int &argc, char **argv)
Definition: profiling.cpp:157
Geometry(const std::string &t, CollisionGeometry *ob)
Definition: profiling.cpp:73
list a
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
bool supportedPair(const CollisionGeometry *o1, const CollisionGeometry *o2)
Definition: profiling.cpp:33
The geometry for the object for collision or distance computation.
const char * sep
Definition: profiling.cpp:99
#define OUT(x)
Definition: profiling.cpp:123
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
collision matrix stores the functions for collision between different types of objects and provides a...


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