From d852c92f62d59d6d8c1cddb3a3205d76a0449c5e Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 26 Jun 2024 13:07:06 +0200
Subject: [PATCH] improved icp test
---
test/gtest_icp.cpp | 187 ++++++++++++++++++++++++++++++++++++---------
1 file changed, 149 insertions(+), 38 deletions(-)
diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index b79c7a0..a633fdc 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -25,6 +25,9 @@
using namespace laserscanutils;
+int N = 50;
+int n_attempts = 5;
+
const Eigen::Vector2d A = Eigen::Vector2d::Zero();
const Eigen::Vector2d B = (Eigen::Vector2d() << 0, 40).finished();
const Eigen::Vector2d C = (Eigen::Vector2d() << 30, 0).finished();
@@ -243,7 +246,7 @@ TEST(TestIcp, TestGenerateRandomPose)
LaserScanParams scan_params = generateLaserScanParams(-180, 1);
// 100 random poses and scans
- for (auto i = 0; i < 100; i++)
+ for (auto i = 0; i < N; i++)
{
auto laser_pose = generateRandomPoseInsideTriangle();
ASSERT_TRUE(insideTriangle(laser_pose));
@@ -256,8 +259,9 @@ TEST(TestIcp, TestIcpSame1)
{
// -180,180 (1 degrees step)
LaserScanParams scan_params = generateLaserScanParams(-180, 1);
+ double pert = 0.0;
- for (auto i = 0; i < 10; i++)
+ for (auto i = 0; i < N; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
@@ -266,34 +270,54 @@ TEST(TestIcp, TestIcpSame1)
auto icp_params = icp_params_default;
// no perturbation
- std::cout << "//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
- std::cout << "//////////// NO perturbation" << std::endl;
+ std::cout << "\n//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
+ std::cout << "------ NO perturbation" << std::endl;
auto icp_output = ICP::align(scan,
scan,
scan_params,
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;
+
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
// perturbation
- std::cout << "//////////// WITH perturbation" << std::endl;
+ 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,
- Eigen::Vector3d::Random() * 0.1);
+ initial_guess);
- if (not icp_output.valid)
+ 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,
- Eigen::Vector3d::Random() * 0.1);
+ 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;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
+
+ pert += 0.5 / N;
}
}
@@ -301,44 +325,96 @@ TEST(TestIcp, TestIcpSame2)
{
// 0,360 (1 degrees step)
LaserScanParams scan_params = generateLaserScanParams(0, 1);
+ double pert = 0.0;
- for (auto i = 0; i < 10; i++)
+ 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 << "//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
- std::cout << "//////////// NO perturbation" << std::endl;
+ std::cout << "\n//////////// TestIcpSame2: random problem " << i << " pose: " << pose.transpose() << std::endl;
+ std::cout << "------ NO perturbation" << std::endl;
auto icp_output = ICP::align(scan,
scan,
scan_params,
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;
+
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
// perturbation
- std::cout << "//////////// WITH perturbation" << std::endl;
+ 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,
- Eigen::Vector3d::Random() * 0.1);
+ initial_guess);
- if (not icp_output.valid)
+ 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,
- Eigen::Vector3d::Random() * 0.1);
+ 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;
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
+
+ pert += 0.5 / N;
}
}
@@ -350,15 +426,15 @@ TEST(TestIcp, TestIcp1)
Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
- for (auto i = 0; i < 10; i++)
+ 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);
- initial_guess = pose_d + 0.2 * Eigen::Vector3d::Random();
-
// icp
auto icp_params = icp_params_default;
+ int n = 1;
auto icp_output = ICP::align(scan_tar,
scan_ref,
@@ -366,17 +442,34 @@ TEST(TestIcp, TestIcp1)
icp_params,
initial_guess);
- std::cout << "\n//////////// TestIcp1: random problem " << i << std::endl;
- std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
- 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 << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
-
+ 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;
+ 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: " << n << std::endl;
+ std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
+
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
+
+ pert += 0.5 / N;
}
}
@@ -388,33 +481,51 @@ TEST(TestIcp, TestIcp10)
Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
- for (auto i = 0; i < 10; i++)
+ 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, 10);
- initial_guess = pose_d + 0.2 * Eigen::Vector3d::Random();
+ 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;
+ 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: " << n << std::endl;
+ std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
- std::cout << "\n//////////// TestIcp1: random problem " << i << std::endl;
- std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
- 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 << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
-
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
+
+ pert += 0.5 / N;
}
} //*/
--
GitLab