Merge branch '17-new-processor-loop-closure' into 'devel'

Resolve "New processor loop closure"

Closes #17

See merge request !24

include/laser/processor/processor_loop_closure_icp.h

@@ -5,7 +5,7 @@
  *      WOLF includes     *
  **************************/
 #include "core/common/wolf.h"
-#include <core/processor/processor_base.h>
+#include <core/processor/processor_loop_closure.h>
 #include <laser/capture/capture_laser_2d.h>
 
 #include <core/factor/factor_relative_pose_2d_with_extrinsics.h>
@@ -22,10 +22,10 @@ namespace wolf
 WOLF_PTR_TYPEDEFS(ProcessorLoopClosureIcp);
 WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcp);
 
-struct ParamsProcessorLoopClosureIcp : public ParamsProcessorBase
+struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure
 {
-    int recent_key_frames_ignored;
-    int key_frames_to_wait;
+    int recent_frames_ignored;
+    int frames_to_wait;
     double max_error_threshold;
     double min_points_percent;
 
@@ -34,14 +34,14 @@ struct ParamsProcessorLoopClosureIcp : public ParamsProcessorBase
 
     ParamsProcessorLoopClosureIcp() = default;
     ParamsProcessorLoopClosureIcp(std::string _unique_name, const ParamsServer& _server):
-        ParamsProcessorBase(_unique_name, _server)
+        ParamsProcessorLoopClosure(_unique_name, _server)
     {
-        recent_key_frames_ignored = _server.getParam<int>    (prefix + _unique_name + "/recent_key_frames_ignored");
-        key_frames_to_wait        = _server.getParam<int>    (prefix + _unique_name + "/key_frames_to_wait");
+        recent_frames_ignored     = _server.getParam<int>    (prefix + _unique_name + "/recent_frames_ignored");
+        frames_to_wait            = _server.getParam<int>    (prefix + _unique_name + "/frames_to_wait");
         max_error_threshold       = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
         min_points_percent        = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
 
-        icp_params.use_point_to_line_distance = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
+        icp_params.cov_factor                 = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
 
         icp_params.use_point_to_line_distance = _server.getParam<int>    (prefix + _unique_name + "/use_point_to_line_distance");
         icp_params.max_correspondence_dist    = _server.getParam<double> (prefix + _unique_name + "/max_correspondence_dist");
@@ -79,22 +79,21 @@ struct ParamsProcessorLoopClosureIcp : public ParamsProcessorBase
     }
     std::string print() const override
     {
-        return "\n" + ParamsProcessorBase::print()
-            + "recent_key_frames_ignored: " + std::to_string(recent_key_frames_ignored) + "\n"
-            + "key_frames_to_wait: " + std::to_string(key_frames_to_wait) + "\n"
-            + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
-            + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+        return "\n" + ParamsProcessorLoopClosure::print()
+            + "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n"
+            + "frames_to_wait: "        + std::to_string(frames_to_wait) + "\n"
+            + "max_error_threshold: "   + std::to_string(max_error_threshold) + "\n"
+            + "min_points_percent: "    + std::to_string(min_points_percent) + "\n";
     }
 };
-    struct CapturesAligned{
-        CaptureBasePtr capture_own;
-        CaptureBasePtr capture_other;
-        laserscanutils::icpOutput align_result;
-
-    };
 
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosureIcp);
+struct MatchLoopClosureIcp : public MatchLoopClosure
+{
+    laserscanutils::icpOutput align_result_;
+};
 
-class ProcessorLoopClosureIcp : public ProcessorBase
+class ProcessorLoopClosureIcp : public ProcessorLoopClosure
 {
     protected:
         // Useful sensor stuff
@@ -103,7 +102,7 @@ class ProcessorLoopClosureIcp : public ProcessorBase
 
         //Closeloop parameters
         ParamsProcessorLoopClosureIcpPtr params_loop_closure_icp_;
-        int key_frames_skipped_;
+        int frames_skipped_;
 
         // ICP algorithm
         std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
@@ -115,32 +114,29 @@ class ProcessorLoopClosureIcp : public ProcessorBase
         void configure(SensorBasePtr _sensor) override;
 
     protected:
-        void processCapture(CaptureBasePtr) override;
-
-        void processKeyFrame(FrameBasePtr _keyframe_ptr,
-                                     const double& _time_tolerance) override;
-
-        bool triggerInCapture(CaptureBasePtr) const override;
-
-        bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
-                                       const double& _time_tolerance) const override;
-
-        bool storeKeyFrame(FrameBasePtr) override;
-
-        bool storeCapture(CaptureBasePtr) override;
-
-        bool voteForKeyFrame() const override;
 
-        FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr,
-                                          const double &_time_tolerance);
+        /** \brief Returns if findLoopClosures() has to be called for the given capture
+         * Since there is no enough information in the capture to decide if a loop can be closed, try always.
+         */
+        bool voteFindLoopClosures(CaptureBasePtr cap) override;
 
-        std::map<double, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own,
-                                                             FrameBasePtrList& _keyframe_candidates);
+        /** \brief detects and emplaces all features of the given capture
+         * Since the loop closures of this processor are not based on any features, it is empty
+         */
+        void emplaceFeatures(CaptureBasePtr cap) override{};
 
-        CapturesAligned bestCandidate(std::map<double, CapturesAligned>& _capture_candidates);
+        /** \brief Find captures that correspond to loop closures with the given capture
+         */
+        std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override;
 
-        FactorBasePtr emplaceFeatureAndFactor(CapturesAligned& _captures_aligned);
+        /** \brief validates a loop closure
+         * Loops are validated just after calling ICP
+         */
+        bool validateLoopClosure(MatchLoopClosurePtr) override{ return true;};
 
+        /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+         */
+        void emplaceFactors(MatchLoopClosurePtr) override;
 };
 
 }