profiling.cpp 10.3 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-fcl.
// hpp-fcl is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-fcl is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.


#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>

#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/BVH/BVH_model.h>
24
#include <hpp/fcl/collision_utility.h>
Joseph Mirabel's avatar
Joseph Mirabel committed
25
26
27
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/collision_func_matrix.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
Joseph Mirabel's avatar
Joseph Mirabel committed
28
#include <hpp/fcl/mesh_loader/assimp.h>
29
#include "utility.h"
Joseph Mirabel's avatar
Joseph Mirabel committed
30
31
#include "fcl_resources/config.h"

32
using namespace hpp::fcl;
Joseph Mirabel's avatar
Joseph Mirabel committed
33

Lucile Remigy's avatar
Lucile Remigy committed
34
CollisionFunctionMatrix lookupTable;
Joseph Mirabel's avatar
Joseph Mirabel committed
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2)
{
  OBJECT_TYPE object_type1 = o1->getObjectType();
  OBJECT_TYPE object_type2 = o2->getObjectType();
  NODE_TYPE node_type1 = o1->getNodeType();
  NODE_TYPE node_type2 = o2->getNodeType();

  if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
    return (lookupTable.collision_matrix[node_type2][node_type1] != NULL);
  else
    return (lookupTable.collision_matrix[node_type1][node_type2] != NULL);
}

template <typename BV/*, SplitMethodType split_method*/>
CollisionGeometryPtr_t objToGeom (const std::string& filename)
{
  std::vector<Vec3f> pt;
  std::vector<Triangle> tri;
  loadOBJFile(filename.c_str(), pt, tri);

  BVHModel<BV>* model (new BVHModel<BV>());
  // model->bv_splitter.reset(new BVSplitter<BV>(split_method));

  model->beginModel();
  model->addSubModel(pt, tri);
  model->endModel();
  
  return CollisionGeometryPtr_t (model);
}

Joseph Mirabel's avatar
Joseph Mirabel committed
65
66
67
68
69
70
71
72
template <typename BV/*, SplitMethodType split_method*/>
CollisionGeometryPtr_t meshToGeom (const std::string& filename, const Vec3f& scale = Vec3f(1, 1, 1))
{
  boost::shared_ptr< BVHModel<BV> > model (new BVHModel<BV>());
  loadPolyhedronFromResource(filename, scale, model);
  return model;
}

Joseph Mirabel's avatar
Joseph Mirabel committed
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
struct Geometry {
  std::string type;
  CollisionGeometryPtr_t o;

  Geometry (const std::string& t, CollisionGeometry* ob) : 
    type(t), o(ob)
  {}
  Geometry (const std::string& t, const CollisionGeometryPtr_t& ob) : 
    type(t), o(ob)
  {}
};

struct Results {
  std::vector<CollisionResult> rs;
  Eigen::VectorXd times; // micro-seconds

  Results (int i) : rs(i), times(i) {}
};

void collide(const std::vector<Transform3f>& tf,
             const CollisionGeometry* o1,
             const CollisionGeometry* o2,
             const CollisionRequest& request,
             Results& results)
{
  Transform3f Id;
  Timer timer;
  for (std::size_t i = 0; i < tf.size(); ++i) {
    timer.start();
    /* int num_contact = */
    collide (o1, tf[i], o2, Id, request, results.rs[i]);
    timer.stop();
    results.times[i] = timer.getElapsedTimeInMicroSec();
  }
}

const char* sep = ", ";

void printResultHeaders ()
{
  std::cout << "Type 1" << sep << "Type 2" << sep << "mean time" << sep << "time std dev" << sep << "min time" << sep << "max time" << std::endl;
}

void printResults (const Geometry& g1, const Geometry& g2, const Results& rs)
{
  double mean = rs.times.mean();
  double var = rs.times.cwiseAbs2().mean() - mean*mean;
  std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var) << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff() << std::endl;
}

123
124
125
#ifndef NDEBUG // if debug mode
int Ntransform = 1;
#else
Joseph Mirabel's avatar
Joseph Mirabel committed
126
int Ntransform = 100;
127
#endif
Joseph Mirabel's avatar
Joseph Mirabel committed
128
FCL_REAL limit = 20;
129
bool verbose = false;
Joseph Mirabel's avatar
Joseph Mirabel committed
130

