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
34
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

CollisionFunctionMatrix<GJKSolver_indep> lookupTable;
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;
}

Joseph Mirabel's avatar
Joseph Mirabel committed
123
int Ntransform = 100;
Joseph Mirabel's avatar
Joseph Mirabel committed
124
FCL_REAL limit = 20;
125
bool verbose = false;
Joseph Mirabel's avatar
Joseph Mirabel committed
126

127
#define OUT(x) if (verbose) std::cout << x << std::endl
Joseph Mirabel's avatar
Joseph Mirabel committed
128
129
#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
130
131
132
133
134
135
136
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]);
137
      OUT("nb_transform = " << Ntransform);
Joseph Mirabel's avatar
Joseph Mirabel committed
138
139
140
141
142
143
144
145
146
      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;
147
148
149
    } else if (a == "-verbose") {
      verbose = true;
      iarg += 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
150
151
152
153
154
    } else {
      break;
    }
  }
}
Joseph Mirabel's avatar
Joseph Mirabel committed
155
156
157
158
#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
159
Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv)
Joseph Mirabel's avatar
Joseph Mirabel committed
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
{
  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);
179
    OUT("Loading " << argv[iarg+2] << " as BVHModel<" << argv[iarg+1] << ">...");
Joseph Mirabel's avatar
Joseph Mirabel committed
180
181
    if (strcmp(argv[iarg+1], "obb") == 0) {
      o = meshToGeom<OBB>(argv[iarg+2]);
182
      OUT("Mesh has " << boost::dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris << " triangles");
Joseph Mirabel's avatar
Joseph Mirabel committed
183
184
185
186
      type = "mesh_obb";
    }
    else if (strcmp(argv[iarg+1], "obbrss") == 0) {
      o = meshToGeom<OBBRSS>(argv[iarg+2]);
187
      OUT("Mesh has " << boost::dynamic_pointer_cast<BVHModel<OBBRSS> >(o)->num_tris << " triangles");
Joseph Mirabel's avatar
Joseph Mirabel committed
188
189
190
191
      type = "mesh_obbrss";
    }
    else
      throw std::invalid_argument ("BV type must be obb or obbrss");
192
    OUT("done.");
Joseph Mirabel's avatar
Joseph Mirabel committed
193
    iarg += 3;
194
195
    if (iarg < argc && strcmp(argv[iarg], "crop") == 0) {
      CHECK_PARAM_NB(6, Crop);
196
      hpp::fcl::AABB aabb(
197
198
199
200
201
202
203
204
205
206
207
208
209
210
          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
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
  } 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
227
228
229
230
{
  
  std::vector<Transform3f> transforms;

231
  CollisionRequest request;
Joseph Mirabel's avatar
Joseph Mirabel committed
232
233
234

  if (argc > 1) {
    int iarg = 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
235
236
237
238
239
240
    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
241
242
243
244
245
    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
246
247
    FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit};
    generateRandomTransforms(extents, transforms, Ntransform);
Joseph Mirabel's avatar
Joseph Mirabel committed
248
249
250
251
252
253
254
255
    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)                                          ));
256
257
    //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
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
    // 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
280
281
282
    }
  }

Joseph Mirabel's avatar
Joseph Mirabel committed
283
  return 0;
Joseph Mirabel's avatar
Joseph Mirabel committed
284
}