diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_closeloop_icp.h
index c8edb9e1b506788836f3c8e4e594432b00e1dc8a..edc50d4bcd5ebcdaa14e283ec39354775c01e3ec 100644
--- a/include/laser/processor/processor_closeloop_icp.h
+++ b/include/laser/processor/processor_closeloop_icp.h
@@ -37,6 +37,9 @@ struct ProcessorParamsCloseLoopICP : public ProcessorParamsBase
{
int recent_key_frames_ignored;
int key_frames_to_wait;
+ double max_error_threshold;
+ double min_points_percent;
+
// ICP Params
int use_point_to_line_distance;
double max_correspondence_dist;
@@ -86,37 +89,39 @@ struct ProcessorParamsCloseLoopICP : public ProcessorParamsBase
ProcessorParamsCloseLoopICP(std::string _unique_name, const ParamsServer& _server):
ProcessorParamsBase(_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");
-
- use_point_to_line_distance = _server.getParam<int>(prefix + _unique_name + "/use_point_to_line_distance");
- max_correspondence_dist = _server.getParam<double>(prefix + _unique_name + "/max_correspondence_dist");
- max_iterations = _server.getParam<int>(prefix + _unique_name + "/max_iterations");
- use_corr_tricks = _server.getParam<int>(prefix + _unique_name + "/use_corr_tricks");
- outliers_maxPerc = _server.getParam<double>(prefix + _unique_name + "/outliers_maxPerc");
- outliers_adaptive_order = _server.getParam<double>(prefix + _unique_name + "/outliers_adaptive_order");
- outliers_adaptive_mult = _server.getParam<double>(prefix + _unique_name + "/outliers_adaptive_mult");
-
- do_compute_covariance = _server.getParam<int> (prefix + _unique_name + "/do_compute_covariance");
- max_angular_correction_deg = _server.getParam<double> (prefix + _unique_name + "/max_angular_correction_deg");
- max_linear_correction = _server.getParam<double> (prefix + _unique_name + "/max_linear_correction");
- epsilon_xy = _server.getParam<double> (prefix + _unique_name + "/epsilon_xy");
- epsilon_theta = _server.getParam<double> (prefix + _unique_name + "/epsilon_theta");
- sigma = _server.getParam<double> (prefix + _unique_name + "/sigma");
- restart = _server.getParam<int> (prefix + _unique_name + "/restart");
- restart_threshold_mean_error = _server.getParam<double> (prefix + _unique_name + "/restart_threshold_mean_error");
- restart_dt = _server.getParam<double> (prefix + _unique_name + "/restart_dt");
- restart_dtheta = _server.getParam<double> (prefix + _unique_name + "/restart_dtheta");
- clustering_threshold = _server.getParam<double> (prefix + _unique_name + "/clustering_threshold");
- orientation_neighbourhood = _server.getParam<int> (prefix + _unique_name + "/orientation_neighbourhood");
- do_alpha_test = _server.getParam<int> (prefix + _unique_name + "/do_alpha_test");
- do_alpha_test_thresholdDeg = _server.getParam<double> (prefix + _unique_name + "/do_alpha_test_thresholdDeg");
- do_visibility_test = _server.getParam<int> (prefix + _unique_name + "/do_visibility_test");
- outliers_remove_doubles = _server.getParam<int> (prefix + _unique_name + "/outliers_remove_doubles");
- debug_verify_tricks = _server.getParam<int> (prefix + _unique_name + "/debug_verify_tricks");
- gpm_theta_bin_size_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_theta_bin_size_deg");
- gpm_extend_range_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_extend_range_deg");
- gpm_interval = _server.getParam<double> (prefix + _unique_name + "/gpm_interval");
+ 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");
+ max_error_threshold = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
+ min_points_percent = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
+
+ use_point_to_line_distance = _server.getParam<int> (prefix + _unique_name + "/use_point_to_line_distance");
+ max_correspondence_dist = _server.getParam<double> (prefix + _unique_name + "/max_correspondence_dist");
+ max_iterations = _server.getParam<int> (prefix + _unique_name + "/max_iterations");
+ use_corr_tricks = _server.getParam<int> (prefix + _unique_name + "/use_corr_tricks");
+ outliers_maxPerc = _server.getParam<double> (prefix + _unique_name + "/outliers_maxPerc");
+ outliers_adaptive_order = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_order");
+ outliers_adaptive_mult = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_mult");
+
+ do_compute_covariance = _server.getParam<int> (prefix + _unique_name + "/do_compute_covariance");
+ max_angular_correction_deg = _server.getParam<double> (prefix + _unique_name + "/max_angular_correction_deg");
+ max_linear_correction = _server.getParam<double> (prefix + _unique_name + "/max_linear_correction");
+ epsilon_xy = _server.getParam<double> (prefix + _unique_name + "/epsilon_xy");
+ epsilon_theta = _server.getParam<double> (prefix + _unique_name + "/epsilon_theta");
+ sigma = _server.getParam<double> (prefix + _unique_name + "/sigma");
+ restart = _server.getParam<int> (prefix + _unique_name + "/restart");
+ restart_threshold_mean_error = _server.getParam<double> (prefix + _unique_name + "/restart_threshold_mean_error");
+ restart_dt = _server.getParam<double> (prefix + _unique_name + "/restart_dt");
+ restart_dtheta = _server.getParam<double> (prefix + _unique_name + "/restart_dtheta");
+ clustering_threshold = _server.getParam<double> (prefix + _unique_name + "/clustering_threshold");
+ orientation_neighbourhood = _server.getParam<int> (prefix + _unique_name + "/orientation_neighbourhood");
+ do_alpha_test = _server.getParam<int> (prefix + _unique_name + "/do_alpha_test");
+ do_alpha_test_thresholdDeg = _server.getParam<double> (prefix + _unique_name + "/do_alpha_test_thresholdDeg");
+ do_visibility_test = _server.getParam<int> (prefix + _unique_name + "/do_visibility_test");
+ outliers_remove_doubles = _server.getParam<int> (prefix + _unique_name + "/outliers_remove_doubles");
+ debug_verify_tricks = _server.getParam<int> (prefix + _unique_name + "/debug_verify_tricks");
+ gpm_theta_bin_size_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_theta_bin_size_deg");
+ gpm_extend_range_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_extend_range_deg");
+ gpm_interval = _server.getParam<double> (prefix + _unique_name + "/gpm_interval");
// Not implemented yet in CSM
// laser_x = _server.getParam<double> (prefix + _unique_name + "/laser_x");
@@ -142,7 +147,9 @@ struct ProcessorParamsCloseLoopICP : public ProcessorParamsBase
{
return "\n" + ProcessorParamsBase::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";
+ + "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";
}
};
struct CapturesAligned{
@@ -163,6 +170,9 @@ class ProcessorCloseLoopICP : public ProcessorBase
//Closeloop parameters
int recent_key_frames_ignored_;
int key_frames_to_wait_;
+ double max_error_threshold;
+ double min_points_percent;
+
int key_frames_skipped_;
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp
index 816fe62ffa0b7c2007d8cf10da8438228ed655fe..6b56ed474bce3a127470877a5ce983b228c37c2f 100644
--- a/src/processor/processor_closeloop_icp.cpp
+++ b/src/processor/processor_closeloop_icp.cpp
@@ -16,6 +16,8 @@ ProcessorCloseLoopICP::ProcessorCloseLoopICP(ProcessorParamsCloseLoopICPPtr _par
recent_key_frames_ignored_ = _params->recent_key_frames_ignored;
key_frames_to_wait_ = _params->key_frames_to_wait;
key_frames_skipped_ = 0;
+ max_error_threshold = _params->max_error_threshold;
+ min_points_percent = _params->min_points_percent;
icp_params_.use_point_to_line_distance = _params->use_point_to_line_distance;
icp_params_.max_correspondence_dist = _params->max_correspondence_dist;
@@ -204,14 +206,14 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
laserscanutils::icpOutput trf =
icp_tools_ptr_->align(capture_laser_own->getScan(), capture_laser_other->getScan(), sensor_own->getScanParams(),
sensor_other->getScanParams(), this->icp_params_, transform_guess);
- Scalar score_normalized =
+ Scalar points_coeff_used =
((Scalar)trf.nvalid / (Scalar)min(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size()));
Scalar mean_error = trf.error / trf.nvalid;
WOLF_INFO("DBG ------------------------------");
- WOLF_INFO("DBG valid? ", trf.valid, " m_er ", mean_error, " ", score_normalized*100, "% own_id: ", _keyframe_own->id(), " other_id: ", _keyframe_other->id());
+ WOLF_INFO("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used*100, "% own_id: ", _keyframe_own->id(), " other_id: ", _keyframe_other->id());
WOLF_INFO("DBG own_POSE: ", _keyframe_own->getState().transpose(), " other_POSE: ", _keyframe_other->getState().transpose(), " ICP_guess: ", transform_guess.transpose(), " ICP_trf: ", trf.res_transf.transpose());
// WOLF_INFO("Covariance \n", trf.res_covar);
- if (trf.valid == 1 and mean_error < 0.01 and score_normalized > 0.5) {
+ if (trf.valid == 1 and mean_error < max_error_threshold and points_coeff_used*100 > min_points_percent) {
WOLF_INFO("DBG ADDED ", _keyframe_own->id(), " and ", _keyframe_other->id());
captures_candidates.emplace(mean_error, CapturesAligned({capture_laser_own, capture_laser_other, trf}));
}else WOLF_INFO("DBG DISCARDED ", _keyframe_own->id(), " and ", _keyframe_other->id());