WIP: Resolve "std::set and std::map instead of std::list in wolf nodes"

Closes #213 (closed)

test/gtest_trajectory.cpp

@@ -84,40 +84,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 }
 
-TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
-{
-
-    ProblemPtr P = Problem::create("PO", 2);
-    TrajectoryBasePtr T = P->getTrajectory();
-
-    // Trajectory status:
-    //  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 = 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
-
-    KF = T->closestKeyFrameToTimeStamp(-0.8);          // before all keyframes    --> return f0
-    ASSERT_EQ(KF->id(), F1->id());                           // same id!
-
-    KF = T->closestKeyFrameToTimeStamp(1.1);           // between keyframes       --> return F1
-    ASSERT_EQ(KF->id(), F1->id());                           // same id!
-
-    KF = T->closestKeyFrameToTimeStamp(1.9);           // between keyframes       --> return F2
-    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(), F3->id());                           // same id!
-
-    KF = T->closestKeyFrameToTimeStamp(3.2);           // after the last frame    --> return F2
-    ASSERT_EQ(KF->id(), F3->id());                           // same id!
-}
-
 TEST(TrajectoryBase, Add_Remove_Frame)
 {
     using std::make_shared;
@@ -149,14 +115,16 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     std::cout << __LINE__ << std::endl;
 
     // add F3
-    // FrameBasePtr F3 = P->emplaceKeyFrame(P->getFrameStructure(), P->getDim(), Eigen::Vector3d::Zero(), 3);
-    FrameBasePtr F3 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 3); // 1 fixed, 1 not
+    FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(),
+                                                              P->getDim(),
+                                                              3,
+                                                              Eigen::Vector3d::Zero()); // 1 fixed, 1 not
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 3);
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 6);
+    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 2);
+    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     std::cout << __LINE__ << std::endl;
 
     N.update();
@@ -166,11 +134,11 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     // remove frames and keyframes
     F2->remove(); // KF
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 2);
+    ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 1);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
     std::cout << __LINE__ << std::endl;
 
     F3->remove(); // F