Collision-detection against some meshes could be much faster

[werner291]

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>, where OBBRSSd is 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.

[werner291]

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 -g -O3) reads:

Time without RSS: 141253
Time with RSS:    1136158
Ratio: 0.124325

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.

[werner291]

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:

Radius: 0.5
N triangles: 10
Time without RSS: 248093
Time with RSS:    2431348
Ratio: 0.102039

Radius: 0.5
N triangles: 100
Time without RSS: 212672
Time with RSS:    11057394
Ratio: 0.0192335

Radius: 0.5
N triangles: 1000
Time without RSS: 282922
Time with RSS:    101410271
Ratio: 0.00278988

Radius: 0.5
N triangles: 10000
Time without RSS: 346399
Time with RSS:    1022417217
Ratio: 0.000338804

Radius: 0.5
N triangles: 100000
Time without RSS: 794320
Time with RSS:    10123470660
Ratio: 7.84632e-05

Radius: 1
N triangles: 10
Time without RSS: 107144
Time with RSS:    1398480
Ratio: 0.0766146

Radius: 1
N triangles: 100
Time without RSS: 117513
Time with RSS:    1970541
Ratio: 0.0596349

Radius: 1
N triangles: 1000
Time without RSS: 122698
Time with RSS:    12056856
Ratio: 0.0101766

Radius: 1
N triangles: 10000
Time without RSS: 124152
Time with RSS:    108524318
Ratio: 0.001144

Radius: 1
N triangles: 100000
Time without RSS: 179818
Time with RSS:    1000385135
Ratio: 0.000179749

Radius: 2
N triangles: 10
Time without RSS: 92321
Time with RSS:    1214556
Ratio: 0.0760121

Radius: 2
N triangles: 100
Time without RSS: 91340
Time with RSS:    1237976
Ratio: 0.0737817

Radius: 2
N triangles: 1000
Time without RSS: 96210
Time with RSS:    3445918
Ratio: 0.02792

Radius: 2
N triangles: 10000
Time without RSS: 100404
Time with RSS:    26001437
Ratio: 0.00386148

Radius: 2
N triangles: 100000
Time without RSS: 104882
Time with RSS:    257522707
Ratio: 0.000407273

Radius: 5
N triangles: 10
Time without RSS: 88668
Time with RSS:    1130016
Ratio: 0.0784661

Radius: 5
N triangles: 100
Time without RSS: 88257
Time with RSS:    1086269
Ratio: 0.0812478

Radius: 5
N triangles: 1000
Time without RSS: 88406
Time with RSS:    1388079
Ratio: 0.0636895

Radius: 5
N triangles: 10000
Time without RSS: 90190
Time with RSS:    5503770
Ratio: 0.0163869

Radius: 5
N triangles: 100000
Time without RSS: 91365
Time with RSS:    1129898
Ratio: 0.0808613

Radius: 10
N triangles: 10
Time without RSS: 111664
Time with RSS:    1101888
Ratio: 0.101339

Radius: 10
N triangles: 100
Time without RSS: 87337
Time with RSS:    1086152
Ratio: 0.0804096

Radius: 10
N triangles: 1000
Time without RSS: 87752
Time with RSS:    1089151
Ratio: 0.0805692

Radius: 10
N triangles: 10000
Time without RSS: 89306
Time with RSS:    1109825
Ratio: 0.0804685

Radius: 10
N triangles: 100000
Time without RSS: 90237
Time with RSS:    1116779
Ratio: 0.0808011

...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]

Some statistics:

• The speedup is greater for larger meshes (definitely non-linear in the number of triangles)
• Ratio also seems strongly dependent on the radius (thus to the density of triangles); speedup seems more pronounced at higher densities.

Looking at the profiler results... ~~I'm almost getting the impression that the OBB-tree is a lot shallower than the OBBRSS-tree~~. (That last bit was wrong; the trees appear to be identical in the number of BVs.)

[github-actions[bot]]

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.

[sjahr]

@werner291 Sorry for the late answer, this issue slipped through :see_no_evil:

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?

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

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

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.