WIP

include/core/common/wolf.h

@@ -23,6 +23,7 @@
 #include <list>
 #include <map>
 #include <memory> // shared_ptr and weak_ptr
+#include <set>
 
 // System specifics
 #include <sys/stat.h>
@@ -243,6 +244,12 @@ WOLF_PTR_TYPEDEFS(TrajectoryBase);
 WOLF_PTR_TYPEDEFS(FrameBase);
 WOLF_LIST_TYPEDEFS(FrameBase);
 
+struct FrameComp{
+    bool operator() (const FrameBasePtr& lhs, const FrameBasePtr& rhs) const;
+};
+typedef std::set<FrameBasePtr, FrameComp> FrameList;
+typedef std::set<FrameBasePtr, FrameComp>::const_iterator FrameListIter;
+typedef std::set<FrameBasePtr, FrameComp>::const_reverse_iterator FrameListRevIter;
 // - Capture
 WOLF_PTR_TYPEDEFS(CaptureBase);
 WOLF_LIST_TYPEDEFS(CaptureBase);

include/core/factor/factor_base.h

@@ -73,7 +73,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
                    FactorStatus _status = FAC_ACTIVE);
 
         FactorBase(const std::string&  _tp,
-                   const FrameBasePtrList& _frame_other_list,
+                   const FrameList& _frame_other_list,
                    const CaptureBasePtrList& _capture_other_list,
                    const FeatureBasePtrList& _feature_other_list,
                    const LandmarkBasePtrList& _landmark_other_list,

include/core/trajectory/trajectory_base.h

@@ -23,7 +23,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
     friend FrameBase;
 
     private:
-        std::list<FrameBasePtr> frame_list_;
+         FrameList frame_list_;
 
     protected:
         std::string frame_structure_;  // Defines the structure of the Frames in the Trajectory.
@@ -38,7 +38,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         std::string getFrameStructure() const;
 
         // Frames
-        const FrameBasePtrList& getFrameList() const;
+        const FrameList& getFrameList() const;
         FrameBasePtr getLastFrame() const;
         FrameBasePtr getLastKeyFrame() const;
         FrameBasePtr getLastKeyOrAuxFrame() const;
@@ -72,14 +72,15 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         void moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place);
 };
 
-inline const FrameBasePtrList& TrajectoryBase::getFrameList() const
+inline const FrameList& TrajectoryBase::getFrameList() const
 {
     return frame_list_;
+    // return frame_list_;
 }
 
 inline FrameBasePtr TrajectoryBase::getLastFrame() const
 {
-    return frame_list_.back();
+    return *(frame_list_.rbegin());
 }
 
 inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const

src/factor/factor_base.cpp

@@ -36,7 +36,7 @@ FactorBase::FactorBase(const std::string&  _tp,
 }
 
 FactorBase::FactorBase(const std::string&  _tp,
-                       const FrameBasePtrList& _frame_other_list,
+                       const FrameList& _frame_other_list,
                        const CaptureBasePtrList& _capture_other_list,
                        const FeatureBasePtrList& _feature_other_list,
                        const LandmarkBasePtrList& _landmark_other_list,

src/sensor/sensor_base.cpp

@@ -223,8 +223,8 @@ CaptureBasePtr SensorBase::lastCapture(void) const
     CaptureBasePtr capture = nullptr;
     if (getProblem())
     {
-        FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-        FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+        auto frame_list = getProblem()->getTrajectory()->getFrameList();
+        FrameListRevIter frame_rev_it = frame_list.rbegin();
         while (frame_rev_it != frame_list.rend())
         {
             capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
@@ -243,8 +243,8 @@ CaptureBasePtr SensorBase::lastKeyCapture(void) const
     CaptureBasePtr capture = nullptr;
     if (getProblem())
     {
-        FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-        FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+        auto frame_list = getProblem()->getTrajectory()->getFrameList();
+        FrameListRevIter frame_rev_it = frame_list.rbegin();
         while (frame_rev_it != frame_list.rend())
         {
             if ((*frame_rev_it)->isKey())
@@ -266,10 +266,10 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
     CaptureBasePtr capture = nullptr;
     if (getProblem())
     {
-        FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+        auto frame_list = getProblem()->getTrajectory()->getFrameList();
 
         // We iterate in reverse since we're likely to find it close to the rbegin() place.
-        FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+        FrameListRevIter frame_rev_it = frame_list.rbegin();
         while (frame_rev_it != frame_list.rend())
         {
             if ((*frame_rev_it)->getTimeStamp() <= _ts)

src/trajectory/trajectory_base.cpp

@@ -17,10 +17,15 @@ TrajectoryBase::~TrajectoryBase()
     //
 }
 
+bool FrameComp::operator() (const FrameBasePtr& lhs, const FrameBasePtr& rhs) const
+{
+    return lhs->getTimeStamp()<rhs->getTimeStamp();
+}
+
 FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
-    frame_list_.push_back(_frame_ptr);
+    frame_list_.insert(_frame_ptr);
 
     if (_frame_ptr->isKeyOrAux())
     {
@@ -37,7 +42,7 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
-    frame_list_.remove(_frame_ptr);
+    frame_list_.erase(_frame_ptr);
 
     // update last_estimated and last_key
     if (_frame_ptr->isKeyOrAux())
@@ -59,7 +64,7 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place)
 {
     if (*_place != _frm_ptr)
     {
-        frame_list_.remove(_frm_ptr);
+        frame_list_.erase(_frm_ptr->id());
         frame_list_.insert(_place, _frm_ptr);
         updateLastFrames();
     }

test/gtest_factor_base.cpp

@@ -53,7 +53,7 @@ class FactorDummy : public FactorBase
         {
             //
         }
-        FactorDummy(const FrameBasePtrList& _frame_other_list,
+        FactorDummy(const FrameList& _frame_other_list,
                     const CaptureBasePtrList& _capture_other_list,
                     const FeatureBasePtrList& _feature_other_list,
                     const LandmarkBasePtrList& _landmark_other_list) :