diff --git a/include/laser_scan_utils/icp.h b/include/laser_scan_utils/icp.h
index 6cfbdb6cbada3619c6347473c7f4786e389606c5..be8676cd2f4bf25d3b11a1b39a4eab7c3535b2f9 100644
--- a/include/laser_scan_utils/icp.h
+++ b/include/laser_scan_utils/icp.h
@@ -34,7 +34,8 @@ namespace laserscanutils
struct icpOutput
{
- bool valid; // If the result is valid
+ bool valid; // If the result is valid (converged & mean_error < max_mean_error & points_ratio > min_points_ratio)
+ bool converged; // If the algorithm found a solution
Eigen::Vector3s res_transf; // Transformation found
Eigen::Matrix3s res_covar; // Covariance of the transformation
int nvalid; // Number of valid correspondences in the match
@@ -129,6 +130,10 @@ namespace laserscanutils
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
+ // Validation ----------------------------------------------------------------
+ double max_mean_error; // mean_error threshold to consider the solution not valid
+ double min_points_ratio; // points ratio threshold to consider the solution valid
+
void print() const
{
std::cout << "verbose: " << std::to_string(verbose) << std::endl;
@@ -162,6 +167,10 @@ namespace laserscanutils
std::cout << "do_compute_covariance: " << std::to_string(do_compute_covariance) << std::endl;
std::cout << "cov_factor: " << std::to_string(cov_factor) << std::endl;
std::cout << "cov_max_eigv_factor: " << std::to_string(cov_max_eigv_factor) << std::endl;
+ std::cout << "icp_attempts: " << std::to_string(icp_attempts) << std::endl;
+ std::cout << "perturbation_new_attempts: " << std::to_string(perturbation_new_attempts) << std::endl;
+ std::cout << "max_mean_error: " << std::to_string(max_mean_error) << std::endl;
+ std::cout << "min_points_ratio: " << std::to_string(min_points_ratio) << std::endl;
}
};
@@ -198,7 +207,9 @@ namespace laserscanutils
5, // double cov_factor
2, // double cov_max_eigv_factor
1, // unsigned int attempts
- 1e-1 // double perturbation_new_attempts
+ 1e-1, // double perturbation_new_attempts
+ 1e-2, // max_mean_error
+ 0.75 // min_points_ratio
};
class ICP
diff --git a/src/icp.cpp b/src/icp.cpp
index cb23c4fd2eb028e59a69d8a06e0ed64761f12699..c627cdf13b79c2c9fc8dc773024ce5467d432cdc 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -166,68 +166,91 @@ icpOutput ICP::align(const LaserScan &_current_ls,
try
{
sm_icp(&csm_input, &csm_output);
- result.valid = csm_output.valid == 1;
- }
- catch (...)
- {
- result.valid = false;
- }
+ result.converged = csm_output.valid == 1;
+ result.valid = result.converged;
- // VALID --> copy output (and modify covariance)
- if (result.valid == 1)
- {
- 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];
-
- if (csm_input.do_compute_covariance)
+ // Further validation
+ if (result.converged)
{
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- result.res_covar(i, j) = _icp_params.cov_factor *
- csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];
+ 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);
- // Grow covariance in the biggest eigenvalue direction
- if (_icp_params.cov_max_eigv_factor - 1 > 1e-6)
+ if (result.mean_error > _icp_params.max_mean_error or
+ result.points_ratio < _icp_params.min_points_ratio)
{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(result.res_covar.topLeftCorner<2, 2>());
-
- if (eigensolver.info() == Eigen::Success)
- {
- Eigen::Vector2d eigvs = eigensolver.eigenvalues();
- Eigen::Index maxRow, maxCol;
- float max_eigv = eigvs.maxCoeff(&maxRow, &maxCol);
-
- eigvs(maxRow) = _icp_params.cov_max_eigv_factor * max_eigv;
- result.res_covar.topLeftCorner<2, 2>() = eigensolver.eigenvectors() *
- eigvs.asDiagonal() *
- eigensolver.eigenvectors().transpose();
- }
+ result.valid = false;
+ if (_icp_params.verbose)
+ std::cout << "Validation NOT passed! mean_error: "
+ << result.mean_error
+ << " (should be < "
+ << _icp_params.max_mean_error
+ << "). points_ratio: "
+ << result.points_ratio
+ << " (should be > "
+ << _icp_params.min_points_ratio
+ << ")." << std::endl;
}
}
}
- else
+ catch (...)
{
- // std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
- result.res_transf = _initial_guess;
- result.res_covar = Eigen::Matrix3s::Identity();
+ if (_icp_params.verbose)
+ std::cout << "ICP failed (catch)" << std::endl;
+
+ result.converged = false;
+ result.valid = false;
}
- if (_icp_params.verbose and not result.valid and result.attempts < _icp_params.icp_attempts)
+
+ if (_icp_params.verbose and not result.valid)
{
std::cout << "Invalid result, trying again!" << std::endl;
}
- if (_icp_params.verbose and result.mean_error > _icp_params.restart_threshold_mean_error and result.attempts < _icp_params.icp_attempts)
+
+ } while (not result.valid and result.attempts < _icp_params.icp_attempts);
+
+ // if valid, copy values and grow covariance
+ if (result.valid)
+ {
+ result.res_transf(0) = csm_output.x[0];
+ result.res_transf(1) = csm_output.x[1];
+ result.res_transf(2) = csm_output.x[2];
+
+ if (csm_input.do_compute_covariance)
{
- std::cout << "Error too big: " << result.mean_error
- << " ( should be < "
- << _icp_params.restart_threshold_mean_error << "). Trying again!" << std::endl;
+ for (int i = 0; i < 3; ++i)
+ for (int j = 0; j < 3; ++j)
+ result.res_covar(i, j) = _icp_params.cov_factor *
+ csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];
+
+ // Grow covariance in the biggest eigenvalue direction
+ if (_icp_params.cov_max_eigv_factor - 1 > 1e-6)
+ {
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(result.res_covar.topLeftCorner<2, 2>());
+
+ if (eigensolver.info() == Eigen::Success)
+ {
+ Eigen::Vector2d eigvs = eigensolver.eigenvalues();
+ Eigen::Index maxRow, maxCol;
+ float max_eigv = eigvs.maxCoeff(&maxRow, &maxCol);
+
+ eigvs(maxRow) = _icp_params.cov_max_eigv_factor * max_eigv;
+ result.res_covar.topLeftCorner<2, 2>() = eigensolver.eigenvectors() *
+ eigvs.asDiagonal() *
+ eigensolver.eigenvectors().transpose();
+ }
+ }
}
- } while ((not result.valid or result.mean_error > _icp_params.restart_threshold_mean_error) and
- result.attempts < _icp_params.icp_attempts);
+ }
+ else
+ {
+ if (_icp_params.verbose)
+ std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
+
+ result.res_transf = _initial_guess;
+ result.res_covar = Eigen::Matrix3s::Identity();
+ }
return result;
}
diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index 991ffe0dafc10e53337296c09f97a61f9e66a895..e6d39686f4a0cebde5e0ac1f49e9b75c927b808f 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -280,9 +280,11 @@ TEST(TestIcp, TestIcpSame1)
icp_params,
Eigen::Vector3d::Zero());
- std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
- std::cout << " icp error: " << icp_output.error << std::endl;
- std::cout << " icp valid: " << icp_output.valid << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -297,10 +299,11 @@ TEST(TestIcp, TestIcpSame1)
initial_guess);
std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
- std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
- std::cout << " icp error: " << icp_output.error << std::endl;
- std::cout << " icp valid: " << icp_output.valid << std::endl;
- std::cout << " icp attempts: " << icp_output.attempts << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -331,9 +334,11 @@ TEST(TestIcp, TestIcpSame2)
icp_params,
Eigen::Vector3d::Zero());
- std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
- std::cout << " icp error: " << icp_output.error << std::endl;
- std::cout << " icp valid: " << icp_output.valid << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -348,10 +353,11 @@ TEST(TestIcp, TestIcpSame2)
initial_guess);
std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
- std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
- std::cout << " icp error: " << icp_output.error << std::endl;
- std::cout << " icp valid: " << icp_output.valid << std::endl;
- std::cout << " icp attempts: " << icp_output.attempts << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points_ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -370,9 +376,9 @@ TEST(TestIcp, TestIcp1)
auto icp_params = icp_params_default;
icp_params.icp_attempts = n_attempts;
-
+
double pert = 0.0;
-
+
for (auto i = 0; i < N; i++)
{
// Random problem
@@ -392,10 +398,11 @@ TEST(TestIcp, TestIcp1)
std::cout << " pose_tar: " << pose_tar.transpose() << std::endl;
std::cout << " groundtruth: " << pose_d.transpose() << std::endl;
std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
- std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
- std::cout << " icp error: " << icp_output.error << std::endl;
- std::cout << " icp valid: " << icp_output.valid << std::endl;
- std::cout << " icp attempts: " << icp_output.attempts << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
ASSERT_TRUE(icp_output.valid);
@@ -437,10 +444,11 @@ TEST(TestIcp, TestIcp10)
std::cout << " pose_tar: " << pose_tar.transpose() << std::endl;
std::cout << " groundtruth: " << pose_d.transpose() << std::endl;
std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
- std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
- std::cout << " icp error: " << icp_output.error << std::endl;
- std::cout << " icp valid: " << icp_output.valid << std::endl;
- std::cout << " icp attempts: " << icp_output.attempts << std::endl;
+ std::cout << " output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " mean error: " << icp_output.mean_error << std::endl;
+ std::cout << " points ratio: " << icp_output.points_ratio << std::endl;
+ std::cout << " valid: " << icp_output.valid << std::endl;
+ std::cout << " attempts: " << icp_output.attempts << std::endl;
std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
ASSERT_TRUE(icp_output.valid);