Fix gtest_trajectory

test/gtest_trajectory.cpp

@@ -150,7 +150,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
 
     // add F3
     // FrameBasePtr F3 = P->emplaceKeyFrame(P->getFrameStructure(), P->getDim(), Eigen::Vector3d::Zero(), 3);
-    FrameBasePtr F3 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2); // 1 fixed, 1 not
+    FrameBasePtr F3 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 3); // 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);
@@ -167,11 +167,10 @@ 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) 4);
+    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastFrame()->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
     std::cout << __LINE__ << std::endl;
 
     F3->remove(); // F