131
#define OUT(x) if (verbose) std::cout << x << std::endl
Joseph Mirabel's avatar
Joseph Mirabel committed
132
133
#define CHECK_PARAM_NB(NB, NAME) \
    if (iarg + NB >= argc) throw std::invalid_argument(#NAME " requires " #NB " numbers")
Joseph Mirabel's avatar
Joseph Mirabel committed
134
135
136
137
138
139
140
void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& request)
{
  while (iarg < argc) {
    std::string a(argv[iarg]);
    if (a == "-nb_transform") {
      CHECK_PARAM_NB(1, nb_transform);
      Ntransform = atoi(argv[iarg+1]);
141
      OUT("nb_transform = " << Ntransform);
Joseph Mirabel's avatar
Joseph Mirabel committed
142
143
144
145
146
147
148
149
150
      iarg += 2;
    } else if (a == "-enable_distance_lower_bound") {
      CHECK_PARAM_NB(1, enable_distance_lower_bound);
      request.enable_distance_lower_bound = bool(atoi(argv[iarg+1]));
      iarg += 2;
    } else if (a == "-limit") {
      CHECK_PARAM_NB(1, limit);
      limit = atof(argv[iarg+1]);
      iarg += 2;
151
152
153
    } else if (a == "-verbose") {
      verbose = true;
      iarg += 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
154
155
156
157
158
    } else {
      break;
    }
  }
}
Joseph Mirabel's avatar
Joseph Mirabel committed
159
160
161
162
#define CREATE_SHAPE_2(var, Name) \
    CHECK_PARAM_NB(2, Name); \
    var.reset(new Name (atof(argv[iarg+1]), atof(argv[iarg+2]))); \
     iarg += 3;
