Transforms Are Often Accumulated Through Entire Tree

[matt-attack]

I was looking through the code some and noticed something unexpected. The system always accumulates the final transform up to the root node and back when calculating the transform between two nodes that are not child and parent. I would have only expected it to do this up to the closest common parent.

While technically accurate, this introduces a bunch of unnecessary lookups and multiplications to the calculation of your final Transform. This both reduces accuracy and speed.

This can be seen here: https://github.com/ros/geometry2/blob/melodic-devel/tf2/src/buffer_core.cpp#L315

Though, older versions (9 years old) seem to crop off these redundant transforms before doing the actual multiplication as is most correct.

geometry2/tf2/src/buffer_core.cpp

Lines 708 to 718 in 1c6c387

	while (lookupFrameNumber(lists.inverseTransforms.back().child_frame_id) == lookupFrameNumber(lists.forwardTransforms.back().child_frame_id))
	{
	lists.inverseTransforms.pop_back();
	lists.forwardTransforms.pop_back();
	// Make sure we don't go beyond the beginning of the list.
	// (The while statement above doesn't fail if you hit the beginning of the list,
	// which happens in the zero distance case.)
	if (lists.inverseTransforms.size() == 0 || lists.forwardTransforms.size() == 0)
	break;
	}

I will note that the frame_chain is appropriately simplified even though the actual calculation isn't. https://github.com/ros/geometry2/blame/melodic-devel/tf2/src/buffer_core.cpp#L480

[tfoote]

The lookups were already being done because the tree traversal for connectivity already is doing the lookups.

And the accuracy loss with double precision computations requires tf frames of litteraly astronomical scale which are generally not recommended.

This restructure was the result of a significant focus on optimizing the throughput capabilities. In that process we improved throughput by several orders of magnitude of throughupt.

Although algebraically doing fewer operations seems like less computation, but the book keeping of which transforms to use is potentially almost as high. The linear math computations for chaining vectors and quaternions is something that cpus can do very quickly where as the branching logic is more expensive. You'll note that we only do the pruneback now if the user has asked for the frame listings.

I'd be happy to review a contribution along those lines.. But to show that it's worth it I'd want to see a benchmark which shows improvement in accuracy without costing speed or an improvement in speed with the same accuracy.