scene_graph: Broadphase culling may be insufficient (see profiling results of the geometry pipeline)

[amcastro-tri]

Description

From #8942 we see that the contact queries comprise about 25% of the total cost. This is a very simple case for which most of the simulation 16 spheres (2 pads modeled with 8 spheres each) collide against a single cylinder (a simple mug model). That is, there are 16 calls to sphereCylinderIntersect() per time step.

Reproduction

Instructions on how to reproduce these profiling results are documented in #8942.

Profiling Results

In a computation with 20,000 time steps, 26% of the cost is spent in the computation of geometric queries starting at MBP::CalcPointPairPenetrations() (called once per time step, e.g., 20,000 invocations).

The breakdown is quite complex given the large stack of calls. However the big take away is the cost in actual computation of contact information:

• fcl::detail::sphereCylinderIntersect<double>(): 0.57%, called 320,000 times (16 x 20,000 steps)

That is most of our 26% is overhead.

Remarks

drake::geometry::internal::(anonymous namespace)::CollisionFilterLegacy::CanCollideWith() (on Drake's side, not FCL) is taking alone 9.8% of the total cost that is, 40% of the contact pipeline.
Another important data point is that it gets called a total of 14M times over the 20K time steps (verified that each time step performs a single call to ProximityEngine::ComputePointPairPenetration()).

Action Items

• Identify the three most time consuming calls in the pipeline.
• Fix each item one at a time in order of importance.
• Profile the example again after these improvements are introduced to find out next bottleneck in the chain.

[SeanCurtis-TRI]

It's also worth drawing a line between the drake side of the code and the fcl side of the code. The entry point is MBP::CalcPointPairPenetrations() which calls SG which calls FCL. It'd be good to see the distribution of cost between drake and FCL.

[SeanCurtis-TRI]

Off of master, I was able to reproduce this. For the 8 spheres per ring, I was able to get consistent results of QueryObject::ComputePointPairPenetration() at ~30% of the calls. I further confirmed that the number of calls to the collision filtering was exactly all of the potential pairs always. Literally, nothing was being culled by the broadphase, ever.

The answer proved to be simple and is shown in the related PR.