improved icp test

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;
   }
 } //*/