From 80808dc0c9754cfa9b387e0816443da3086f5709 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 17 Jun 2020 17:18:17 +0200
Subject: [PATCH] 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 | 50 ++++++++++++++++++++++++------
1 file changed, 40 insertions(+), 10 deletions(-)
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 023a64a5c..528b25e03 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;
}
--
GitLab