Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	test/benchmark.cpp	Lines:	0	100	0.0 %
Date:	2024-02-09 12:57:42	Branches:	0	192	0.0 %


// Copyright (c) 2016, 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/filesystem.hpp>

#include <hpp/fcl/internal/traversal_node_setup.h>
#include <hpp/fcl/internal/traversal_node_bvhs.h>
#include "../src/collision_node.h"
#include <hpp/fcl/internal/BV_splitter.h>

#include "utility.h"
#include "fcl_resources/config.h"

#define RUN_CASE(BV, tf, models, split) \
  run<BV>(tf, models, split, #BV " - " #split ":\t")

using namespace hpp::fcl;

bool verbose = false;
FCL_REAL DELTA = 0.001;

template <typename BV>
void makeModel(const std::vector<Vec3f>& vertices,
               const std::vector<Triangle>& triangles,
               SplitMethodType split_method, BVHModel<BV>& model);

template <typename BV, typename TraversalNode>
double distance(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
                const BVHModel<BV>& m2, bool verbose);

template <typename BV, typename TraversalNode>
double collide(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
               const BVHModel<BV>& m2, bool verbose);

template <typename BV>
double run(const std::vector<Transform3f>& tf,
           const BVHModel<BV> (&models)[2][3], int split_method,
           const char* sm_name);

template <typename BV>
struct traits {};

template <>
struct traits<RSS> {
  typedef MeshCollisionTraversalNodeRSS CollisionTraversalNode;
  typedef MeshDistanceTraversalNodeRSS DistanceTraversalNode;
};

template <>
struct traits<kIOS> {
  typedef MeshCollisionTraversalNodekIOS CollisionTraversalNode;
  typedef MeshDistanceTraversalNodekIOS DistanceTraversalNode;
};

template <>
struct traits<OBB> {
  typedef MeshCollisionTraversalNodeOBB CollisionTraversalNode;
  // typedef MeshDistanceTraversalNodeOBB  DistanceTraversalNode;
};

template <>
struct traits<OBBRSS> {
  typedef MeshCollisionTraversalNodeOBBRSS CollisionTraversalNode;
  typedef MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode;
};

template <typename BV>
void makeModel(const std::vector<Vec3f>& vertices,
               const std::vector<Triangle>& triangles,
               SplitMethodType split_method, BVHModel<BV>& model) {
  model.bv_splitter.reset(new BVSplitter<BV>(split_method));
  model.bv_splitter.reset(new BVSplitter<BV>(split_method));

  model.beginModel();
  model.addSubModel(vertices, triangles);
  model.endModel();
}

template <typename BV, typename TraversalNode>
double distance(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
                const BVHModel<BV>& m2, bool verbose) {
  Transform3f pose2;

  DistanceResult local_result;
  DistanceRequest request(true);
  TraversalNode node;

  node.enable_statistics = verbose;

  BenchTimer timer;
  timer.start();

  for (std::size_t i = 0; i < tf.size(); ++i) {
    if (!initialize(node, m1, tf[i], m2, pose2, request, local_result))
      std::cout << "initialize error" << std::endl;

    distance(&node, NULL);
  }
  timer.stop();
  return timer.getElapsedTimeInMicroSec();
}

template <typename BV, typename TraversalNode>
double collide(const std::vector<Transform3f>& tf, const BVHModel<BV>& m1,
               const BVHModel<BV>& m2, bool verbose) {
  Transform3f pose2;

  CollisionResult local_result;
  CollisionRequest request;
  TraversalNode node(request);

  node.enable_statistics = verbose;

  BenchTimer timer;
  timer.start();

  for (std::size_t i = 0; i < tf.size(); ++i) {
    bool success(initialize(node, m1, tf[i], m2, pose2, local_result));
    (void)success;
    assert(success);

    CollisionResult result;
    collide(&node, request, result);
  }

  timer.stop();
  return timer.getElapsedTimeInMicroSec();
}

template <typename BV>
double run(const std::vector<Transform3f>& tf,
           const BVHModel<BV> (&models)[2][3], int split_method,
           const char* prefix) {
  double col = collide<BV, typename traits<BV>::CollisionTraversalNode>(
      tf, models[0][split_method], models[1][split_method], verbose);
  double dist = distance<BV, typename traits<BV>::DistanceTraversalNode>(
      tf, models[0][split_method], models[1][split_method], verbose);

  std::cout << prefix << " (" << col << ", " << dist << ")\n";
  return col + dist;
}

template <>
double run<OBB>(const std::vector<Transform3f>& tf,
                const BVHModel<OBB> (&models)[2][3], int split_method,
                const char* prefix) {
  double col = collide<OBB, traits<OBB>::CollisionTraversalNode>(
      tf, models[0][split_method], models[1][split_method], verbose);
  double dist = 0;

  std::cout << prefix << " (\t" << col << ", \tNaN)\n";
  return col + dist;
}

int main(int, char*[]) {
  std::vector<Vec3f> p1, p2;
  std::vector<Triangle> t1, t2;
  boost::filesystem::path path(TEST_RESOURCES_DIR);
  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);

  // Make models
  BVHModel<RSS> ms_rss[2][3];
  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_rss[0][SPLIT_METHOD_MEAN]);
  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_rss[0][SPLIT_METHOD_BV_CENTER]);
  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_rss[0][SPLIT_METHOD_MEDIAN]);
  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_rss[1][SPLIT_METHOD_MEAN]);
  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_rss[1][SPLIT_METHOD_BV_CENTER]);
  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_rss[1][SPLIT_METHOD_MEDIAN]);

  BVHModel<kIOS> ms_kios[2][3];
  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_kios[0][SPLIT_METHOD_MEAN]);
  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_kios[0][SPLIT_METHOD_BV_CENTER]);
  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_kios[0][SPLIT_METHOD_MEDIAN]);
  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_kios[1][SPLIT_METHOD_MEAN]);
  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_kios[1][SPLIT_METHOD_BV_CENTER]);
  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_kios[1][SPLIT_METHOD_MEDIAN]);

  BVHModel<OBB> ms_obb[2][3];
  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obb[0][SPLIT_METHOD_MEAN]);
  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_obb[0][SPLIT_METHOD_BV_CENTER]);
  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obb[0][SPLIT_METHOD_MEDIAN]);
  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obb[1][SPLIT_METHOD_MEAN]);
  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_obb[1][SPLIT_METHOD_BV_CENTER]);
  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obb[1][SPLIT_METHOD_MEDIAN]);

  BVHModel<OBBRSS> ms_obbrss[2][3];
  makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obbrss[0][SPLIT_METHOD_MEAN]);
  makeModel(p1, t1, SPLIT_METHOD_BV_CENTER,
            ms_obbrss[0][SPLIT_METHOD_BV_CENTER]);
  makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obbrss[0][SPLIT_METHOD_MEDIAN]);
  makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obbrss[1][SPLIT_METHOD_MEAN]);
  makeModel(p2, t2, SPLIT_METHOD_BV_CENTER,
            ms_obbrss[1][SPLIT_METHOD_BV_CENTER]);
  makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]);

  std::vector<Transform3f> transforms;  // t0
  FCL_REAL extents[] = {-3000, -3000, -3000, 3000, 3000, 3000};
  std::size_t n = 10000;

  generateRandomTransforms(extents, transforms, n);
  double total_time = 0;

  total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEAN);
  total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_BV_CENTER);
  total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEDIAN);

  total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEAN);
  total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_BV_CENTER);
  total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEDIAN);

  total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEAN);
  total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_BV_CENTER);
  total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEDIAN);

  total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEAN);
  total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_BV_CENTER);
  total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEDIAN);

  std::cout << "\n\nTotal time: " << total_time << std::endl;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
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			}