Resolve "New ProcessorLoopClosure"

Closes #398 (closed)

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