Skip to content
Snippets Groups Projects
Commit e6bb58bf authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Update last_key_frame_ptr_ after sortFrame(frm)

All tests passed
parent 666b3e88
No related branches found
No related tags found
No related merge requests found
...@@ -17,6 +17,9 @@ ...@@ -17,6 +17,9 @@
using namespace wolf; using namespace wolf;
/// Set to true if you want debug info
bool debug = false;
TEST(TrajectoryBase, ClosestKeyFrame) TEST(TrajectoryBase, ClosestKeyFrame)
{ {
...@@ -24,7 +27,6 @@ TEST(TrajectoryBase, ClosestKeyFrame) ...@@ -24,7 +27,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
TrajectoryBasePtr T = P->getTrajectoryPtr(); TrajectoryBasePtr T = P->getTrajectoryPtr();
// Trajectory status: // Trajectory status:
//
// kf1 kf2 f3 frames // kf1 kf2 f3 frames
// 1 2 3 time stamps // 1 2 3 time stamps
// --+-----+-----+---> time // --+-----+-----+---> time
...@@ -36,8 +38,6 @@ TEST(TrajectoryBase, ClosestKeyFrame) ...@@ -36,8 +38,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
T->addFrame(f2); T->addFrame(f2);
T->addFrame(f3); T->addFrame(f3);
P->print();
FrameBasePtr kf; // closest key-frame queried FrameBasePtr kf; // closest key-frame queried
kf = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0 kf = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0
...@@ -64,7 +64,6 @@ TEST(TrajectoryBase, Add_Remove_Frame) ...@@ -64,7 +64,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
TrajectoryBasePtr T = P->getTrajectoryPtr(); TrajectoryBasePtr T = P->getTrajectoryPtr();
// Trajectory status: // Trajectory status:
//
// kf1 kf2 f3 frames // kf1 kf2 f3 frames
// 1 2 3 time stamps // 1 2 3 time stamps
// --+-----+-----+---> time // --+-----+-----+---> time
...@@ -75,19 +74,19 @@ TEST(TrajectoryBase, Add_Remove_Frame) ...@@ -75,19 +74,19 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// add frames and keyframes // add frames and keyframes
T->addFrame(f1); // KF T->addFrame(f1); // KF
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 1); ASSERT_EQ(T->getFrameList(). size(), 1);
ASSERT_EQ(P->getStateBlockList(). size(), 2); ASSERT_EQ(P->getStateBlockList(). size(), 2);
ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
T->addFrame(f2); // KF T->addFrame(f2); // KF
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 2); ASSERT_EQ(T->getFrameList(). size(), 2);
ASSERT_EQ(P->getStateBlockList(). size(), 4); ASSERT_EQ(P->getStateBlockList(). size(), 4);
ASSERT_EQ(P->getStateBlockNotificationList().size(), 4); ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
T->addFrame(f3); // F T->addFrame(f3); // F
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 3); ASSERT_EQ(T->getFrameList(). size(), 3);
ASSERT_EQ(P->getStateBlockList(). size(), 4); ASSERT_EQ(P->getStateBlockList(). size(), 4);
ASSERT_EQ(P->getStateBlockNotificationList().size(), 4); ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
...@@ -97,7 +96,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) ...@@ -97,7 +96,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// remove frames and keyframes // remove frames and keyframes
f2->remove(); // KF f2->remove(); // KF
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 2); ASSERT_EQ(T->getFrameList(). size(), 2);
ASSERT_EQ(P->getStateBlockList(). size(), 2); ASSERT_EQ(P->getStateBlockList(). size(), 2);
ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
...@@ -106,7 +105,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) ...@@ -106,7 +105,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
f3->remove(); // F f3->remove(); // F
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 1); ASSERT_EQ(T->getFrameList(). size(), 1);
ASSERT_EQ(P->getStateBlockList(). size(), 2); ASSERT_EQ(P->getStateBlockList(). size(), 2);
ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
...@@ -114,7 +113,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) ...@@ -114,7 +113,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
f1->remove(); // KF f1->remove(); // KF
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 0); ASSERT_EQ(T->getFrameList(). size(), 0);
ASSERT_EQ(P->getStateBlockList(). size(), 0); ASSERT_EQ(P->getStateBlockList(). size(), 0);
ASSERT_EQ(P->getStateBlockNotificationList().size(), 0); ASSERT_EQ(P->getStateBlockNotificationList().size(), 0);
...@@ -129,7 +128,6 @@ TEST(TrajectoryBase, KeyFramesAreSorted) ...@@ -129,7 +128,6 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
TrajectoryBasePtr T = P->getTrajectoryPtr(); TrajectoryBasePtr T = P->getTrajectoryPtr();
// Trajectory status: // Trajectory status:
//
// kf1 kf2 f3 frames // kf1 kf2 f3 frames
// 1 2 3 time stamps // 1 2 3 time stamps
// --+-----+-----+---> time // --+-----+-----+---> time
...@@ -140,36 +138,61 @@ TEST(TrajectoryBase, KeyFramesAreSorted) ...@@ -140,36 +138,61 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// add frames and keyframes in random order --> keyframes must be sorted after that // add frames and keyframes in random order --> keyframes must be sorted after that
T->addFrame(f2); // KF2 T->addFrame(f2); // KF2
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFramePtr() ->id(), f2->id()); ASSERT_EQ(T->getLastFramePtr() ->id(), f2->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
T->addFrame(f3); // F3 T->addFrame(f3); // F3
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id()); ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
T->addFrame(f1); // KF1 T->addFrame(f1); // KF1
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id()); ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
f3->setKey(); // set KF3 f3->setKey(); // set KF3
P->print(2,0,0,0); if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id()); ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
T->addFrame(f4); T->addFrame(f4);
P->print(2,0,1,0); // Trajectory status:
// kf1 kf2 kf3 f4 frames
// 1 2 3 1.5 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
ASSERT_EQ(T->getLastFramePtr() ->id(), f4->id()); ASSERT_EQ(T->getLastFramePtr() ->id(), f4->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
f4->setKey(); f4->setKey();
P->print(2,0,1,0); // Trajectory status:
// kf1 kf4 kf2 kf3 frames
// 1 1.5 2 3 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id()); ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
f2->setTimeStamp(4);
// Trajectory status:
// kf1 kf4 kf3 kf2 frames
// 1 1.5 3 4 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
ASSERT_EQ(T->getLastFramePtr() ->id(), f2->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
f4->setTimeStamp(0.5);
// Trajectory status:
// kf4 kf2 kf3 kf2 frames
// 0.5 1 3 4 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
ASSERT_EQ(T->getFrameList().front()->id(), f4->id());
} }
int main(int argc, char **argv) int main(int argc, char **argv)
......
...@@ -48,6 +48,7 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list) ...@@ -48,6 +48,7 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr) void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
{ {
moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr)); moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
last_key_frame_ptr_ = findLastKeyFramePtr();
} }
FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr) FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment