Merge branch '388-reimplement-framebase-getpreviousframe-and-getnextframe' into 'devel'

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

Closes #388

See merge request !409

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)

test/gtest_frame_base.cpp

@@ -80,9 +80,6 @@ TEST(FrameBase, LinksToTree)
     //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
     ASSERT_FALSE(F2->getConstrainedByList().empty());
 
-    // tree is inconsistent since we are missing the constrained_by link
-    // ASSERT_FALSE(P->check(0));
-
     // tree is now consistent
     ASSERT_TRUE(P->check(0));
 
@@ -113,6 +110,70 @@ TEST(FrameBase, LinksToTree)
     ASSERT_TRUE(F1->isFixed());
 }
 
+TEST(FrameBase, Frames)
+{
+    // Problem with 10 frames
+    ProblemPtr P = Problem::create("PO", 2);
+    TrajectoryBasePtr T = P->getTrajectory();
+    ParamsSensorOdom2d intrinsics_odo;
+    intrinsics_odo.k_disp_to_disp = 1;
+    intrinsics_odo.k_rot_to_rot = 1;
+    auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
+    auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+
+    // tree is now consistent
+    ASSERT_TRUE(P->check(0));
+
+    // First and last
+    ASSERT_EQ(F0, T->getFirstFrame());
+    ASSERT_EQ(F9, T->getLastFrame());
+
+    // Find by timestamp
+    ASSERT_EQ(F0, T->closestFrameToTimeStamp(0));
+    ASSERT_EQ(F1, T->closestFrameToTimeStamp(1));
+    ASSERT_EQ(F2, T->closestFrameToTimeStamp(2));
+    ASSERT_EQ(F3, T->closestFrameToTimeStamp(3));
+    ASSERT_EQ(F4, T->closestFrameToTimeStamp(4));
+    ASSERT_EQ(F5, T->closestFrameToTimeStamp(5));
+    ASSERT_EQ(F6, T->closestFrameToTimeStamp(6));
+    ASSERT_EQ(F7, T->closestFrameToTimeStamp(7));
+    ASSERT_EQ(F8, T->closestFrameToTimeStamp(8));
+    ASSERT_EQ(F9, T->closestFrameToTimeStamp(9));
+
+    // Next frame
+    ASSERT_EQ(F1, F0->getNextFrame());
+    ASSERT_EQ(F2, F1->getNextFrame());
+    ASSERT_EQ(F3, F2->getNextFrame());
+    ASSERT_EQ(F4, F3->getNextFrame());
+    ASSERT_EQ(F5, F4->getNextFrame());
+    ASSERT_EQ(F6, F5->getNextFrame());
+    ASSERT_EQ(F7, F6->getNextFrame());
+    ASSERT_EQ(F8, F7->getNextFrame());
+    ASSERT_EQ(F9, F8->getNextFrame());
+    ASSERT_EQ(nullptr, F9->getNextFrame());
+
+    // Prev frame
+    ASSERT_EQ(nullptr, F0->getPreviousFrame());
+    ASSERT_EQ(F0, F1->getPreviousFrame());
+    ASSERT_EQ(F1, F2->getPreviousFrame());
+    ASSERT_EQ(F2, F3->getPreviousFrame());
+    ASSERT_EQ(F3, F4->getPreviousFrame());
+    ASSERT_EQ(F4, F5->getPreviousFrame());
+    ASSERT_EQ(F5, F6->getPreviousFrame());
+    ASSERT_EQ(F6, F7->getPreviousFrame());
+    ASSERT_EQ(F7, F8->getPreviousFrame());
+    ASSERT_EQ(F8, F9->getPreviousFrame());
+}
+
 #include "core/state_block/state_quaternion.h"
 TEST(FrameBase, GetSetState)
 {