working on processor loop closure icp

include/laser/processor/params_icp.h

@@ -3,7 +3,6 @@
 
 #include "core/common/wolf.h"
 #include "laser_scan_utils/icp.h"
-#undef max
 
 namespace wolf {
 

include/laser/processor/processor_loop_closure_icp.h

@@ -46,29 +46,41 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcp);
 
 struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure, public ParamsIcp
 {
-    int recent_frames_ignored, frames_ignored_after_loop;
-    double max_error_threshold;
-    double min_points_percent;
+    int recent_frames_ignored;          // recent frames not considered candidates
+    int frames_ignored_after_loop;      // wait frames after closing a loop
+    int max_attempts;                   // max number of ICP attempts for stopping creating candidates
+    int max_candidates;                 // max number of candidats (ICP success) for stopping creating candidates
+
+    std::string candidate_generation;   // order of selection of frames: "deterministic" or "random"
+
+    double max_error_threshold;         // maximum error for accepting ICP
+    double min_points_percent;          // minimum correspondence % for accepting ICP
 
     ParamsProcessorLoopClosureIcp() = default;
     ParamsProcessorLoopClosureIcp(std::string _unique_name, const ParamsServer& _server) :
         ParamsProcessorLoopClosure(_unique_name, _server),
         ParamsIcp(prefix + _unique_name, _server)
     {
-        recent_frames_ignored     = _server.getParam<int>    (prefix + _unique_name + "/recent_frames_ignored");
-        frames_ignored_after_loop = _server.getParam<int>    (prefix + _unique_name + "/frames_ignored_after_loop");
-        max_error_threshold       = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
-        min_points_percent        = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
+        recent_frames_ignored     = _server.getParam<int>           (prefix + _unique_name + "/recent_frames_ignored");
+        frames_ignored_after_loop = _server.getParam<int>           (prefix + _unique_name + "/frames_ignored_after_loop");
+        max_attempts              = _server.getParam<int>           (prefix + _unique_name + "/max_attempts");
+        max_candidates            = _server.getParam<int>           (prefix + _unique_name + "/max_candidates");
+        candidate_generation      = _server.getParam<std::string>   (prefix + _unique_name + "/candidate_generation");
+        max_error_threshold       = _server.getParam<double>        (prefix + _unique_name + "/max_error_threshold");
+        min_points_percent        = _server.getParam<double>        (prefix + _unique_name + "/min_points_percent");
     }
 
     std::string print() const override
     {
         return "\n" + ParamsProcessorLoopClosure::print()
             + ParamsIcp::print() + "\n"
-            + "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n"
+            + "recent_frames_ignored: "     + std::to_string(recent_frames_ignored)     + "\n"
             + "frames_ignored_after_loop: " + std::to_string(frames_ignored_after_loop) + "\n"
-            + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
-            + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+            + "max_attempts: "              + std::to_string(max_attempts)              + "\n"
+            + "max_candidates: "            + std::to_string(max_candidates)            + "\n"
+            + "candidate_generation: "      + candidate_generation                      + "\n"
+            + "max_error_threshold: "       + std::to_string(max_error_threshold)       + "\n"
+            + "min_points_percent: "        + std::to_string(min_points_percent)        + "\n";
     }
 };
 
@@ -134,6 +146,10 @@ class ProcessorLoopClosureIcp : public ProcessorLoopClosure
         /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
          */
         void emplaceFactors(MatchLoopClosurePtr) override;
+
+        /** \brief returns a list containing the loop closure frame candidates list
+         */
+        FrameBasePtrList generateSearchList(CaptureBasePtr) const;
 };
 
 }

src/processor/processor_loop_closure_icp.cpp

@@ -22,6 +22,7 @@
 #include "laser/processor/processor_loop_closure_icp.h"
 #include "laser/sensor/sensor_laser_2d.h"
 #include "core/math/covariance.h"
+#include <random>
 
 using namespace laserscanutils;
 using namespace Eigen;
@@ -75,42 +76,38 @@ std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(C
     auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(_capture);
     assert(cap_laser != nullptr);
 
-    // Ignore the most recent 'recent_frames_ignored' frames
-    const auto &trajectory = getProblem()->getTrajectory();
-    int n_ignored = 0;
-    for(auto frm_it  = trajectory->rbegin();
-             frm_it != trajectory->rend();
-             frm_it++)
-    {
-        if (frm_it->second == _capture->getFrame())
-            continue;
-
-        if (n_ignored < params_loop_closure_icp_->recent_frames_ignored)
-        {
-            n_ignored++;
-            continue;
-        }
+    // generate search list
+    auto frames_search_list = generateSearchList(_capture);
 
-        auto cap_ref_list = frm_it->second->getCapturesOfType<CaptureLaser2d>();
-        for (auto cap_ref : cap_ref_list)
+    // Iterate search list looking for loop closure candidates
+    for (auto frm : frames_search_list)
+    {
+        // Frame can have more than one laser capture
+        for (auto cap_ref : frm->getCapturesOfType<CaptureLaser2d>())
         {
             auto cap_ref_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap_ref);
-            if (cap_ref_laser != nullptr)
+            assert(cap_ref_laser != nullptr);
+
+            // Match Scans
+            auto match = matchScans(cap_ref_laser, cap_laser);
+
+            // IT'S A MATCH!
+            if (match != nullptr)
             {
-                // Match Scans
-                auto match = matchScans(cap_ref_laser, cap_laser);
+                // avoid duplicated scores (std::map)
+                while (match_lc_map.count(match->normalized_score))
+                    match->normalized_score -= 1e-9;
 
-                // IT'S A MATCH!
-                if (match != nullptr)
-                {
-                    // avoid duplicated scores (std::map)
-                    while (match_lc_map.count(match->normalized_score))
-                        match->normalized_score -= 1e-9;
+                match_lc_map.emplace(match->normalized_score, match);
 
-                    match_lc_map.emplace(match->normalized_score, match);
-                }
+                // one factor to each frame (in case of more than one laser capture..)
+                break;
             }
         }
+
+        // stop if enough candidates
+        if (match_lc_map.size() == params_loop_closure_icp_->max_candidates)
+            return match_lc_map;
     }
 
     return match_lc_map;
@@ -225,6 +222,105 @@ void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr match)
     last_loop_closure_ = match;
 }
 
+FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap) const
+{
+    assert(_cap->getFrame() and _cap->getFrame()->getTrajectory());
+
+    int N = getProblem()->getTrajectory()->getFrameMap().size() - 1 - params_loop_closure_icp_->recent_frames_ignored;
+
+    auto map_begin = getProblem()->getTrajectory()->getFrameMap().begin();
+
+    //std::vector<int> idxs;
+    std::vector<bool> done(N, false);
+    FrameBasePtrList frame_list;
+
+    if (params_loop_closure_icp_->candidate_generation == "tree" or
+        params_loop_closure_icp_->candidate_generation == "TREE" or
+        params_loop_closure_icp_->candidate_generation == "Tree")
+    {
+        // generates the series:
+        // 0, 1/2, 1/4, 3/4, 1/8, 3/8, 5/8, 7/8,...
+
+        double frac = N;
+        int checked = 1;
+
+        // initialize with first: 0
+        done.at(0) = true;
+        if (not map_begin->second->getCapturesOfType<CaptureLaser2d>().empty())
+        {
+            //idxs.push_back(0);
+            frame_list.push_back(map_begin->second);
+        }
+
+        // continue with tree
+        while (checked < N and
+               frame_list.size() < params_loop_closure_icp_->max_attempts)
+        {
+            frac *= 0.5;
+            for (auto i = 1; ; i+=2)
+            {
+                int idx = (int)round(i*frac);
+                if (idx >= N)
+                    break;
+
+                if (not done.at(idx))
+                {
+                    done.at(idx) = true;
+                    checked++;
+                    auto frame_idx = std::next(map_begin, idx)->second;
+                    assert(frame_idx);
+                    assert(frame_idx != _cap->getFrame());
+                    if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
+                    {
+                        //idxs.push_back(idx);
+                        frame_list.push_back(frame_idx);
+                    }
+                }
+            }
+        }
+    }
+    else if (params_loop_closure_icp_->candidate_generation == "random" or
+            params_loop_closure_icp_->candidate_generation == "RANDOM" or
+            params_loop_closure_icp_->candidate_generation == "Random")
+    {
+        int checked = 1;
+        while (checked < N and
+               frame_list.size() < params_loop_closure_icp_->max_attempts)
+        {
+            std::random_device rd; // obtain a random number from hardware
+            std::mt19937 gen(rd()); // seed the generator
+            std::uniform_int_distribution<> distr(0, N-1); // define the range
+
+            int idx = distr(gen); // generate random number
+
+            // already added to list
+            if (done.at(idx))
+                continue;
+
+            // mark as done
+            done.at(idx) = true;
+            checked++;
+
+            // consider adding
+            auto frame_idx = std::next(map_begin, idx)->second;
+            assert(frame_idx);
+            assert(frame_idx != _cap->getFrame());
+            if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
+            {
+                //idxs.push_back(idx);
+                frame_list.push_back(frame_idx);
+            }
+        }
+    }
+    else
+    {
+        std::cout << params_loop_closure_icp_->print() << std::endl;
+        throw std::runtime_error("ParamsProcessorLoopClosureIcp::candidate_generation only accepts 'tree' or 'random'");
+    }
+
+    return frame_list;
+}
+
 }
 // Register in the FactorySensor
 #include "core/processor/factory_processor.h"

test/gtest_processor_loop_closure_icp.cpp

@@ -52,6 +52,9 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
             params->frames_ignored_after_loop               = 0;
             params->max_error_threshold                     = 0.1;
             params->min_points_percent                      = 50;
+            params->max_candidates                          = 50;
+            params->max_attempts                            = 50;
+            params->candidate_generation                    = "random";
 
             params->icp_params.use_point_to_line_distance   = 1;
             params->icp_params.max_correspondence_dist      = 1e9;

test/yaml/processor_loop_closure_icp.yaml

@@ -6,7 +6,10 @@ voting_active               : true
 min_features_for_keyframe   : 0
 max_new_features            : 0
 
-# from processor odom ICP
+# from processor loop closure ICP
+candidate_generation        : random
+max_candidates              : 2
+max_attempts                : 10
 cov_factor                  : 1
 use_point_to_line_distance  : true
 max_correspondence_dist     : 2
@@ -17,10 +20,4 @@ outliers_adaptive_order     : 6
 outliers_adaptive_mult      : 7
 max_reading                 : 100
 min_reading                 : 0
-do_compute_covariance       : true
-vfk_min_dist                : 0
-vfk_min_angle               : 0
-vfk_min_time                : 0
-vfk_min_error               : 0
-vfk_max_points              : 0
-initial_guess               : "zero"
+do_compute_covariance       : true
\ No newline at end of file