Rework closestKeyFrameToTimeStamp O(n) -> O(log n)

Take advantage of the new map data structure to compute
closest keyframe in logarithmic time

src/trajectory/trajectory_base.cpp

@@ -41,20 +41,50 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
 
 FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
 {
-    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
     FrameBasePtr closest_kf = nullptr;
-    double min_dt = 1e9;
-
-    for (auto frm_rit = rbegin(); frm_rit != rend(); frm_rit++)
+    //If frame_list_ is empty then closestKeyFrameToTimeStamp is meaningless
+    if(not frame_list_.empty())
     {
-        double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
-        if (dt < min_dt)
+        //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
+        //In the std they fulfill the following properties:
+        //        lb is the first element such that ts <= lb, alternatively the smallest element that is NOT less than ts.
+        //        ub is the first element such that ts < lb.
+        // The ub definition is fine, and what one would expect. On the other hand the lb definition is NOT the ACTUAL lower bound but the following position
+        // so, lb = lb_true + 1.
+        auto lower_bound = frame_list_.lower_bound(_ts);
+        auto upper_bound = frame_list_.upper_bound(_ts);
+
+        //We get out of the way the easy cases. If lb is exactly the first element it can mean two things:
+        //    * _ts belongs to the "past", thus the closest frame is the first one
+        //    * _ts matches exactly the first frame
+        //either way we return lb.
+        //It could also be that we have hit jackpot and there is some frame whose timestamp is precisely _ts.
+        if(lower_bound == this->begin() or lower_bound->first == _ts) closest_kf = lower_bound->second;
+        else
         {
-            min_dt = dt;
-            closest_kf = *frm_rit;
+            //If ub points to end() it means that the last frame is still less than _ts, therefore certainly
+            //it will be the closest one
+            if(upper_bound == this->end()) closest_kf = std::prev(upper_bound)->second;
+            else
+            {
+                //I find it easier to reason if lb < ts < ub. Remember that we have got rid of the
+                //equality case and the out of bounds cases so this inequality is complete (it is not misssing cases).
+                //Therefore, we need to decrease the lower_bound to the previous element
+                lower_bound = std::prev(lower_bound);
+
+                //Easy stuff just calculate the distance return minimum
+                auto lb_diff = fabs(lower_bound->first - _ts);
+                auto ub_diff = fabs(upper_bound->first - _ts);
+                if(lb_diff < ub_diff)
+                {
+                    closest_kf = lower_bound->second;
+                }
+                else
+                {
+                    closest_kf = upper_bound->second;
+                }
+            }
         }
-        else
-            break;
     }
     return closest_kf;
 }

[Joan Solà Ortega]

You can save 50% of search time by avoiding one of these calls, e.g. that on L55. The other iterator will be equal or equal+1 depending on the time stamp matching that of the KF or not, so with a simple if() you can set upper_bound from the info in the time stamps and the lower_bound.