Joseph Mirabel's avatar
Joseph Mirabel committed
163
Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv)
Joseph Mirabel's avatar
Joseph Mirabel committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
{
  if (iarg >= argc) throw std::invalid_argument("An argument is required.");
  std::string a(argv[iarg]);
  std::string type;
  CollisionGeometryPtr_t o;
  if (a == "-box") {
    CHECK_PARAM_NB(3, Box);
    o.reset(new Box (atof(argv[iarg+1]),
                     atof(argv[iarg+2]),
                     atof(argv[iarg+3])));
    iarg += 4;
    type = "box";
  } else if (a == "-sphere") {
    CHECK_PARAM_NB(1, Sphere);
    o.reset(new Sphere (atof(argv[iarg+1])));
    iarg += 2;
    type = "sphere";
  } else if (a == "-mesh") {
    CHECK_PARAM_NB(2, Mesh);
183
    OUT("Loading " << argv[iarg+2] << " as BVHModel<" << argv[iarg+1] << ">...");
Joseph Mirabel's avatar
Joseph Mirabel committed
184
185
    if (strcmp(argv[iarg+1], "obb") == 0) {
      o = meshToGeom<OBB>(argv[iarg+2]);
186
      OUT("Mesh has " << boost::dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris << " triangles");
Joseph Mirabel's avatar
Joseph Mirabel committed
187
188
189
190
      type = "mesh_obb";
    }
    else if (strcmp(argv[iarg+1], "obbrss") == 0) {
      o = meshToGeom<OBBRSS>(argv[iarg+2]);
191
      OUT("Mesh has " << boost::dynamic_pointer_cast<BVHModel<OBBRSS> >(o)->num_tris << " triangles");
Joseph Mirabel's avatar
Joseph Mirabel committed
192
193
194
195
      type = "mesh_obbrss";
    }
    else
      throw std::invalid_argument ("BV type must be obb or obbrss");
196
    OUT("done.");
Joseph Mirabel's avatar
Joseph Mirabel committed
197
    iarg += 3;
198
199
    if (iarg < argc && strcmp(argv[iarg], "crop") == 0) {
      CHECK_PARAM_NB(6, Crop);
200
      hpp::fcl::AABB aabb(
201
202
203
204
205
206
207
208
209
210
211
212
213
214
          Vec3f(atof(argv[iarg+1]),
                atof(argv[iarg+2]),
                atof(argv[iarg+3])),
          Vec3f(atof(argv[iarg+4]),
                atof(argv[iarg+5]),
                atof(argv[iarg+6])));
      OUT("Cropping " << aabb.min_.transpose() << " ---- " << aabb.max_.transpose() << " ...");
      o->computeLocalAABB();
      OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- " << o->aabb_local.max_.transpose() << " ...");
      o.reset(extract(o.get(), Transform3f(), aabb));
      if (!o) throw std::invalid_argument ("Failed to crop.");
      OUT("Crop has " << boost::dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris << " triangles");
      iarg += 7;
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
  } else if (a == "-capsule") {
    CREATE_SHAPE_2(o, Capsule);
    type = "capsule";
  } else if (a == "-cone") {
    CREATE_SHAPE_2(o, Cone);
    type = "cone";
  } else if (a == "-cylinder") {
    CREATE_SHAPE_2(o, Cylinder);
    type = "cylinder";
  } else {
    throw std::invalid_argument (std::string("Unknown argument: ") + a);
  }
  return Geometry(type, o);
}

int main(int argc, char** argv)
Joseph Mirabel's avatar
Joseph Mirabel committed
231
232
233
234
{
  
  std::vector<Transform3f> transforms;

235
  CollisionRequest request;
Joseph Mirabel's avatar
Joseph Mirabel committed
236
237
238

  if (argc > 1) {
    int iarg = 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
239
240
241
242
243
244
    handleParam(iarg, argc, argv, request);
    Geometry first  = makeGeomFromParam(iarg, argc, argv);
    Geometry second = makeGeomFromParam(iarg, argc, argv);

    FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit};
    generateRandomTransforms(extents, transforms, Ntransform);
Joseph Mirabel's avatar
Joseph Mirabel committed
245
246
247
248
249
    printResultHeaders();
    Results results (Ntransform);
    collide(transforms, first.o.get(), second.o.get(), request, results);
    printResults(first, second, results);
  } else {
Joseph Mirabel's avatar
Joseph Mirabel committed
250
251
    FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit};
    generateRandomTransforms(extents, transforms, Ntransform);
Joseph Mirabel's avatar
Joseph Mirabel committed
252
253
254
255
256
257
258
259
    boost::filesystem::path path(TEST_RESOURCES_DIR);

    std::vector<Geometry> geoms;
    geoms.push_back(Geometry ("Box"       , new Box       (1, 2, 3)                                       ));
    geoms.push_back(Geometry ("Sphere"    , new Sphere    (2)                                             ));
    geoms.push_back(Geometry ("Capsule"   , new Capsule   (2, 1)                                          ));
    geoms.push_back(Geometry ("Cone"      , new Cone      (2, 1)                                          ));
    geoms.push_back(Geometry ("Cylinder"  , new Cylinder  (2, 1)                                          ));
260
261
    //geoms.push_back(Geometry ("Plane"     , new Plane     (Vec3f(1,1,1).normalized(), 0)                  ));
    //geoms.push_back(Geometry ("Halfspace" , new Halfspace (Vec3f(1,1,1).normalized(), 0)                  ));
Joseph Mirabel's avatar
Joseph Mirabel committed
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
    // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0))     ));

    geoms.push_back(Geometry ("rob BVHModel<OBB>"   , objToGeom<OBB>   ((path / "rob.obj").string())));
    // geoms.push_back(Geometry ("rob BVHModel<RSS>"   , objToGeom<RSS>   ((path / "rob.obj").string())));
    // geoms.push_back(Geometry ("rob BVHModel<kIOS>"  , objToGeom<kIOS>  ((path / "rob.obj").string())));
    geoms.push_back(Geometry ("rob BVHModel<OBBRSS>", objToGeom<OBBRSS>((path / "rob.obj").string())));

    geoms.push_back(Geometry ("env BVHModel<OBB>"   , objToGeom<OBB>   ((path / "env.obj").string())));
    // geoms.push_back(Geometry ("env BVHModel<RSS>"   , objToGeom<RSS>   ((path / "env.obj").string())));
    // geoms.push_back(Geometry ("env BVHModel<kIOS>"  , objToGeom<kIOS>  ((path / "env.obj").string())));
    geoms.push_back(Geometry ("env BVHModel<OBBRSS>", objToGeom<OBBRSS>((path / "env.obj").string())));

    printResultHeaders();

    // collision
    for(std::size_t i = 0; i < geoms.size(); ++i) {
      for (std::size_t j = i; j < geoms.size(); ++j) {
        if (!supportedPair(geoms[i].o.get(), geoms[j].o.get())) continue;
        Results results (Ntransform);
        collide(transforms, geoms[i].o.get(), geoms[j].o.get(), request, results);
        printResults(geoms[i], geoms[j], results);
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
284
285
286
    }
  }

Joseph Mirabel's avatar
Joseph Mirabel committed
287
  return 0;
Joseph Mirabel's avatar
Joseph Mirabel committed
288
}