From b9d651b25baf1aa683ab19676517c628f94d5fcf Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 28 Jun 2024 12:20:57 +0200
Subject: [PATCH] icp more outputs
---
include/laser_scan_utils/icp.h | 4 +++-
src/icp.cpp | 8 +++++---
2 files changed, 8 insertions(+), 4 deletions(-)
diff --git a/include/laser_scan_utils/icp.h b/include/laser_scan_utils/icp.h
index ec849b4..6cfbdb6 100644
--- a/include/laser_scan_utils/icp.h
+++ b/include/laser_scan_utils/icp.h
@@ -39,6 +39,8 @@ namespace laserscanutils
Eigen::Matrix3s res_covar; // Covariance of the transformation
int nvalid; // Number of valid correspondences in the match
double error; // Total correspondence error
+ double mean_error; // Average error (total error / nvalid)
+ double points_ratio; // Ratio of valid correspondences of totall amount of points
unsigned int attempts; // Number of ICP calls to obtain a valid result (<= params.icp_attempts)
};
@@ -124,7 +126,7 @@ namespace laserscanutils
double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm
// Attempts ------------------------------------------------------------------
- unsigned int icp_attempts; // number of icp attempts if result fails (not valid or error > restart_threshold_mean_error)
+ unsigned int icp_attempts; // number of icp attempts if result fails (not valid or error > restart_threshold_mean_error)
double perturbation_new_attempts; // perturbation noise amplitude applied to initial guess in new attempts
void print() const
diff --git a/src/icp.cpp b/src/icp.cpp
index b32b581..cb23c4f 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -178,6 +178,8 @@ icpOutput ICP::align(const LaserScan &_current_ls,
{
result.nvalid = csm_output.nvalid;
result.error = csm_output.error;
+ result.mean_error = csm_output.error / csm_output.nvalid;
+ result.points_ratio = ((double)csm_output.nvalid) / ((double)num_rays);
result.res_transf(0) = csm_output.x[0];
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
@@ -218,13 +220,13 @@ icpOutput ICP::align(const LaserScan &_current_ls,
{
std::cout << "Invalid result, trying again!" << std::endl;
}
- if (_icp_params.verbose and result.error > _icp_params.restart_threshold_mean_error and result.attempts < _icp_params.icp_attempts)
+ if (_icp_params.verbose and result.mean_error > _icp_params.restart_threshold_mean_error and result.attempts < _icp_params.icp_attempts)
{
- std::cout << "Error too big: " << result.error
+ std::cout << "Error too big: " << result.mean_error
<< " ( should be < "
<< _icp_params.restart_threshold_mean_error << "). Trying again!" << std::endl;
}
- } while ((not result.valid or result.error > _icp_params.restart_threshold_mean_error) and
+ } while ((not result.valid or result.mean_error > _icp_params.restart_threshold_mean_error) and
result.attempts < _icp_params.icp_attempts);
return result;
--
GitLab