sm: matchICP() is working after some tuning

See comments in the code and in the csm library

src/icp.cpp

@@ -18,7 +18,7 @@ public:
                 laser_data->readings[i] = it;
                 laser_data->valid[i] = 1;
             }else{
-                laser_data->readings[i] = -1;
+                laser_data->readings[i] = NAN;
                 laser_data->valid[i] = 0;
             }
             laser_data->cluster[i] = -1;
@@ -53,7 +53,8 @@ ICP::~ICP()
 
 icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf)
 {
-    sm_debug_write(true);
+    // Uncomment to enable debug messages from the CSM library
+    // sm_debug_write(true);
 
     LDWrapper last   = LDWrapper(_last_ls, params);
     LDWrapper origin = LDWrapper(_origin_ls, params);
@@ -73,15 +74,43 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
     csm_input.first_guess[1] = _last_transf(1);
     csm_input.first_guess[2] = _last_transf(2);
 
-    csm_input.use_point_to_line_distance = true;
+    csm_input.use_point_to_line_distance = 1;
+
+    csm_input.max_correspondence_dist = 1;
+    csm_input.max_iterations = 20;
+
+    csm_input.use_corr_tricks = 1;
+    csm_input.outliers_maxPerc = 0.9;
+    csm_input.outliers_adaptive_order = 0.7;
+    csm_input.outliers_adaptive_mult = 1.5;
 
     sm_icp(&csm_input, &csm_output);
-    // std::cout << "Result: " << csm_output.valid << '\n';
-    // std::cout << "My solution " << csm_output.x[0] << "," << csm_output.x[1] << "," << csm_output.x[2] << std::endl;
+
     icp_output result{};
     result.res_transf(0) = csm_output.x[0];
     result.res_transf(1) = csm_output.x[1];
     result.res_transf(2) = csm_output.x[2];
 
+    std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
+    std::cout << "Number of iterations: " << csm_output.iterations << '\n';
+    std::cout << "Error: " << csm_output.error << '\n';
+
     return result;
 }
+
+void ICP::printLaserData(LDP &laser_data)
+{
+    std::cout << "Laser Reading: " << laser_data->readings[0] << '\n';
+}
+
+void ICP::printTwoLaserData(sm_params &params)
+{
+
+    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';
+    }
+
+}

src/icp.h

@@ -19,6 +19,9 @@ class ICP
         ~ICP();
 
     static icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf);
+
+    static void printTwoLaserData(sm_params &params);
+    static void printLaserData(LDP &laser_data);
 };
 
 }