Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-fcl
Commits
bc304f1e
Commit
bc304f1e
authored
May 10, 2017
by
Joseph Mirabel
Committed by
Joseph Mirabel
May 10, 2017
Browse files
Add profile test.
parent
a1edfb87
Changes
2
Hide whitespace changes
Inline
Side-by-side
test/CMakeLists.txt
View file @
bc304f1e
...
...
@@ -43,6 +43,8 @@ add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp test_fcl_utility.
add_fcl_test
(
test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp
)
add_fcl_test
(
test_fcl_profiling test_fcl_profiling.cpp test_fcl_utility.cpp
)
if
(
FCL_HAVE_OCTOMAP
)
add_fcl_test
(
test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp
)
PKG_CONFIG_USE_DEPENDENCY
(
test_fcl_octomap octomap
)
...
...
test/test_fcl_profiling.cpp
0 → 100644
View file @
bc304f1e
// 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>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<hpp/fcl/collision_func_matrix.h>
#include
<hpp/fcl/narrowphase/narrowphase.h>
#include
"test_fcl_utility.h"
#include
"fcl_resources/config.h"
using
namespace
fcl
;
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
);
}
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 Result {
// CollisionResult r;
// double time_us;
// };
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
;
}
// int main(int argc, char** argv)
int
main
(
int
,
char
**
)
{
boost
::
filesystem
::
path
path
(
TEST_RESOURCES_DIR
);
std
::
vector
<
Geometry
>
geoms
;
// | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
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
)
));
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
)
));
// 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
())));
std
::
vector
<
Transform3f
>
transforms
;
FCL_REAL
extents
[]
=
{
-
20
,
-
20
,
0
,
20
,
20
,
20
};
std
::
size_t
n
=
100
;
// bool verbose = false;
generateRandomTransforms
(
extents
,
transforms
,
n
);
printResultHeaders
();
// collision
CollisionRequest
request
;
// request.num_max_contacts = 1;
// request.enable_contact = false;
// request.enable_distance_lower_bound = false;
// request.num_max_cost_sources = 1;
// request.enable_cost = false;
// request.use_approximate_cost = true;
// request.gjk_solver_type = GST_INDEP;
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
(
n
);
collide
(
transforms
,
geoms
[
i
].
o
.
get
(),
geoms
[
j
].
o
.
get
(),
request
,
results
);
printResults
(
geoms
[
i
],
geoms
[
j
],
results
);
}
}
// request.enable_distance_lower_bound = true;
// for(std::size_t i = 0; i < geoms.size(); ++i)
// {
// Results results (n);
// selfCollide(transforms, geoms[i].o.get(), request, results);
// printResults(geoms[i], results);
// }
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment