sorting frames

src/trajectory_base.cpp

@@ -16,13 +16,16 @@ TrajectoryBase::~TrajectoryBase()
 
 FrameBase* TrajectoryBase::addFrame(FrameBase* _frame_ptr)
 {
-	addDownNode(_frame_ptr);
     if (_frame_ptr->isKey())
     {
         _frame_ptr->registerNewStateBlocks();
         if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp())
             last_key_frame_ptr_ = _frame_ptr;
+
+        insertDownNode(_frame_ptr, computeFrameOrder(_frame_ptr));
     }
+    else
+        addDownNode(_frame_ptr);
 
     return _frame_ptr;
 }
@@ -33,4 +36,17 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
 		(*fr_it)->getConstraintList(_ctr_list);
 }
 
+void TrajectoryBase::sortFrame(FrameBase* _frame_ptr)
+{
+    moveDownNode(_frame_ptr, computeFrameOrder(_frame_ptr));
+}
+
+FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBase* _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())
+            return frm_rit.base();
+    return getFrameListPtr()->begin();
+}
+
 } // namespace wolf

src/trajectory_base.h

@@ -62,7 +62,17 @@ class TrajectoryBase : public NodeLinked<Problem,FrameBase>
          **/
         void getConstraintList(ConstraintBaseList & _ctr_list);
         
+        /** \brief Returns the frame structure (see wolf.h)
+         **/
         FrameStructure getFrameStructure() const;
+
+        /** \brief Sorts the frame by timestamp
+         **/
+        void sortFrame(FrameBase* _frame_iter);
+
+        /** \brief Compute the position where the frame should be
+         **/
+        FrameBaseIter computeFrameOrder(FrameBase* _frame_ptr);
 };
 
 inline void TrajectoryBase::removeFrame(const FrameBaseIter& _frame_iter)