added attempts to icp

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

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 &params)
 {
 
-    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';
     }
-
 }

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);