diff --git a/include/laser/processor/params_icp.h b/include/laser/processor/params_icp.h
index b93bfb469f45db6e98b419395d55200db82fc6fd..3e0d9ef405eaffe9eeee5f6bc26b140c15ac3c02 100644
--- a/include/laser/processor/params_icp.h
+++ b/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 {
diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index b1ac1868cfe669ede330bb20b7c2579e7a106f23..8d95796f4c6795b9bbdae8e1b0b7ff88c6c12261 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/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;
};
}
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index b7f4b651a20c28f6f067b9647851d67e0fb83f34..a609404ec3ce49042b23b7f668a4a5b7473e06e4 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/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"
diff --git a/test/gtest_processor_loop_closure_icp.cpp b/test/gtest_processor_loop_closure_icp.cpp
index 998bc2e77021e116347ac20be0f34577082a47d8..12f33d6c9568a12d52233393fc21d7ded7c6063b 100644
--- a/test/gtest_processor_loop_closure_icp.cpp
+++ b/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;
diff --git a/test/yaml/processor_loop_closure_icp.yaml b/test/yaml/processor_loop_closure_icp.yaml
index a553959b916989ffb66b1a70f9afe900f3a21c77..c5125820901c97e44dc0066bdbe662bbec732eb7 100644
--- a/test/yaml/processor_loop_closure_icp.yaml
+++ b/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