Performance improvement for lower/upper bound

[zivsha]

Changed updateQueries() to use member functions of multiset instead of std::lower_bound and std::upper_bound

Without this change, playing a bag file containing over 200K messages (under a single topic) yields in a major performance hit.
Running performance diagnostic shows that most of the time is spent in std::advance and std::distance which are used inside std::lower_bound and std::upper_bound.

From cppreference - lower_bound:

Complexity
The number of comparisons performed is logarithmic in the distance between first and last (At most log2(last - first) + O(1) comparisons). However, for non-RandomAccessIterators, the number of iterator increments is linear.

Changing to std::multiset<IndexEntry>::lower_bound results in a major performance improvement.
I have not measure the improvement, but I have a bag file containing a video of a stopwatch. In my code, for each sensor_msgs::Image I try to find a matching (single) message.Just by looking at the playback, I can see that the FPS is now fast enough to show a real second, whereas without this change, 1 second in the video (time it took for the stop watch to advanced by 1 second) took about 17 seconds in the real world!

[racko]

Huh ... multiset::lower_bound does a tree search, which is "obviously efficient", while doing a (generic) binary search on the iterators of a tree structure is a bad idea: Even computing the mid of the range requires you to iterate over all elements to count them, which makes this operation linear instead of logarithmic :)

[clalancette]

This code looks OK to me, and seems to be an obvious performance improvement. @mikepurvis any thoughts?

[dirk-thomas]

Thank you for the patch.