Improvements on processor loop closure

include/core/processor/processor_loop_closure.h

@@ -16,6 +16,20 @@ struct ParamsProcessorLoopClosure : public ParamsProcessorBase
     // add neccesery parameters for loop closure initialisation here and initialize
     // them in constructor
 };
+    
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
+
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
+struct MatchLoopClosure
+{
+        CaptureBasePtr capture_reference_ptr_; ///< Capture reference
+        CaptureBasePtr capture_target_ptr_; ///< Capture target
+        double normalized_score_;    ///< normalized similarity score (0 is bad, 1 is good)
+};
 
 /** \brief General loop closure processor
  *
@@ -29,7 +43,6 @@ struct ParamsProcessorLoopClosure : public ParamsProcessorBase
  * + You can override the following classes :
  *   - process(CaptureBasePtr)
  */
-
 class ProcessorLoopClosure : public ProcessorBase
 {
 protected:
@@ -61,15 +74,15 @@ protected:
 
     /** \brief Find captures that correspond to loop closures with the given capture
      */
-    virtual CaptureBasePtrList findLoopClosures(CaptureBasePtr _capture) = 0;
+    virtual std::list<MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
 
     /** \brief validates a loop closure
      */
-    virtual bool validateLoopClosure(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 0;
+    virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
 
     /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
      */
-    virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 0;
+    virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
 
     void processCapture(CaptureBasePtr) override;
     void processKeyFrame(FrameBasePtr, const double&) override;

src/processor/processor_loop_closure.cpp

@@ -132,14 +132,14 @@ void ProcessorLoopClosure::process(CaptureBasePtr _capture)
         WOLF_DEBUG("finding loop closures...");
 
         // Find loop closures
-        auto cap_lc_list = findLoopClosures(_capture);
+        auto match_lc_list = findLoopClosures(_capture);
 
-        WOLF_DEBUG(cap_lc_list.size(), " loop closures found");
+        WOLF_DEBUG(match_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))
-                emplaceFactors(cap_lc, _capture);
+        for (auto match_lc : match_lc_list)
+            if (validateLoopClosure(match_lc))
+                emplaceFactors(match_lc);
     }
 }
 

test/dummy/processor_loop_closure_dummy.h

@@ -19,7 +19,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
 
     protected:
         bool voteFindLoopClosures(CaptureBasePtr cap) override { return true;};
-        bool validateLoopClosure(CaptureBasePtr, CaptureBasePtr) override { return true;};
+        bool validateLoopClosure(MatchLoopClosurePtr match) override { return true;};
 
         void emplaceFeatures(CaptureBasePtr cap) override
         {
@@ -30,9 +30,9 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
                                               MatrixXd::Identity(3,3));
         }
 
-        CaptureBasePtrList findLoopClosures(CaptureBasePtr _capture) override
+        std::list<MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
         {
-            CaptureBasePtrList cap_lc_list;
+            std::list<MatchLoopClosurePtr> match_lc_list;
 
             auto old_frame = _capture->getFrame()->getPreviousFrame();
             while (old_frame)
@@ -43,19 +43,24 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
                         if (feat->getType() == "FeatureLoopClosureDummy" and
                             (feat->getMeasurement() - _capture->getFeatureList().front()->getMeasurement()).norm() < 1e-3)
                         {
-                            cap_lc_list.push_back(cap);
+                            MatchLoopClosurePtr match = std::make_shared<MatchLoopClosure>();
+                            match->capture_reference_ptr_ = cap;
+                            match->capture_target_ptr_ = _capture;
+                            match->normalized_score_ = 1;
+
+                            match_lc_list.push_back(match);
                         }
 
                 old_frame = old_frame->getPreviousFrame();
             }
 
-            return cap_lc_list;
+            return match_lc_list;
         }
 
-        void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
+        void emplaceFactors(MatchLoopClosurePtr match) override
         {
             FeatureBasePtr feat_2;
-            for (auto feat : _capture_2->getFeatureList())
+            for (auto feat : match->capture_target_ptr_->getFeatureList())
                 if (feat->getType() == "FeatureLoopClosureDummy")
                 {
                     feat_2 = feat;
@@ -63,7 +68,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
                 }
 
             FactorBase::emplace<FactorRelativePose2d>(feat_2, feat_2,
-                                                      _capture_1->getFrame(),
+                                                      match->capture_reference_ptr_->getFrame(),
                                                       shared_from_this(),
                                                       false,
                                                       TOP_LOOP);