Merge branch '15-adapt-to-std-set-and-std-map-instead-of-std-list-in-wolf-nodes' into devel

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();

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

@@ -169,15 +169,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
 
@@ -217,7 +217,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)));
@@ -586,7 +586,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

@@ -141,15 +141,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)));