diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 023a64a5cdab1ca95da0ae91785b777a930a064e..528b25e03eb7c56671f90279246cfb13b1d6979d 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/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;
 }