gtest working

src/processor/processor_loop_closure.cpp

@@ -20,13 +20,19 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
      *  3. Otherwise -> store capture (Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one)
      */
 
+    WOLF_INFO("ProcessorLoopClosure::processCapture capture ", _capture->id());
+
     // CASE 1:
     if (_capture->getFrame())
     {
+        WOLF_INFO("CASE 1");
+
         process(_capture->getFrame(), _capture);
 
         // remove the frame and older frames
         buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+
+        return;
     }
 
     // Search for any stored frame within time tolerance of capture
@@ -35,16 +41,20 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
     // CASE 2:
     if (frame_pack)
     {
+        WOLF_INFO("CASE 2");
+
         _capture->link(frame_pack->key_frame);
 
         process(frame_pack->key_frame, _capture);
 
         // remove the frame and older frames
         buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+
+        return;
     }
     // CASE 3:
-    else
-        buffer_capture_.add(_capture->getTimeStamp(), _capture);
+    WOLF_INFO("CASE 3");
+    buffer_capture_.add(_capture->getTimeStamp(), _capture);
 }
 
 void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
@@ -56,14 +66,20 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
      *  4. Otherwise: The frame is not compatible with any stored capture -> discard frame
      */
 
+    WOLF_INFO("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+
     // CASE 1:
     auto cap = _frame->getCaptureOf(getSensor());
     if (cap)
     {
+        WOLF_INFO("CASE 1");
+
         process(_frame, cap);
 
         // remove the capture (if stored)
         buffer_capture_.getContainer().erase(cap->getTimeStamp());
+
+        return;
     }
 
     // Search for any stored capture within time tolerance of frame
@@ -72,6 +88,10 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
     // CASE 2:
     if (capture)
     {
+        WOLF_INFO("CASE 2");
+
+        capture->link(_frame);
+
         process(_frame, capture);
 
         // remove the capture (if stored)
@@ -79,30 +99,43 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
 
         // remove old captures (10s of old captures are kept in case frames arrives unordered)
         buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+
+        return;
     }
     // CASE 3:
-    else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+    if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
     {
+        WOLF_INFO("CASE 3");
+
         // store frame
         buffer_pack_kf_.add(_frame, _time_tolerance);
+
+        return;
     }
     // CASE 4:
+    WOLF_INFO("CASE 4");
     // nothing (discard frame)
 }
 
 void ProcessorLoopClosure::process(FrameBasePtr _frame, CaptureBasePtr _capture)
 {
+    WOLF_INFO("ProcessorLoopClosure::process frame ", _frame->id(), " capture ", _capture->id());
     assert(_capture->getFrame() == _frame && "ProcessorLoopClosure::process _capture not linked to _frame");
 
     // Detect and emplace features
+    WOLF_INFO("emplacing features...");
     emplaceFeatures(_capture);
 
     // Vote for loop closure search
     if (voteFindLoopClosures(_capture))
     {
+        WOLF_INFO("finding loop closures...");
+
         // Find loop closures
         auto cap_lc_list = findLoopClosures(_capture);
 
+        WOLF_INFO(cap_lc_list.size(), " loop closures found");
+
         // Emplace factors for each LC if validated
         for (auto cap_lc : cap_lc_list)
             if (validateLoopClosure(cap_lc, _capture))

test/gtest_processor_loop_closure.cpp

@@ -13,60 +13,311 @@
 using namespace wolf;
 using namespace Eigen;
 
-// Wolf problem
-ProblemPtr problem = Problem::create("PO", 2);
+class ProcessorLoopClosureTest : public testing::Test
+{
+    protected:
+        // Wolf problem
+        ProblemPtr problem = Problem::create("PO", 2);
+        SensorBasePtr sensor;
+        ProcessorLoopClosureDummyPtr processor;
+
+        virtual void SetUp()
+        {
+            // Emplace sensor
+            sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+                                                     "SensorBase",
+                                                     std::make_shared<StateBlock>(Vector2d::Zero()),
+                                                     std::make_shared<StateBlock>(Vector1d::Zero()),
+                                                     nullptr,
+                                                     2);
+
+            // Emplace processor
+            ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
+            params->time_tolerance = 0.5;
+            processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor,
+                                                                          params);
+        }
 
-ProcessorLoopClosureDummyPtr proc_lc;
+        FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
+        {
+            // new frame
+            return problem->emplaceFrame(ts, x);
+        }
 
-void setup()
+        CaptureBasePtr emplaceCapture(FrameBasePtr frame)
+        {
+            // new capture
+            return CaptureBase::emplace<CaptureBase>(frame,
+                                                     "CaptureBase",
+                                                     frame->getTimeStamp(),
+                                                     sensor);
+        }
+
+        CaptureBasePtr createCapture(TimeStamp ts)
+        {
+            // new capture
+            return std::make_shared<CaptureBase>("CaptureBase",
+                                                 ts,
+                                                 sensor);
+        }
+};
+
+TEST_F(ProcessorLoopClosureTest, installProcessor)
 {
-    // Emplace sensor
-    auto sens_lc = SensorBase::emplace<SensorBase>(problem->getHardware(),
-                                                   "SensorBase",
-                                                   std::make_shared<StateBlock>(Vector2d::Zero()),
-                                                   std::make_shared<StateBlock>(Vector1d::Zero()),
-                                                   nullptr,
-                                                   2);
-
-    // Emplace processor
-    ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
-    proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params);
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, frame_stored)
+{
+    // new frame
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+    // keyframecallback
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+    EXPECT_EQ(processor->getNStoredFrames(), 1);
+    EXPECT_EQ(processor->getNStoredCaptures(), 0);
 }
 
