Frames keep original order if equal timestamp

src/trajectory_base.cpp

@@ -66,7 +66,7 @@ void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
 FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
 {
     for (auto frm_rit = getFrameListPtr()->rbegin(); frm_rit != getFrameListPtr()->rend(); frm_rit++)
-        if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() < _frame_ptr->getTimeStamp())
+        if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
             return frm_rit.base();
     return getFrameListPtr()->begin();
 }

src/trajectory_base.h

@@ -55,7 +55,7 @@ class TrajectoryBase : public NodeBase
         FrameBasePtr getLastKeyFramePtr();
         FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
         void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr);
-        void sortFrame(FrameBasePtr _frame_iter);
+        void sortFrame(FrameBasePtr _frm_ptr);
         void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
         FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);