Skip to content
Snippets Groups Projects
Commit b027ea59 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Fix all tests except track_matrix & trajectory

parent a790a6bb
No related branches found
No related tags found
1 merge request!362WIP: Resolve "std::set and std::map instead of std::list in wolf nodes"
Pipeline #5446 failed
......@@ -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());
}
......
......@@ -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);
......
......@@ -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);
......
......@@ -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);
......
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