new parameter max_loops

include/core/processor/processor_loop_closure.h

@@ -10,11 +10,20 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
 
 struct ParamsProcessorLoopClosure : public ParamsProcessorBase
 {
-    using ParamsProcessorBase::ParamsProcessorBase;
-    //  virtual ~ParamsProcessorLoopClosure() = default;
-
-    // add neccesery parameters for loop closure initialisation here and initialize
-    // them in constructor
+    int max_loops=-1;
+
+    ParamsProcessorLoopClosure() = default;
+    ParamsProcessorLoopClosure(std::string _unique_name, const ParamsServer& _server):
+        ParamsProcessorBase(_unique_name, _server)
+    {
+        max_loops = _server.getParam<int>(prefix + _unique_name + "/max_loops");
+    }
+
+    std::string print() const override
+    {
+        return "\n" + ParamsProcessorBase::print()
+            + "max_loops: " + std::to_string(max_loops) + "\n";
+    }
 };
     
 WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
@@ -74,7 +83,7 @@ protected:
 
     /** \brief Find captures that correspond to loop closures with the given capture
      */
-    virtual std::list<MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
+    virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
 
     /** \brief validates a loop closure
      */

src/processor/processor_loop_closure.cpp

@@ -132,14 +132,22 @@ void ProcessorLoopClosure::process(CaptureBasePtr _capture)
         WOLF_DEBUG("finding loop closures...");
 
         // Find loop closures
-        auto match_lc_list = findLoopClosures(_capture);
+        auto match_lc_map = findLoopClosures(_capture);
 
-        WOLF_DEBUG(match_lc_list.size(), " loop closures found");
+        WOLF_DEBUG(match_lc_map.size(), " loop closures found");
 
         // Emplace factors for each LC if validated
-        for (auto match_lc : match_lc_list)
-            if (validateLoopClosure(match_lc))
-                emplaceFactors(match_lc);
+        auto n_loops = 0;
+        for (const auto& match_pair : match_lc_map)
+            if (validateLoopClosure(match_pair.second))
+            {
+                emplaceFactors(match_pair.second);
+                n_loops++;
+
+                if (params_loop_closure_->max_loops > 0 and
+                    n_loops >= params_loop_closure_->max_loops)
+                    break;
+            }
     }
 }
 

test/dummy/processor_loop_closure_dummy.h

@@ -30,9 +30,9 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
                                               MatrixXd::Identity(3,3));
         }
 
-        std::list<MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
+        std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
         {
-            std::list<MatchLoopClosurePtr> match_lc_list;
+            std::map<double,MatchLoopClosurePtr> match_lc_map;
 
             auto old_frame = _capture->getFrame()->getPreviousFrame();
             while (old_frame)
@@ -48,13 +48,16 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
                             match->capture_target_ptr_ = _capture;
                             match->normalized_score_ = 1;
 
-                            match_lc_list.push_back(match);
+                            while (match_lc_map.count(match->normalized_score_))
+                                match->normalized_score_ -= 1e-9;
+
+                            match_lc_map.emplace(match->normalized_score_, match);
                         }
 
                 old_frame = old_frame->getPreviousFrame();
             }
 
-            return match_lc_list;
+            return match_lc_map;
         }
 
         void emplaceFactors(MatchLoopClosurePtr match) override