Skip to content
Snippets Groups Projects
Commit d51f9644 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

improvements in ProcessorLoopClosureIcp

parent de083a0e
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
include/laser/processor/processor_loop_closure_icp.h
+ 1
− 0
View file @ d51f9644
......@@ -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"
......
src/processor/processor_loop_closure_icp.cpp
+ 27
− 22
View file @ d51f9644
......@@ -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;
}
......
test/gtest_processor_loop_closure_icp.cpp
+ 96
− 101
View file @ d51f9644
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment