From f0993e97b1b77350c8a0d1050b1a24e8f5e01334 Mon Sep 17 00:00:00 2001 From: Sergi Pujol <sergi.pujol.badell@estudiantat.upc.edu> Date: Tue, 23 Mar 2021 13:06:08 +0100 Subject: [PATCH] Improvements on processor loop closure --- .../core/processor/processor_loop_closure.h | 21 +++++++++++++++---- src/processor/processor_loop_closure.cpp | 10 ++++----- test/dummy/processor_loop_closure_dummy.h | 21 ++++++++++++------- 3 files changed, 35 insertions(+), 17 deletions(-) diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h index efe275fe3..9ef06e8f8 100644 --- a/include/core/processor/processor_loop_closure.h +++ b/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; diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp index 6642e1c66..574ac512e 100644 --- a/src/processor/processor_loop_closure.cpp +++ b/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); } } diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h index 8fff4db18..14e71a004 100644 --- a/test/dummy/processor_loop_closure_dummy.h +++ b/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); -- GitLab