-TEST(ProcessorLoopClosure, installProcessor)
+TEST_F(ProcessorLoopClosureTest, capture_stored)
 {
-    setup();
+    // new capture
+    auto cap1 = createCapture(1);
+
+    // captureCallback
+    processor->captureCallback(cap1);
 
-    EXPECT_EQ(proc_lc->getNStoredFrames(), 0);
-    EXPECT_EQ(proc_lc->getNStoredCaptures(), 0);
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 1);
 }
 
-TEST(ProcessorLoopClosure, frame_stored)
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase1)
 {
-    setup();
+    // emplace frame and capture
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
+    auto cap1 = emplaceCapture(frm1);
+
+    // captureCallback
+    processor->captureCallback(cap1);
+
+    EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
 
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
+{
     // new frame
-    auto fr1 = problem->emplaceFrame(0, Vector3d::Zero());
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+    // new capture
+    auto cap1 = createCapture(1);
 
     // keyframecallback
-    problem->keyFrameCallback(fr1, nullptr, 1);
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
 
-    EXPECT_EQ(proc_lc->getNStoredFrames(), 1);
-    EXPECT_EQ(proc_lc->getNStoredCaptures(), 0);
+    // captureCallback
+    processor->captureCallback(cap1);
+
+    EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+    EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 0);
 }
 
