Remove call to upper_bound. Reuse lower_bound

src/trajectory/trajectory_base.cpp

@@ -46,32 +46,25 @@ FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
     if(not frame_map_.empty())
     {
         //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
-        //In the std they fulfill the following properties:
+        //In the std they fulfill the following property:
         //        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.
+        // lb definition is NOT the ACTUAL lower bound but the following position so, lb = lb_true + 1.
+
         auto lower_bound = frame_map_.lower_bound(_ts);
-        auto upper_bound = frame_map_.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;
+        if((lower_bound != this->end() and lower_bound->first == _ts) or lower_bound == this->begin() ) closest_kf = lower_bound->second;
         else
         {
+            auto upper_bound = lower_bound;
+            //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);
+
             //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;
+            if(upper_bound == this->end()) closest_kf = lower_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);