[WIP] fix trajectory iterators & emplaceKeyFrame

One test fails

demos/demo_simple_AHP.cpp

@@ -97,11 +97,11 @@ int main(int argc, char** argv)
     /* 1 */
     ProblemPtr problem = Problem::create("PO", 3);
     // One anchor frame to define the lmk, and a copy to make a factor
-    FrameBasePtr kf_1 = problem->emplaceFrame(KEY,(Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
-    FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_1 = problem->emplaceKeyFrame((Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_2 = problem->emplaceKeyFrame((Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
     // and two other frames to observe the lmk
-    FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7d()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
-    FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7d()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_3 = problem->emplaceKeyFrame((Vector7d()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_4 = problem->emplaceKeyFrame((Vector7d()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
 
     kf_1->fix();
     kf_2->fix();

demos/demo_sort_keyframes.cpp

@@ -65,12 +65,12 @@ int main()
 
     printFrames(problem_ptr);
 
-    FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXd::Zero(3), TimeStamp(0.7));
+    FrameBasePtr frm7 = problem_ptr->emplaceKeyFrame(Eigen::VectorXd::Zero(3), TimeStamp(0.7));
     std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
 
     printFrames(problem_ptr);
 
-    FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXd::Zero(3), TimeStamp(0.35));
+    FrameBasePtr frm8 = problem_ptr->emplaceKeyFrame(Eigen::VectorXd::Zero(3), TimeStamp(0.35));
     std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
 
     printFrames(problem_ptr);

demos/demo_sparsification.cpp

@@ -216,7 +216,7 @@ int main(int argc, char** argv)
 
 	// ------------------------ START EXPERIMENT ------------------------
 	// First frame FIXED
-	last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3d::Zero(),TimeStamp(0));
+	last_frame_ptr = bl_problem_ptr->emplaceKeyFrame(Eigen::Vector3d::Zero(),TimeStamp(0));
 	last_frame_ptr->fix();
 	bl_problem_ptr->print(4, true, false, true);
 
@@ -238,7 +238,7 @@ int main(int argc, char** argv)
 				Eigen::Vector3d from_pose = frame_from_ptr->getState();
 				R.topLeftCorner(2,2) = Eigen::Rotation2Dd(from_pose(2)).matrix();
 				Eigen::Vector3d new_frame_pose = from_pose + R*meas;
-				last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to)));
+				last_frame_ptr = bl_problem_ptr->emplaceKeyFrame(new_frame_pose, TimeStamp(double(edge_to)));
 
 				frame_to_ptr = last_frame_ptr;
 

test/gtest_factor_epipolar.cpp

@@ -41,8 +41,8 @@ TEST(FactorEpipolar, exemple)
     auto S      = P->installSensor("SensorCamera", "camera", posecam, intr);
     auto camera = std::static_pointer_cast<SensorCamera>(S);
 
-    auto F0 = P             ->emplaceFrame(KEY, 0.0, pose0);
-    auto F1 = P             ->emplaceFrame(KEY, 1.0, pose1);
+    auto F0 = P             ->emplaceKeyFrame(0.0, pose0);
+    auto F1 = P             ->emplaceKeyFrame(1.0, pose1);
     auto C0 = CaptureBase   ::emplace<CaptureImage>(F0, F0->getTimeStamp(), camera, cv::Mat());
     auto C1 = CaptureBase   ::emplace<CaptureImage>(F1, F1->getTimeStamp(), camera, cv::Mat());
     auto f0 = FeatureBase   ::emplace<FeaturePointImage>(C0, pix0, 0, cv::Mat(), Matrix2d::Identity());

test/gtest_factor_pixel_hp.cpp

@@ -172,15 +172,15 @@ class FactorPixelHpTest : public testing::Test{
         	cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
         	cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
 
-            F1 = problem->emplaceFrame(KEY, 1.0, pose1);
+            F1 = problem->emplaceKeyFrame(1.0, pose1);
             I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
             f11 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp, 0, des, pix_cov)); // pixel at origin
 
-            F2 = problem->emplaceFrame(KEY, 2.0, pose2);
+            F2 = problem->emplaceKeyFrame(2.0, pose2);
             I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1))));
             f21 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp, 0, des, pix_cov));  // pixel at origin
 
-            F3 = problem->emplaceFrame(KEY, 3.0, pose3);
+            F3 = problem->emplaceKeyFrame(3.0, pose3);
             I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
             f31 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp, 0, des, pix_cov));  // pixel at origin
 
@@ -221,7 +221,7 @@ TEST(ProcessorFactorPixelHp, testZeroResidual)
     ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
 
     // Frame
-    FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, problem_ptr->stateZero());
+    FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, problem_ptr->stateZero());
 
     // Capture
     auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));
@@ -590,7 +590,7 @@ TEST_F(FactorPixelHpTest, testSolveBundleAdjustment)
 	// perturb states
 
     // kfs
-	for (auto kf : problem->getTrajectory()->getFrameList())
+	for (auto kf : *problem->getTrajectory())
 	{
 		if (kf == F1) continue;
 

test/gtest_factor_trifocal.cpp

@@ -142,15 +142,15 @@ class FactorTrifocalTest : public testing::Test{
             Vector2d pix(0,0);
             Matrix2d pix_cov(Matrix2d::Identity() * pow(pixel_noise_std, 2));
 
-            F1 = problem->emplaceFrame(KEY, 1.0, pose1);
+            F1 = problem->emplaceKeyFrame(1.0, pose1);
             I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(2,2,CV_8UC1)));
             f1 = FeatureBase::emplace<FeatureBase>(I1, "PIXEL", pix, pix_cov); // pixel at origin
 
-            F2 = problem->emplaceFrame(KEY, 2.0, pose2);
+            F2 = problem->emplaceKeyFrame(2.0, pose2);
             I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(2,2,CV_8UC1))));
             f2 = FeatureBase::emplace<FeatureBase>(I2, "PIXEL", pix, pix_cov); // pixel at origin
 
-            F3 = problem->emplaceFrame(KEY, 3.0, pose3);
+            F3 = problem->emplaceKeyFrame(3.0, pose3);
             I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(2,2,CV_8UC1)));
             f3 = FeatureBase::emplace<FeatureBase>(I3, "PIXEL", pix, pix_cov); // pixel at origin
 

test/gtest_processor_bundle_adjustment.cpp

@@ -255,7 +255,7 @@ TEST(ProcessorBundleAdjustment, emplaceLandmark)
     ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
 
     //Frame
-	FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, problem_ptr->stateZero());
+	FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, problem_ptr->stateZero());
 
 	// Capture, feature and factor
 	auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));