-TEST(ProcessorLoopClosure, capture_stored)
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
 {
-    setup();
+    // new frame
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
+    // new capture
+    auto cap1 = createCapture(2);
+
+    // keyframecallback
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+    // captureCallback
+    processor->captureCallback(cap1);
+
+    EXPECT_TRUE(cap1->getFrame() == nullptr);
+    EXPECT_EQ(cap1->getFeatureList().size(), 0);
+    EXPECT_EQ(processor->getNStoredFrames(), 1);
+    EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
 
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase1)
+{
+    // emplace frame and capture
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
+    auto cap1 = emplaceCapture(frm1);
+
+    // keyframecallback
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+    EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
+{
+    // new frame
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
     // new capture
-    auto cap1 = std::make_shared<CaptureBase>("CaptureBase", 0, nullptr);
+    auto cap1 = createCapture(1);
+
+    // captureCallback
+    processor->captureCallback(cap1);
 
     // keyframecallback
-    proc_lc->captureCallback(cap1);
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+    EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+    EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
+{
+    // new frame
+    auto frm1 = emplaceFrame(2, Vector3d::Zero());
+    // new capture
+    auto cap1 = createCapture(1);
+
+    // captureCallback
+    processor->captureCallback(cap1);
+
+    // keyframecallback
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+    EXPECT_TRUE(cap1->getFrame() == nullptr);
+    EXPECT_EQ(cap1->getFeatureList().size(), 0);
+    EXPECT_EQ(processor->getNStoredFrames(), 1);
+    EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
+{
+    // new frame
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
+    // new capture
+    auto cap1 = createCapture(2);
+
+    // captureCallback
+    processor->captureCallback(cap1);
+
+    // keyframecallback
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+    EXPECT_TRUE(cap1->getFrame() == nullptr);
+    EXPECT_EQ(cap1->getFeatureList().size(), 0);
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
+{
+    // new frame
+    auto frm1 = emplaceFrame(1, Vector3d::Zero());
+    auto frm2 = emplaceFrame(2, Vector3d::Zero());
+    auto frm3 = emplaceFrame(3, Vector3d::Zero());
+    auto frm4 = emplaceFrame(4, Vector3d::Zero());
+    auto frm5 = emplaceFrame(5, Vector3d::Zero());
+    // new captures
+    auto cap4 = createCapture(4);
+
+    // keyframecallback
+    problem->keyFrameCallback(frm1, nullptr, 0.5);
+    problem->keyFrameCallback(frm2, nullptr, 0.5);
+    problem->keyFrameCallback(frm3, nullptr, 0.5);
+    problem->keyFrameCallback(frm4, nullptr, 0.5);
+    problem->keyFrameCallback(frm5, nullptr, 0.5);
+
+    // captureCallback
+    processor->captureCallback(cap4);
+
+    EXPECT_EQ(frm1->getCaptureList().size(), 0);
+    EXPECT_EQ(frm2->getCaptureList().size(), 0);
+    EXPECT_EQ(frm3->getCaptureList().size(), 0);
+    EXPECT_EQ(frm4->getCaptureList().size(), 1);
+    EXPECT_EQ(frm5->getCaptureList().size(), 0);
+
+    EXPECT_TRUE(cap4->getFrame() == frm4);
+    EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+    EXPECT_EQ(processor->getNStoredFrames(), 1); // all oldest frames are removed from buffer
+    EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
+{
+    // new frame
+    auto frm2 = emplaceFrame(2, Vector3d::Zero());
+    // new captures
+    auto cap1 = createCapture(1);
+    auto cap2 = createCapture(2);
+    auto cap3 = createCapture(3);
+    auto cap4 = createCapture(4);
+    auto cap5 = createCapture(5);
+
+    // captureCallback
+    processor->captureCallback(cap1);
+    processor->captureCallback(cap2);
+    processor->captureCallback(cap3);
+    processor->captureCallback(cap4);
+    processor->captureCallback(cap5);
+
+    // keyframecallback
+    problem->keyFrameCallback(frm2, nullptr, 0.5);
+
+    EXPECT_TRUE(cap1->getFrame() == nullptr);
+    EXPECT_TRUE(cap2->getFrame() == frm2);
+    EXPECT_TRUE(cap3->getFrame() == nullptr);
+    EXPECT_TRUE(cap4->getFrame() == nullptr);
+    EXPECT_TRUE(cap5->getFrame() == nullptr);
+
+    EXPECT_EQ(cap1->getFeatureList().size(), 0);
+    EXPECT_EQ(cap2->getFeatureList().size(), 1);
+    EXPECT_EQ(cap3->getFeatureList().size(), 0);
+    EXPECT_EQ(cap4->getFeatureList().size(), 0);
+    EXPECT_EQ(cap5->getFeatureList().size(), 0);
+
+    EXPECT_EQ(processor->getNStoredFrames(), 0);
+    EXPECT_EQ(processor->getNStoredCaptures(), 4);
+}
+
+TEST_F(ProcessorLoopClosureTest, emplaceFactors)
+{
+    // emplace frame and capture
+    auto cap1 = emplaceCapture(emplaceFrame(1, Vector3d::Zero()));
+    processor->captureCallback(cap1);
+
+    auto cap2 = emplaceCapture(emplaceFrame(2, Vector3d::Ones()));
+    processor->captureCallback(cap2);
+
+    auto cap3 = emplaceCapture(emplaceFrame(3, 2*Vector3d::Ones()));
+    processor->captureCallback(cap3);
+
+    auto cap4 = emplaceCapture(emplaceFrame(4, Vector3d::Zero()));
+    processor->captureCallback(cap4);
+
+    EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 1);
+    EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 0);
+    EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 0);
+    EXPECT_EQ(cap4->getFrame()->getConstrainedByList().size(), 0);
+
+    EXPECT_EQ(cap1->getFeatureList().size(), 1);
+    EXPECT_EQ(cap2->getFeatureList().size(), 1);
+    EXPECT_EQ(cap3->getFeatureList().size(), 1);
+    EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+    EXPECT_EQ(cap1->getFeatureList().front()->getFactorList().size(), 0);
+    EXPECT_EQ(cap2->getFeatureList().front()->getFactorList().size(), 0);
+    EXPECT_EQ(cap3->getFeatureList().front()->getFactorList().size(), 0);
+    EXPECT_EQ(cap4->getFeatureList().front()->getFactorList().size(), 1);
 
-    EXPECT_EQ(proc_lc->getNStoredFrames(), 0);
-    EXPECT_EQ(proc_lc->getNStoredCaptures(), 1);
+    EXPECT_EQ(cap1->getFrame()->getConstrainedByList().front(), cap4->getFeatureList().front()->getFactorList().front());
 }
 
 int main(int argc, char **argv)