Collision-detection against some meshes could be much faster #2534
Labels
collision-detection
enhancement
New feature or request
stale
Inactive issues and PRs are marked as stale and may be closed automatically.
Comments
werner291
added a commit
to werner291/Multigoal-Orchard-Drone-Planning-Library
that referenced
this issue
Nov 15, 2023
Signed-off-by: Werner Kroneman <[email protected]>
|
Here's a MWE: #include <chrono>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/geometry/shape/box.h>
#include <fcl/narrowphase/collision-inl.h>
#include <fcl/narrowphase/collision_object.h>
#include <fcl/narrowphase/collision_request.h>
#include <random_numbers/random_numbers.h>
int main(int argc, char** argv)
{
std::shared_ptr<fcl::BVHModel<fcl::OBBd>> without_RSS = std::make_shared<fcl::BVHModel<fcl::OBBd>>();
std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> with_RSS = std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>();
random_numbers::RandomNumberGenerator rng(42);
const size_t N_TRIANGLES = 30000;
const double radius = 5.0;
without_RSS->beginModel();
with_RSS->beginModel();
for (size_t i = 0; i < N_TRIANGLES; i)
{
fcl::Vector3d a(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
fcl::Vector3d b(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
fcl::Vector3d c(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
without_RSS->addTriangle(a, b, c);
with_RSS->addTriangle(a, b, c);
}
with_RSS->endModel();
without_RSS->endModel();
fcl::CollisionObjectd co_without(without_RSS, fcl::Transform3d::Identity());
fcl::CollisionObjectd co_with(with_RSS, fcl::Transform3d::Identity());
const auto box = std::make_shared<fcl::Boxd>(1.0, 0.1, 1.0);
size_t N_SAMPLES = 1000;
long time_without = 0;
long time_with = 0;
for (size_t sample_i = 0; sample_i < N_SAMPLES; sample_i)
{
// Generate a random transform.
fcl::Transform3d tf = fcl::Translation3d(rng.uniformReal(-radius, radius), rng.uniformReal(-radius, radius), rng.uniformReal(-radius, radius)) *
fcl::Quaterniond(rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0)).normalized();
fcl::CollisionObjectd co(box, tf);
bool c1 = false;
bool c2 = false;
auto time_1 = std::chrono::high_resolution_clock::now();
{
fcl::CollisionRequestd req;
fcl::CollisionResultd res;
fcl::collide(&co_without, &co, req, res);
c1 = res.isCollision();
}
auto time_2 = std::chrono::high_resolution_clock::now();
{
fcl::CollisionRequestd req;
fcl::CollisionResultd res;
fcl::collide(&co_with, &co, req, res);
c2 = res.isCollision();
}
auto time_3 = std::chrono::high_resolution_clock::now();
time_without = std::chrono::duration_cast<std::chrono::nanoseconds>(time_2 - time_1).count();
time_with = std::chrono::duration_cast<std::chrono::nanoseconds>(time_3 - time_2).count();
assert(c1 == c2);
}
std::cout << "Time without RSS: " << time_without << std::endl;
std::cout << "Time with RSS: " << time_with << std::endl;
std::cout << "Ratio: " << (double)time_without / (double)time_with << std::endl;
}Output on my system (with That's almost a 90% improvement (with a bit of an artificial example, admittedly, but still...) I'll see if I can get some more stats. |
|
Another example that varies the density and size of the triangle mesh: #include <chrono>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/geometry/shape/box.h>
#include <fcl/narrowphase/collision-inl.h>
#include <fcl/narrowphase/collision_object.h>
#include <fcl/narrowphase/collision_request.h>
#include <random_numbers/random_numbers.h>
int main(int argc, char** argv)
{
random_numbers::RandomNumberGenerator rng(42);
for (const double radius: {0.5, 1.0, 2.0, 5.0, 10.0})
{
for (const int n_triangles: { 10, 100, 1000, 10000, 100000 })
{
std::shared_ptr<fcl::BVHModel<fcl::OBBd>> without_RSS = std::make_shared<fcl::BVHModel<fcl::OBBd>>();
std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> with_RSS = std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>();
without_RSS->beginModel();
with_RSS->beginModel();
for (size_t i = 0; i < n_triangles; i)
{
fcl::Vector3d a(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
fcl::Vector3d b(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
fcl::Vector3d c(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
without_RSS->addTriangle(a, b, c);
with_RSS->addTriangle(a, b, c);
}
with_RSS->endModel();
without_RSS->endModel();
fcl::CollisionObjectd co_without(without_RSS, fcl::Transform3d::Identity());
fcl::CollisionObjectd co_with(with_RSS, fcl::Transform3d::Identity());
const auto box = std::make_shared<fcl::Boxd>(1.0, 1.0, 1.0);
size_t N_SAMPLES = 1000;
long time_without = 0;
long time_with = 0;
for (size_t sample_i = 0; sample_i < N_SAMPLES; sample_i)
{
// Generate a random transform.
fcl::Transform3d tf = fcl::Translation3d(rng.uniformReal(-radius, radius), rng.uniformReal(-radius, radius), rng.uniformReal(-radius, radius)) *
fcl::Quaterniond(rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0)).normalized();
fcl::CollisionObjectd co(box, tf);
bool c1 = false;
bool c2 = false;
auto time_1 = std::chrono::high_resolution_clock::now();
{
fcl::CollisionRequestd req;
fcl::CollisionResultd res;
fcl::collide(&co_without, &co, req, res);
c1 = res.isCollision();
}
auto time_2 = std::chrono::high_resolution_clock::now();
{
fcl::CollisionRequestd req;
fcl::CollisionResultd res;
fcl::collide(&co_with, &co, req, res);
c2 = res.isCollision();
}
auto time_3 = std::chrono::high_resolution_clock::now();
time_without = std::chrono::duration_cast<std::chrono::nanoseconds>(time_2 - time_1).count();
time_with = std::chrono::duration_cast<std::chrono::nanoseconds>(time_3 - time_2).count();
assert(c1 == c2);
}
std::cout << "Radius: " << radius << std::endl;
std::cout << "N triangles: " << n_triangles << std::endl;
std::cout << "Time without RSS: " << time_without << std::endl;
std::cout << "Time with RSS: " << time_with << std::endl;
std::cout << "Ratio: " << (double)time_without / (double)time_with << std::endl;
std::cout << std::endl;
}
}
}Here's the output: ...what is going on? That's well over a 90% speedup; far more in some cases. And yes, it extends to smaller meshes, it seems. |
werner291
changed the title
|
Some statistics:
Looking at the profiler results... |
|
This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups. |
github-actions
bot
added
the
stale
|
@werner291 Sorry for the late answer, this issue slipped through 🙈
Yes! We have a monthly maintainer meeting (Next date is Jan 25, 5:00 - 6:00 pm CEST) and we'd be very interested to have a discussion around this and maybe hear your findings. Let us know if you can make it. Happy new year! |
github-actions
bot
removed
the
stale
|
This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups. |
github-actions
bot
added
the
stale
github-actions
bot
removed
the
stale
|
This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups. |
github-actions
bot
added
the
stale
|
This issue was closed because it has been stalled for 45 days with no activity. |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Hello,
in my PhD project, I am frequently collision-checking against fairly large (>38k triangles) triangle meshes (it's a model of an apple tree; my planning space is highly cluttered).
In Moveit's FCL collision-checking environment, meshes are typically represented as a
fcl::BVHModel<fcl::OBBRSSd>, whereOBBRSSdis essentially a combination of an oriented bounding box and a rectangle sphere-swept bounding volume.Since I was only collision-checking rather than measuring distances, I tried using ``fcl::BVHModelfcl::OBBd` instead, thus only using an OBB without the distance-computation acceleration data.
To my surprise, the collision check ran over 60-70% faster in my particular circumstances. (Note that most collision checks failed.)
Since collision-checking is often the main performance bottleneck in motion planning, would there be any interest in me investigating this further and preparing a minimum working example? Is it common to have such large triangle meshes representing obstacles?
I'll almost certainly be working with this more, but I'd like to know if there's interest in integrating this into Moveit proper.
The text was updated successfully, but these errors were encountered: