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

icp validation

parent b9d651b2
No related branches found
No related tags found
No related merge requests found
Pipeline #18683 passed
Hide whitespace changes
Inline Side-by-side
include/laser_scan_utils/icp.h
+ 13
− 2
View file @ 1c5d27d2
......@@ -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
......
src/icp.cpp
+ 71
− 48
View file @ 1c5d27d2
......@@ -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;
}
......
test/gtest_icp.cpp
+ 32
− 24
View file @ 1c5d27d2
......@@ -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);
......
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