diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index 8d95796f4c6795b9bbdae8e1b0b7ff88c6c12261..e6e92bdad1ec9d70aa5394b6e0162276da02a9ad 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -25,6 +25,7 @@
/**************************
* WOLF includes *
**************************/
+#include "laser/internal/config.h"
#include "core/common/wolf.h"
#include "core/processor/processor_loop_closure.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index a609404ec3ce49042b23b7f668a4a5b7473e06e4..d962983064fd578e3687503e7428fa71d1f3ff4e 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -134,19 +134,7 @@ MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_re
Rotation2Dd R(T_s_ref_s_tar.rotation());
transform_guess(2) = R.angle();
- // WOLF_DEBUG("LOOP CLOSURE: Aligning key frames: ", _keyframe_tar->id(), " and ", frm_ref->id(), "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
- // WOLF_DEBUG("Sensor own scan params\n");
- // sen_tar->getScanParams().print();
- // WOLF_DEBUG("Sensor other scan params\n");
- // sensor_ref->getScanParams().print();
- // WOLF_DEBUG("Icp params\n");
- // WOLF_DEBUG("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
- // "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance,
- // "\n max_iterations: ", this->icp_params_.max_iterations,
- // "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
- // "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order,
- // "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
- WOLF_DEBUG("DBG Attempting to close loop with ", frm_tar->id(), " and ", frm_ref->id());
+ WOLF_DEBUG("Attempting to close loop between frames ", frm_tar->id(), " and ", frm_ref->id());
try
{
icpOutput icp_out = icp_tools_ptr_->align(cap_tar->getScan(),
@@ -161,13 +149,15 @@ MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_re
double mean_error = icp_out.error / icp_out.nvalid;
WOLF_DEBUG("DBG ------------------------------");
- WOLF_DEBUG("DBG valid? ", icp_out.valid,
- " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", frm_tar->id(),
- " other_id: ", frm_ref->id());
- WOLF_DEBUG("DBG own_POSE: ", frm_tar->getState().vector("PO").transpose(),
+ WOLF_DEBUG_COND(not icp_out.valid, "ICP not valid");
+ WOLF_DEBUG_COND(icp_out.valid,
+ "ICP valid ",
+ " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", frm_tar->id(),
+ " other_id: ", frm_ref->id());
+ WOLF_DEBUG("own_POSE: ", frm_tar->getState().vector("PO").transpose(),
" other_POSE: ", frm_ref->getState().vector("PO").transpose(),
- " Icp_guess: ", transform_guess.transpose(),
- " Icp_trf: ", icp_out.res_transf.transpose());
+ " icp_guess: ", transform_guess.transpose(),
+ " icp_trf: ", icp_out.res_transf.transpose());
// WOLF_DEBUG("Covariance \n", icp_out.res_covar);
// Valid output
@@ -175,7 +165,7 @@ MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_re
mean_error < params_loop_closure_icp_->max_error_threshold and
points_coeff_used * 100 > params_loop_closure_icp_->min_points_percent)
{
- WOLF_DEBUG("DBG MATCH CONFIRMED ", frm_tar->id(), " and ", frm_ref->id());
+ WOLF_DEBUG("MATCH CONFIRMED ", frm_tar->id(), " and ", frm_ref->id());
auto match = std::make_shared<MatchLoopClosureIcp>();
match->capture_reference = cap_ref;
@@ -188,7 +178,7 @@ MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_re
}
else
{
- WOLF_DEBUG("DBG DISCARDED ", frm_tar->id(), " and ", frm_ref->id());
+ WOLF_DEBUG("MATCH DISCARDED ", frm_tar->id(), " and ", frm_ref->id());
return nullptr;
}
@@ -228,6 +218,11 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
int N = getProblem()->getTrajectory()->getFrameMap().size() - 1 - params_loop_closure_icp_->recent_frames_ignored;
+ WOLF_DEBUG("ProcessorLoopClosureIcp::generateSearchList N = ", N);
+
+ if (N <= 0)
+ return FrameBasePtrList();
+
auto map_begin = getProblem()->getTrajectory()->getFrameMap().begin();
//std::vector<int> idxs;
@@ -283,7 +278,7 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
params_loop_closure_icp_->candidate_generation == "RANDOM" or
params_loop_closure_icp_->candidate_generation == "Random")
{
- int checked = 1;
+ int checked = 0;
while (checked < N and
frame_list.size() < params_loop_closure_icp_->max_attempts)
{
@@ -318,6 +313,16 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
throw std::runtime_error("ParamsProcessorLoopClosureIcp::candidate_generation only accepts 'tree' or 'random'");
}
+ WOLF_DEBUG("ProcessorLoopClosureIcp::generateSearchList: in mode ",
+ params_loop_closure_icp_->candidate_generation,
+ " resulting list:");
+ #ifdef _WOLF_DEBUG
+ std::cout << "\t";
+ for (auto frm : frame_list)
+ std::cout << frm->id() << ", ";
+ std::cout << std::endl;
+ #endif
+
return frame_list;
}
diff --git a/test/gtest_processor_loop_closure_icp.cpp b/test/gtest_processor_loop_closure_icp.cpp
index 12f33d6c9568a12d52233393fc21d7ded7c6063b..9c60e1481de880f30994f609325fbddcbb792504 100644
--- a/test/gtest_processor_loop_closure_icp.cpp
+++ b/test/gtest_processor_loop_closure_icp.cpp
@@ -26,6 +26,7 @@
#include "laser/processor/processor_loop_closure_icp.h"
#include <stdlib.h>
+#include <random>
using namespace wolf;
using namespace Eigen;
@@ -56,35 +57,35 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
params->max_attempts = 50;
params->candidate_generation = "random";
- params->icp_params.use_point_to_line_distance = 1;
+ params->icp_params.use_point_to_line_distance = true;
params->icp_params.max_correspondence_dist = 1e9;
params->icp_params.max_iterations = 1e3;
- params->icp_params.use_corr_tricks = 1;
- params->icp_params.outliers_maxPerc = 5;
- params->icp_params.outliers_adaptive_order = 6;
- params->icp_params.outliers_adaptive_mult = 7;
- params->icp_params.do_compute_covariance = 1;
+ params->icp_params.use_corr_tricks = true;
+ params->icp_params.outliers_maxPerc = 0.8;
+ params->icp_params.outliers_adaptive_order = 0.7;
+ params->icp_params.outliers_adaptive_mult = 2;
+ params->icp_params.do_compute_covariance = true;
params->icp_params.cov_factor = 1;
- params->icp_params.max_angular_correction_deg = 1e9;
- params->icp_params.max_linear_correction = 1e9;
- params->icp_params.epsilon_xy = 0.1;
- params->icp_params.epsilon_theta = 0.1;
+ params->icp_params.max_angular_correction_deg = 1.5;
+ params->icp_params.max_linear_correction = 10;
+ params->icp_params.epsilon_xy = 0.01;
+ params->icp_params.epsilon_theta = 0.01;
params->icp_params.sigma = 0.1;
- params->icp_params.restart = 0;
+ params->icp_params.restart = false;
params->icp_params.restart_threshold_mean_error = 0;
params->icp_params.restart_dt = 0;
params->icp_params.restart_dtheta = 0;
- params->icp_params.clustering_threshold = 0;
- params->icp_params.orientation_neighbourhood = 0;
- params->icp_params.do_alpha_test = 0;
+ params->icp_params.clustering_threshold = 0.2;
+ params->icp_params.orientation_neighbourhood = 5;
+ params->icp_params.do_alpha_test = false;
params->icp_params.do_alpha_test_thresholdDeg = 0;
- params->icp_params.do_visibility_test = 0;
- params->icp_params.outliers_remove_doubles = 0;
- params->icp_params.debug_verify_tricks = 0;
+ params->icp_params.do_visibility_test = false;
+ params->icp_params.outliers_remove_doubles = false;
+ params->icp_params.debug_verify_tricks = false;
params->icp_params.min_reading = 0;
params->icp_params.max_reading = 100;
- params->icp_params.use_ml_weights = 0;
- params->icp_params.use_sigma_weights = 0;
+ params->icp_params.use_ml_weights = false;
+ params->icp_params.use_sigma_weights = false;
processor = std::static_pointer_cast<ProcessorLoopClosureIcp>(problem->installProcessor("ProcessorLoopClosureIcp",
"prc icp",
@@ -101,6 +102,9 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
// shape: 0-2 half circles, 1-square, 2-star-like
int shape = frame_idx % 3;
+ WOLF_DEBUG_COND(shape == 0, "Generating ranges: HALF CIRCLES");
+ WOLF_DEBUG_COND(shape == 1, "Generating ranges: SQUARE");
+ WOLF_DEBUG_COND(shape == 2, "Generating ranges: STAR-LIKE");
std::vector<float> ranges(n_ranges);
for (auto i = 0; i < n_ranges; i++)
@@ -114,15 +118,18 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
ranges[i] = 2.0 * sqrt(std::pow(cos(i * lidar->getScanParams().angle_step_),2) +
std::pow(sin(i * lidar->getScanParams().angle_step_),2));
- // STAR-LIKE (2m to 10m)
+ // STAR-LIKE (3m to 10m)
else if (shape == 2)
- ranges[i] = 2.0 + (i % (n_ranges / 6)) * 8.0 / (n_ranges / 6);
+ ranges[i] = 3.0 + (i % (n_ranges / 6)) * 8.0 / (n_ranges / 6);
}
// rotate randomly
- srand (time(NULL)); // initialize random seed
- int init = rand() % n_ranges; // half starting randomly between 0 and n_ranges-1
+ std::random_device rd; // obtain a random number from hardware
+ std::mt19937 gen(rd()); // seed the generator
+ std::uniform_int_distribution<> distr(0, 5); // define the range of random
+ int init = distr(gen); // generate random number
std::rotate(ranges.begin(),ranges.begin()+init,ranges.end());
+ WOLF_DEBUG("Rotated ranges in ", init, " beams");
return ranges;
}
@@ -138,6 +145,48 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
// new capture
return CaptureBase::emplace<CaptureLaser2d>(frame, frame->getTimeStamp(), lidar, _ranges);
}
+
+ void test()
+ {
+ TimeStamp t(0.0);
+
+ auto i_loop = 3;
+ auto N = 6;
+ while (i_loop < processor->getParams()->recent_frames_ignored)
+ {
+ i_loop += 3;
+ N += 3;
+ }
+
+ for (int i = 0; i < N; i++)
+ {
+ auto frm = emplaceFrame(t, Vector3d::Zero());
+ frm->perturb();
+ auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+
+ processor->keyFrameCallback(frm);
+
+ WOLF_INFO_COND(i == i_loop, "Loop should have been detected:");
+ problem->print(4,1,1,1);
+
+ if (i == i_loop)
+ {
+ ASSERT_EQ(cap->getFeatureList().size(), 1);
+ ASSERT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
+ ASSERT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
+
+ i_loop += 1 + processor->getParams()->frames_ignored_after_loop;
+ }
+ else
+ {
+ EXPECT_TRUE(cap->getFeatureList().empty());
+ EXPECT_TRUE(frm->getConstrainedByList().empty());
+ }
+ t += 1;
+ }
+ }
};
@@ -146,95 +195,41 @@ TEST_F(ProcessorLoopClosureIcp_Test, setup)
ASSERT_TRUE(problem->check());
}
-TEST_F(ProcessorLoopClosureIcp_Test, loop_closure)
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_random)
{
- TimeStamp t(0.0);
-
- for (int i = 0; i < 6; i++)
- {
- auto frm = emplaceFrame(t, Vector3d::Zero());
- auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
-
- processor->keyFrameCallback(frm);
- if (i > 2)
- {
- EXPECT_EQ(cap->getFeatureList().size(), 1);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
- EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
- problem->print(4,1,1,1);
- }
- else
- {
- EXPECT_TRUE(cap->getFeatureList().empty());
- EXPECT_TRUE(frm->getConstrainedByList().empty());
- problem->print(4,1,1,1);
- }
- t += 1;
- }
+ test();
}
-TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_ignore_previous)
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_ignore_previous_random)
{
- processor->getParams()->recent_frames_ignored = 3;
-
- TimeStamp t(0.0);
-
- for (int i = 0; i < 6; i++)
- {
- auto frm = emplaceFrame(t, Vector3d::Zero());
- auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
-
- processor->keyFrameCallback(frm);
- if (i > 5)
- {
- EXPECT_EQ(cap->getFeatureList().size(), 1);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
- EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
- problem->print(4,1,1,1);
- }
- else
- {
- EXPECT_TRUE(cap->getFeatureList().empty());
- EXPECT_TRUE(frm->getConstrainedByList().empty());
- problem->print(4,1,1,1);
- }
- t += 1;
- }
+ processor->getParams()->recent_frames_ignored = 4;
+ test();
}
-TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_frames_ignored_after_loop)
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_frames_ignored_after_loop_random)
{
processor->getParams()->frames_ignored_after_loop = 2;
+ test();
+}
- TimeStamp t(0.0);
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_tree)
+{
+ processor->getParams()->candidate_generation = "tree";
+ test();
+}
- for (int i = 0; i < 6; i++)
- {
- auto frm = emplaceFrame(t, Vector3d::Zero());
- auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_ignore_previous_tree)
+{
+ processor->getParams()->candidate_generation = "tree";
+ processor->getParams()->recent_frames_ignored = 4;
+ test();
+}
- processor->keyFrameCallback(frm);
- if (i == 3)
- {
- EXPECT_EQ(cap->getFeatureList().size(), 1);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
- EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
- EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
- problem->print(4,1,1,1);
- }
- else
- {
- EXPECT_TRUE(cap->getFeatureList().empty());
- EXPECT_TRUE(frm->getConstrainedByList().empty());
- problem->print(4,1,1,1);
- }
- t += 1;
- }
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_frames_ignored_after_loop_tree)
+{
+ processor->getParams()->candidate_generation = "tree";
+ processor->getParams()->frames_ignored_after_loop = 2;
+ test();
}
int main(int argc, char **argv)