diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp index ade207815d753393d15110d0a703c860d6dafe1e..1ca871af231c0b3f7fbe7c0cbb42011c9ed217df 100644 --- a/src/test/gtest_trajectory.cpp +++ b/src/test/gtest_trajectory.cpp @@ -17,6 +17,9 @@ using namespace wolf; +/// Set to true if you want debug info +bool debug = false; + TEST(TrajectoryBase, ClosestKeyFrame) { @@ -24,7 +27,6 @@ TEST(TrajectoryBase, ClosestKeyFrame) TrajectoryBasePtr T = P->getTrajectoryPtr(); // Trajectory status: - // // kf1 kf2 f3 frames // 1 2 3 time stamps // --+-----+-----+---> time @@ -36,8 +38,6 @@ TEST(TrajectoryBase, ClosestKeyFrame) T->addFrame(f2); T->addFrame(f3); - P->print(); - FrameBasePtr kf; // closest key-frame queried kf = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0 @@ -64,7 +64,6 @@ TEST(TrajectoryBase, Add_Remove_Frame) TrajectoryBasePtr T = P->getTrajectoryPtr(); // Trajectory status: - // // kf1 kf2 f3 frames // 1 2 3 time stamps // --+-----+-----+---> time @@ -75,19 +74,19 @@ TEST(TrajectoryBase, Add_Remove_Frame) // add frames and keyframes 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(P->getStateBlockList(). size(), 2); ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); 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(P->getStateBlockList(). size(), 4); ASSERT_EQ(P->getStateBlockNotificationList().size(), 4); 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(P->getStateBlockList(). size(), 4); ASSERT_EQ(P->getStateBlockNotificationList().size(), 4); @@ -97,7 +96,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) // remove frames and keyframes 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(P->getStateBlockList(). size(), 2); ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); @@ -106,7 +105,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id()); 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(P->getStateBlockList(). size(), 2); ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); @@ -114,7 +113,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id()); 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(P->getStateBlockList(). size(), 0); ASSERT_EQ(P->getStateBlockNotificationList().size(), 0); @@ -129,7 +128,6 @@ TEST(TrajectoryBase, KeyFramesAreSorted) TrajectoryBasePtr T = P->getTrajectoryPtr(); // Trajectory status: - // // kf1 kf2 f3 frames // 1 2 3 time stamps // --+-----+-----+---> time @@ -140,36 +138,61 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // add frames and keyframes in random order --> keyframes must be sorted after that 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->getLastKeyFramePtr()->id(), f2->id()); 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->getLastKeyFramePtr()->id(), f2->id()); 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->getLastKeyFramePtr()->id(), f2->id()); 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->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 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->getLastKeyFramePtr()->id(), f3->id()); 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->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) diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp index 6eee6707e25f5db2c0d786e89f048b7bf4480057..20c5427cf393f1b5cfb2c1757fa8b50dfea65321 100644 --- a/src/trajectory_base.cpp +++ b/src/trajectory_base.cpp @@ -48,6 +48,7 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list) void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr) { moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr)); + last_key_frame_ptr_ = findLastKeyFramePtr(); } FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)