diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h index 9ef06e8f8ca46796d0f3cf36508237df95526e06..bb0c9f17041de6cf80919b046c6a5218eaf05518 100644 --- a/include/core/processor/processor_loop_closure.h +++ b/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 */ diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp index 67e724a538057be1e82ff0c4892c4320bedde1dc..f0ee9f469125e86af2d0b3a82be90f1b0de55b5e 100644 --- a/src/processor/processor_loop_closure.cpp +++ b/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; + } } } diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h index 14e71a00409f4d6032627d4f7b208bb51bad0515..a391ad9bd593b164b5bfbb0607295e72763852e4 100644 --- a/test/dummy/processor_loop_closure_dummy.h +++ b/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