diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 2accaa608175f73ffe6d056e54180013d98c3a07..3e5fdd7fd3263726383af28d691d9ef784955f52 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -118,9 +118,6 @@ WOLF_INFO("F2 id", F2->id());
F1->getO()->fix();
ASSERT_TRUE(F1->isFixed());
- // set key
- F1->setKey(P);
- ASSERT_TRUE(F1->isKey());
ASSERT_TRUE(F1->isKey());
}
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index c32acbd19e304d6b97212f7277616a0b88f426c0..6b403ea31ca156613e8e17f03b922cb3bffa3e68 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -41,8 +41,8 @@ class HasStateBlocksTest : public testing::Test
sbo1 = std::make_shared<StateQuaternion>();
sbv1 = std::make_shared<StateBlock>(3); // size 3
- F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
- F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
+ F0 = std::make_shared<FrameBase>(NON_ESTIMATED, 0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.0, nullptr); // non KF
}
virtual void TearDown(){}
@@ -55,9 +55,9 @@ TEST_F(HasStateBlocksTest, Notifications_setKey_add)
Notification n;
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->link(problem->getTrajectory());
+ // F0->link(problem->getTrajectory());
- ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
F0->setKey(problem);
@@ -73,11 +73,11 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->link(problem->getTrajectory());
+ // F0->link(problem->getTrajectory());
- ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
F0->addStateBlock("V", sbv0);
@@ -99,7 +99,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
// first make KF, then add SB
- F1->link(problem->getTrajectory());
+ // F1->link(problem->getTrajectory());
F1->setKey(problem);
F1->addStateBlock("P", sbp1);
@@ -130,7 +130,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->link(problem->getTrajectory());
+ // F0->link(problem->getTrajectory());
F0->addStateBlock("V", sbv0);
F0->setKey(problem);
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 66a711db5fb62ad804f8df12211653aa154c6357..ef66aa3dff9df69f39fcdd8f5079fd41f59ca969 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -22,9 +22,11 @@ class TrackMatrixTest : public testing::Test
FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
FeatureBasePtr f0, f1, f2, f3, f4;
+ ProblemPtr problem;
virtual void SetUp()
{
+ problem = Problem::create("PO", 2);
// unlinked captures
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0);
@@ -50,8 +52,8 @@ class TrackMatrixTest : public testing::Test
f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
- F0->setKey(nullptr);
- F4->setKey(nullptr);
+ F0->setKey(problem);
+ F4->setKey(problem);
// link captures
C0->link(F0);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 04cede086256d544656095d2ce89ee5c0d9aef22..61b1259593a0204f419a67be4bfdf1ff221ddfe6 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -44,7 +44,7 @@ struct DummySolverManager : public SolverManager
};
/// Set to true if you want debug info
-bool debug = false;
+bool debug = true;
TEST(TrajectoryBase, ClosestKeyFrame)
{
@@ -91,14 +91,14 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
- // KF1 KF2 F3 frames
+ // KF1 KF2 KF3 frames
// 1 2 3 time stamps
// --+-----+-----+---> time
FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1);
//WARNING! MIGHT NEED TO ROLLBACK THIS TO AUXILIARY, FELLA
- FrameBasePtr F2 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 2, Eigen::Vector3d::Zero());
- FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero());
+ FrameBasePtr F2 = P->emplaceKeyFrame(P->getFrameStructure(), P->getDim(), Eigen::Vector3d::Zero(), 2);
+ FrameBasePtr F3 = P->emplaceKeyFrame(P->getFrameStructure(), P->getDim(), Eigen::Vector3d::Zero(), 3);
FrameBasePtr KF; // closest key-frame queried
@@ -112,10 +112,10 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
ASSERT_EQ(KF->id(), F2->id()); // same id!
KF = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ ASSERT_EQ(KF->id(), F3->id()); // same id!
KF = T->closestKeyFrameToTimeStamp(3.2); // after the last frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ ASSERT_EQ(KF->id(), F3->id()); // same id!
}
TEST(TrajectoryBase, Add_Remove_Frame)
@@ -149,14 +149,14 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero());
+ // FrameBasePtr F3 = P->emplaceKeyFrame(P->getFrameStructure(), P->getDim(), Eigen::Vector3d::Zero(), 3);
+ FrameBasePtr F3 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 6);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
std::cout << __LINE__ << std::endl;
N.update();
@@ -167,7 +167,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
F2->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), F3->id());
@@ -192,124 +192,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
}
-TEST(TrajectoryBase, KeyFramesAreSorted)
-{
- using std::make_shared;
-
- ProblemPtr P = Problem::create("PO", 2);
- TrajectoryBasePtr T = P->getTrajectory();
-
- // Trajectory status:
- // KF1 KF2 F3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- // add frames and keyframes in random order --> keyframes must be sorted after that
- FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2);
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero()); // non-key-frame
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1);
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F3->setKey(P); // set KF3
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- auto F4 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 1.5, Eigen::Vector3d::Zero());
- // 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->getLastFrame() ->id(), F4->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- F4->setKey(P);
- // 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->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->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->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->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->getLastFrame()->id(), F4->id());
-
- //WARNING! MIGHT NEED TO ROLLBACK THIS TO AUXILIARY, FELLA
- auto F5 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 1.5, Eigen::Vector3d::Zero());
- // Trajectory status:
- // KF4 KF2 AuxF5 KF3 KF2 frames
- // 0.5 1 1.5 3 4 time stamps
- // --+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F5->setTimeStamp(5);
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 frames
- // 0.5 1 3 4 5 time stamps
- // --+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- auto F6 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 6, Eigen::Vector3d::Zero());
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 F6 frames
- // 0.5 1 3 4 5 6 time stamps
- // --+-----+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F6->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- auto F7 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 5.5, Eigen::Vector3d::Zero());
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 F7 F6 frames
- // 0.5 1 3 4 5 5.5 6 time stamps
- // --+-----+-----+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F7->id()); //Only auxiliary and key-frames are sorted
- ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
-}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);