diff --git a/include/laser_scan_utils/icp.h b/include/laser_scan_utils/icp.h
index c05911203fa1000a5d0e9391bd9526fd80a147f2..ec849b4ad01a660b7dda27dd0d525c65a01053d6 100644
--- a/include/laser_scan_utils/icp.h
+++ b/include/laser_scan_utils/icp.h
@@ -39,6 +39,7 @@ namespace laserscanutils
Eigen::Matrix3s res_covar; // Covariance of the transformation
int nvalid; // Number of valid correspondences in the match
double error; // Total correspondence error
+ unsigned int attempts; // Number of ICP calls to obtain a valid result (<= params.icp_attempts)
};
struct icpParams
@@ -122,6 +123,10 @@ namespace laserscanutils
double cov_factor; // Factor multiplying the cov output of csm
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)
+ double perturbation_new_attempts; // perturbation noise amplitude applied to initial guess in new attempts
+
void print() const
{
std::cout << "verbose: " << std::to_string(verbose) << std::endl;
@@ -170,7 +175,7 @@ namespace laserscanutils
1e-4, // double epsilon_xy
1e-3, // double epsilon_theta
false, // bool restart
- 0, // double restart_threshold_mean_error
+ 1e-1, // double restart_threshold_mean_error
0, // double restart_dt
0, // double restart_dtheta
0.023, // double min_reading
@@ -189,7 +194,9 @@ namespace laserscanutils
0.2, // double sigma
true, // bool do_compute_covariance
5, // double cov_factor
- 2 // double cov_max_eigv_factor
+ 2, // double cov_max_eigv_factor
+ 1, // unsigned int attempts
+ 1e-1 // double perturbation_new_attempts
};
class ICP
diff --git a/src/icp.cpp b/src/icp.cpp
index 354a905d86521fac7a72650b6cac461a32849e03..066786e41941cf06cd10ef7b237bf9c66edd8b28 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -26,66 +26,63 @@
using namespace laserscanutils;
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
-std::mt19937 generator (seed);
+std::mt19937 generator(seed);
std::uniform_real_distribution<double> dis(0.0, 1.0);
class LDWrapper
{
- public:
- LDP laser_data;
+public:
+ LDP laser_data;
- LDWrapper(const LaserScan& scan, const LaserScanParams& scan_params)
+ LDWrapper(const LaserScan &scan, const LaserScanParams &scan_params)
+ {
+ int num_rays = scan.ranges_raw_.size();
+ laser_data = ld_alloc_new(num_rays);
+ int i = 0;
+ for (auto it : scan.ranges_raw_)
{
- int num_rays = scan.ranges_raw_.size();
- laser_data = ld_alloc_new(num_rays);
- int i = 0;
- for(auto it : scan.ranges_raw_)
- {
- laser_data->theta[i] = scan_params.angle_min_ + i*scan_params.angle_step_;
+ laser_data->theta[i] = scan_params.angle_min_ + i * scan_params.angle_step_;
- if (scan_params.range_min_ <= it and
- it <= scan_params.range_max_ and
- 0 < it and it < 100)
- {
- laser_data->readings[i] = it;
- laser_data->valid[i] = 1;
- }
- else
- {
- laser_data->readings[i] = NAN;
- laser_data->valid[i] = 0;
- }
- laser_data->cluster[i] = -1;
- ++i;
+ if (scan_params.range_min_ <= it and
+ it <= scan_params.range_max_ and
+ 0 < it and it < 100)
+ {
+ laser_data->readings[i] = it;
+ laser_data->valid[i] = 1;
}
+ else
+ {
+ laser_data->readings[i] = NAN;
+ laser_data->valid[i] = 0;
+ }
+ laser_data->cluster[i] = -1;
+ ++i;
+ }
- laser_data->min_theta = laser_data->theta[0];
- laser_data->max_theta = laser_data->theta[num_rays-1];
-
- laser_data->odometry[0] = 0.0;
- laser_data->odometry[1] = 0.0;
- laser_data->odometry[2] = 0.0;
+ laser_data->min_theta = laser_data->theta[0];
+ laser_data->max_theta = laser_data->theta[num_rays - 1];
- laser_data->true_pose[0] = 0.0;
- laser_data->true_pose[1] = 0.0;
- laser_data->true_pose[2] = 0.0;
+ laser_data->odometry[0] = 0.0;
+ laser_data->odometry[1] = 0.0;
+ laser_data->odometry[2] = 0.0;
- }
+ laser_data->true_pose[0] = 0.0;
+ laser_data->true_pose[1] = 0.0;
+ laser_data->true_pose[2] = 0.0;
+ }
- ~LDWrapper()
- {
- ld_free(laser_data);
- }
+ ~LDWrapper()
+ {
+ ld_free(laser_data);
+ }
};
ICP::ICP()
{
-
}
ICP::~ICP()
{
-
}
icpOutput ICP::align(const LaserScan &_current_ls,
@@ -147,69 +144,97 @@ icpOutput ICP::align(const LaserScan &_current_ls,
csm_input.use_ml_weights = _icp_params.use_ml_weights ? 1 : 0;
csm_input.use_sigma_weights = _icp_params.use_sigma_weights ? 1 : 0;
- try
- {
- sm_icp(&csm_input, &csm_output);
- }
- catch(...)
- {
- icpOutput result{};
- result.valid = false;
- return result;
- }
-
icpOutput result{};
- result.nvalid = csm_output.nvalid;
- result.valid = csm_output.valid == 1;
- result.error = csm_output.error;
-
- if (result.valid)
+ result.attempts = 0;
+ do
{
- result.res_transf(0) = csm_output.x[0];
- result.res_transf(1) = csm_output.x[1];
- result.res_transf(2) = csm_output.x[2];
+ // perturb from 2nd attempt
+ if (result.attempts > 0)
+ {
+ Eigen::Vector3d initial_guess_perturbed = _initial_guess + Eigen::Vector3d::Random() * _icp_params.perturbation_new_attempts;
+ csm_input.first_guess[0] = initial_guess_perturbed(0);
+ csm_input.first_guess[1] = initial_guess_perturbed(1);
+ csm_input.first_guess[2] = initial_guess_perturbed(2);
+ }
+
+ // call icp
+ result.attempts++;
+ if (_icp_params.verbose)
+ {
+ std::cout << "Calling ICP, attempt: " << result.attempts << std::endl;
+ }
+ try
+ {
+ sm_icp(&csm_input, &csm_output);
+ result.valid = csm_output.valid == 1;
+ }
+ catch (...)
+ {
+ result.valid = false;
+ }
- if (csm_input.do_compute_covariance)
+ // VALID --> copy output (and modify covariance)
+ if (result.valid == 1)
{
- 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)
+ result.nvalid = csm_output.nvalid;
+ result.error = csm_output.error;
+ 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)
{
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(result.res_covar.topLeftCorner<2,2>());
+ 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];
- if (eigensolver.info() == Eigen::Success)
+ // Grow covariance in the biggest eigenvalue direction
+ if (_icp_params.cov_max_eigv_factor - 1 > 1e-6)
{
- 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();
+ 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();
+ }
}
}
}
- }
- else
- {
- //std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
- result.res_transf = _initial_guess;
- result.res_covar = Eigen::Matrix3s::Identity();
- }
+ else
+ {
+ // 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 and not result.valid and result.attempts < _icp_params.icp_attempts)
+ {
+ std::cout << "Invalid result, trying again!" << std::endl;
+ }
+ if (_icp_params.verbose and not result.valid and result.attempts < _icp_params.icp_attempts)
+ {
+ std::cout << "Error too big: " << result.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
+ result.attempts < _icp_params.icp_attempts);
return result;
}
-//Legacy code
-icpOutput ICP::align(const LaserScan &_current_ls,
- const LaserScan &_ref_ls,
- const LaserScanParams& scan_params,
- const icpParams &icp_params,
+// Legacy code
+icpOutput ICP::align(const LaserScan &_current_ls,
+ const LaserScan &_ref_ls,
+ const LaserScanParams &scan_params,
+ const icpParams &icp_params,
const Eigen::Vector3s &_initial_guess)
{
return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _initial_guess);
@@ -223,11 +248,10 @@ void ICP::printLaserData(LDP &laser_data)
void ICP::printTwoLaserData(sm_params ¶ms)
{
- for (int ii=0; ii<params.laser_ref->nrays-1; ++ii)
+ for (int ii = 0; ii < params.laser_ref->nrays - 1; ++ii)
{
std::cout << "Theta: " << params.laser_ref->theta[ii] << "; Readings: "
- << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
- << '\n';
+ << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
+ << '\n';
}
-
}
diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index a633fdc3575f5442e06757a2ec350461ac4748ef..991ffe0dafc10e53337296c09f97a61f9e66a895 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -26,7 +26,7 @@
using namespace laserscanutils;
int N = 50;
-int n_attempts = 5;
+int n_attempts = 10;
const Eigen::Vector2d A = Eigen::Vector2d::Zero();
const Eigen::Vector2d B = (Eigen::Vector2d() << 0, 40).finished();
@@ -268,6 +268,8 @@ TEST(TestIcp, TestIcpSame1)
// icp
auto icp_params = icp_params_default;
+ icp_params.icp_attempts = n_attempts;
+ // icp_params.verbose = true;
// no perturbation
std::cout << "\n//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
@@ -288,31 +290,17 @@ TEST(TestIcp, TestIcpSame1)
// perturbation
std::cout << "------ WITH perturbation: " << pert << std::endl;
Eigen::Vector3d initial_guess = Eigen::Vector3d::Random() * pert;
- int n = 1;
icp_output = ICP::align(scan,
scan,
scan_params,
icp_params,
initial_guess);
- while (not icp_output.valid or icp_output.error > 1e-1)
- {
- n++;
- initial_guess = Eigen::Vector3d::Random() * pert;
- icp_output = ICP::align(scan,
- scan,
- scan_params,
- icp_params,
- initial_guess);
- if (n == n_attempts)
- break;
- }
-
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: " << n << std::endl;
+ std::cout << " icp attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -326,46 +314,14 @@ TEST(TestIcp, TestIcpSame2)
// 0,360 (1 degrees step)
LaserScanParams scan_params = generateLaserScanParams(0, 1);
double pert = 0.0;
+ auto icp_params = icp_params_default;
+ icp_params.icp_attempts = n_attempts;
for (auto i = 0; i < N; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
- // icp
- auto icp_params = icp_params_default;
- icp_params.verbose = false; // false, // bool verbose (prints debug messages)
- icp_params.use_point_to_line_distance = false; // true, // bool use_point_to_line_distance
- // 5.0, // double max_angular_correction_deg
- // 1, // double max_linear_correction
- // 0.5, // double max_correspondence_dist
- icp_params.use_corr_tricks = false; // false, // bool use_corr_tricks
- icp_params.debug_verify_tricks = false; // false, // bool debug_verify_tricks
- // 50, // int max_iterations
- // 1e-4, // double epsilon_xy
- // 1e-3, // double epsilon_theta
- icp_params.restart = true; // false, // bool restart
- icp_params.restart_threshold_mean_error = 0.1; // 0, // double restart_threshold_mean_error
- // 0, // double restart_dt
- // 0, // double restart_dtheta
- // 0.023, // double min_reading
- // 60, // max_reading
- // 1, // double outliers_maxPerc
- // 0.8, // double outliers_adaptive_order
- // 2, // double outliers_adaptive_mult
- // false, // bool outliers_remove_doubles
- // false, // bool do_visibility_test
- // false, // bool do_alpha_test
- // 10, // double do_alpha_test_thresholdDeg
- // 0.5, // double clustering_threshold
- // 4, // int orientation_neighbourhood
- // false, // bool use_ml_weights
- // false, // bool use_sigma_weights
- // 0.2, // double sigma
- // true, // bool do_compute_covariance
- // 5, // double cov_factor
- // 2 // double cov_max_eigv_factor
-
// no perturbation
std::cout << "\n//////////// TestIcpSame2: random problem " << i << " pose: " << pose.transpose() << std::endl;
std::cout << "------ NO perturbation" << std::endl;
@@ -385,31 +341,17 @@ TEST(TestIcp, TestIcpSame2)
// perturbation
std::cout << "------ WITH perturbation: " << pert << std::endl;
Eigen::Vector3d initial_guess = Eigen::Vector3d::Random() * pert;
- int n = 1;
icp_output = ICP::align(scan,
scan,
scan_params,
icp_params,
initial_guess);
- while (not icp_output.valid or icp_output.error > 1e-1)
- {
- n++;
- initial_guess = Eigen::Vector3d::Random() * pert;
- icp_output = ICP::align(scan,
- scan,
- scan_params,
- icp_params,
- initial_guess);
- if (n == n_attempts)
- break;
- }
-
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: " << n << std::endl;
+ std::cout << " icp attempts: " << icp_output.attempts << std::endl;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -426,35 +368,25 @@ TEST(TestIcp, TestIcp1)
Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
+ 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
generateRandomProblem(pose_ref, pose_tar, pose_d, scan_params, scan_ref, scan_tar);
- // icp
- auto icp_params = icp_params_default;
- int n = 1;
+ initial_guess = pose_d + pert * Eigen::Vector3d::Random();
+ // icp
auto icp_output = ICP::align(scan_tar,
scan_ref,
scan_params,
icp_params,
initial_guess);
- while (not icp_output.valid or icp_output.error > 1e-1)
- {
- n++;
- initial_guess = pose_d + pert * Eigen::Vector3d::Random();
- icp_output = ICP::align(scan_tar,
- scan_ref,
- scan_params,
- icp_params,
- initial_guess);
- if (n == n_attempts)
- break;
- }
-
std::cout << "\n//////////// TestIcp1: random problem " << i << " - perturbation: " << pert << std::endl;
std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
std::cout << " pose_tar: " << pose_tar.transpose() << std::endl;
@@ -463,7 +395,7 @@ TEST(TestIcp, TestIcp1)
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: " << n << std::endl;
+ std::cout << " icp attempts: " << icp_output.attempts << std::endl;
std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
ASSERT_TRUE(icp_output.valid);
@@ -481,7 +413,11 @@ TEST(TestIcp, TestIcp10)
Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
+ 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
@@ -490,26 +426,11 @@ TEST(TestIcp, TestIcp10)
initial_guess = pose_d + pert * Eigen::Vector3d::Random();
// icp
- auto icp_params = icp_params_default;
- int n = 1;
-
auto icp_output = ICP::align(scan_tar,
scan_ref,
scan_params,
icp_params,
initial_guess);
- while (not icp_output.valid or icp_output.error > 1e-1)
- {
- n++;
- initial_guess = pose_d + pert * Eigen::Vector3d::Random();
- icp_output = ICP::align(scan_tar,
- scan_ref,
- scan_params,
- icp_params,
- initial_guess);
- if (n == n_attempts)
- break;
- }
std::cout << "\n//////////// TestIcp10: random problem " << i << " - perturbation: " << pert << std::endl;
std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
@@ -519,7 +440,7 @@ TEST(TestIcp, TestIcp10)
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: " << n << std::endl;
+ std::cout << " icp attempts: " << icp_output.attempts << std::endl;
std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
ASSERT_TRUE(icp_output.valid);