Resolve "Emplace API inconsistent with ProcessorTrackerFeature/Landmark functions"

Closes #191 (closed)

test/gtest_processor_tracker_landmark_dummy.cpp

@@ -62,6 +62,27 @@ class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
         };
 };
 
+bool isFeatureLinked(FeatureBasePtr ftr, CaptureBasePtr cap)
+{
+    return ftr->getCapture() == cap &&
+           std::find(cap->getFeatureList().begin(), cap->getFeatureList().end(), ftr) != cap->getFeatureList().end();
+
+}
+
+bool isFactorLinked(FactorBasePtr fac, FeatureBasePtr ftr)
+{
+    return fac->getFeature() == ftr &&
+           std::find(ftr->getFactorList().begin(), ftr->getFactorList().end(), fac) != ftr->getFactorList().end();
+
+}
+
+bool isLandmarkLinked(LandmarkBasePtr lmk, MapBasePtr map)
+{
+    return lmk->getMap() == map &&
+           std::find(map->getLandmarkList().begin(), map->getLandmarkList().end(), lmk) != map->getLandmarkList().end();
+
+}
+
 // Use the following in case you want to initialize tests with predefines variables or methods.
 class ProcessorTrackerLandmarkDummyTest : public testing::Test
 {
@@ -93,93 +114,103 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
         }
 };
 
-//TEST_F(ProcessorTrackerLandmarkDummyTest, installProcessor)
-//{
-//    ASSERT_EQ(processor->getProblem(), problem);
-//    ASSERT_TRUE(problem->check(0));
-//}
-//
-//TEST_F(ProcessorTrackerLandmarkDummyTest, detectNewFeatures)
-//{
-//    // Put a capture on last_ptr_
-//    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
-//    processor->setLast(last_cap);
-//
-//    FeatureBasePtrList feat_list;
-//
-//    // demo callDetectNewFeatures
-//    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
-//    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
-//    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
-//}
-//
-//TEST_F(ProcessorTrackerLandmarkDummyTest, createLandmark)
-//{
-//    // Put a capture on last_ptr_
-//    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
-//    processor->setLast(last_cap);
-//
-//    FeatureBasePtrList feat_list;
-//
-//    // demo callDetectNewFeatures
-//    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
-//    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
-//    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
-//
-//    for (auto ftr : feat_list)
-//    {
-//        auto lmk = processor->callEmplaceLandmark(ftr);
-//        lmk->link(problem->getMap());
-//    }
-//    ASSERT_EQ(problem->getMap()->getLandmarkList().size(),n_feat); // created 10 landmarks
-//}
-//
-//TEST_F(ProcessorTrackerLandmarkDummyTest, createNewLandmarks)
-//{
-//    // Put a capture on last_ptr_
-//    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
-//    processor->setLast(last_cap);
-//
-//    FeatureBasePtrList feat_list;
-//
-//    // test detectNewFeatures
-//    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
-//    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
-//    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
-//
-//    // test createNewLandmarks
-//    processor->setNewFeaturesLast(feat_list);
-//    processor->callEmplaceNewLandmarks();
-//    ASSERT_EQ(processor->getNewLandmarks().size(),n_feat); // created 10 landmarks
-//}
-//
-//TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
-//{
-//    // Put a capture on last_ptr_
-//    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
-//    processor->setLast(last_cap);
-//
-//    FeatureBasePtrList feat_list;
-//
-//    // test detectNewFeatures
-//    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, feat_list);
-//    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
-//    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
-//
-//    // test createNewLandmarks
-//    processor->setNewFeaturesLast(feat_list);
-//    processor->callEmplaceNewLandmarks();
-//    LandmarkBasePtrList new_landmarks = processor->getNewLandmarks();
-//    ASSERT_EQ(new_landmarks.size(),n_feat); // created 10 landmarks
-//
-//    //test findLandmarks
-//    LandmarkMatchMap feature_landmark_correspondences;
-//    FeatureBasePtrList feat_found;
-//    processor->callFindLandmarks(new_landmarks, feat_found, feature_landmark_correspondences);
-//    ASSERT_EQ(feature_landmark_correspondences.size(), feat_found.size());
-//    ASSERT_EQ(feat_list.size(), feat_found.size()+1); // one of each 10 tracks is lost
-//}
-//
+TEST_F(ProcessorTrackerLandmarkDummyTest, installProcessor)
+{
+    ASSERT_EQ(processor->getProblem(), problem);
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, detectNewFeatures)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // demo callDetectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+    ASSERT_EQ(n_feat, feat_list.size()); // detected 10 features
+    ASSERT_EQ(n_feat, params->max_new_features); // detected 10 features
+
+    // check the features are emplaced
+    ASSERT_EQ(n_feat, last_cap->getFeatureList().size());
+    for (auto feat : feat_list)
+        ASSERT_TRUE(isFeatureLinked(feat, last_cap));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, createLandmark)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // demo callDetectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+
+    for (auto ftr : feat_list)
+    {
+        auto lmk = processor->callEmplaceLandmark(ftr);
+        // check that it is correctly emplaced
+        ASSERT_TRUE(isLandmarkLinked(lmk,problem->getMap()));
+    }
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(),n_feat); // emplaced 10 landmarks
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, createNewLandmarks)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // test detectNewFeatures
+    unsigned int n_feat = processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+
+    // test createNewLandmarks
+    processor->setNewFeaturesLast(feat_list);
+    processor->callEmplaceNewLandmarks();
+    ASSERT_EQ(processor->getNewLandmarks().size(),n_feat); // emplaced 10 landmarks
+
+    // check that it is correctly emplaced
+    for (auto lmk : processor->getNewLandmarks())
+        ASSERT_TRUE(isLandmarkLinked(lmk,problem->getMap()));
+}
+
+TEST_F(ProcessorTrackerLandmarkDummyTest, findLandmarks)
+{
+    // Put a capture on last_ptr_
+    CaptureBasePtr last_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setLast(last_cap);
+
+    FeatureBasePtrList feat_list;
+
+    // test detectNewFeatures
+    processor->callDetectNewFeatures(params->max_new_features, last_cap, feat_list);
+
+    // test createNewLandmarks
+    processor->setNewFeaturesLast(feat_list);
+    processor->callEmplaceNewLandmarks();
+    LandmarkBasePtrList new_landmarks = processor->getNewLandmarks();
+
+    //test findLandmarks
+    LandmarkMatchMap feature_landmark_correspondences;
+    FeatureBasePtrList feat_found;
+
+    CaptureBasePtr inc_cap = std::make_shared<CaptureVoid>(0, sensor);
+    processor->setInc(inc_cap);
+
+    processor->callFindLandmarks(new_landmarks, inc_cap, feat_found, feature_landmark_correspondences);
+    ASSERT_EQ(feature_landmark_correspondences.size(), feat_found.size());
+    ASSERT_EQ(feat_list.size(), feat_found.size()+1); // one of each 10 tracks is lost
+
+    for (auto feat : feat_found)
+        ASSERT_TRUE(isFeatureLinked(feat, inc_cap));
+}
+
 //TEST_F(ProcessorTrackerLandmarkDummyTest, processNew)
 //{
 //    // Put a capture on last_ptr_