Resolve "Reimplement FrameBase::getPreviousFrame() and getNextFrame()"

Closes #388 (closed)

src/frame/frame_base.cpp

@@ -141,43 +141,28 @@ FrameBasePtr FrameBase::getPreviousFrame() const
 {
     assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
 
-    //look for the position of this node in the upper list (frame list of trajectory)
-    for (auto f_it = getTrajectory()->rbegin(); f_it != getTrajectory()->rend(); f_it++ )
-    {
-        if ( this->frame_id_ == (*f_it)->id() )
-        {
-        	f_it++;
-        	if (f_it != getTrajectory()->rend())
-            {
-                return *f_it;
-            }
-        	else
-        	{
-        	    return nullptr;
-        	}
-        }
-    }
-    return nullptr;
+    auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
+
+    assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
+
+    if (current_frame_it == getTrajectory()->getFrameMap().begin())
+        return nullptr;
+
+    return std::prev(current_frame_it)->second;
 }
 
 FrameBasePtr FrameBase::getNextFrame() const
 {
-    //std::cout << "finding next frame of " << this->frame_id_ << std::endl;
-	auto f_it = getTrajectory()->rbegin();
-	f_it++; //starting from second last frame
+    assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
 
-    //look for the position of this node in the frame list of trajectory
-    while (f_it != getTrajectory()->rend())
-    {
-        if ( this->frame_id_ == (*f_it)->id())
-        {
-        	f_it--;
-			return *f_it;
-        }
-    	f_it++;
-    }
-    std::cout << "next frame not found!" << std::endl;
-    return nullptr;
+    auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
+
+    assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
+
+    if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
+        return nullptr;
+
+    return std::next(current_frame_it)->second;
 }
 
 CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)