Added align functionality for two arbitrary laser scans from different sensors

src/icp.cpp

 #include "icp.h"
+#include <algorithm>
+
 using namespace laserscanutils;
 unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
 std::mt19937 generator (seed);
@@ -56,6 +58,74 @@ ICP::~ICP()
 
 }
 
+icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, const LaserScanParams &_current_scan_params, const LaserScanParams &_ref_scan_params,const icpParams &_icp_params,
+           Eigen::Vector3s &_transf_ref_current) {
+  // Uncomment to enable debug messages from the CSM library
+  // sm_debug_write(true);
+
+  LDWrapper laser_scan_current = LDWrapper(_current_ls, _current_scan_params);
+  LDWrapper laser_scan_ref = LDWrapper(_ref_ls, _ref_scan_params);
+
+  int num_rays = _current_ls.ranges_raw_.size();
+
+  sm_params csm_input{};
+  sm_result csm_output{};
+
+  csm_input.min_reading = max(_current_scan_params.range_min_, _ref_scan_params.range_min_);
+  csm_input.max_reading = min(_current_scan_params.range_max_, _ref_scan_params.range_max_);
+  csm_input.sigma = _ref_scan_params.range_std_dev_;
+
+  csm_input.laser_ref = laser_scan_ref.laser_data;
+  csm_input.laser_sens = laser_scan_current.laser_data;
+
+  csm_input.first_guess[0] = _transf_ref_current(0);
+  csm_input.first_guess[1] = _transf_ref_current(1);
+  csm_input.first_guess[2] = _transf_ref_current(2);
+
+  csm_input.use_point_to_line_distance = _icp_params.use_point_to_line_distance;
+  csm_input.max_correspondence_dist = _icp_params.max_correspondence_dist;
+  csm_input.max_iterations = _icp_params.max_iterations;
+  csm_input.use_corr_tricks = _icp_params.use_corr_tricks;
+  csm_input.outliers_maxPerc = _icp_params.outliers_maxPerc;
+  csm_input.outliers_adaptive_order = _icp_params.outliers_adaptive_order;
+  csm_input.outliers_adaptive_mult = _icp_params.outliers_adaptive_mult;
+
+  csm_input.do_compute_covariance = 1;
+
+  sm_icp(&csm_input, &csm_output);
+
+  icpOutput result{};
+  result.nvalid = csm_output.nvalid;
+  result.valid = csm_output.valid;
+  result.error = csm_output.error;
+
+  if (result.valid == 1) {
+    result.res_transf(0) = csm_output.x[0];
+    result.res_transf(1) = csm_output.x[1];
+    result.res_transf(2) = csm_output.x[2];
+
+    for (int i = 0; i < 3; ++i)
+      for (int j = 0; j < 3; ++j)
+        result.res_covar(i, j) =
+            // gsl_matrix_get(csm_output.cov_x_m, i, j);                 // NOT
+            // COMPILING
+            csm_output.cov_x_m
+                ->data[i * csm_output.cov_x_m->tda + j]; // This does the same
+  } else {
+    std::cout << "ICP valid != 1, providing first guess transformation and "
+                 "identity covariance\n";
+    result.res_transf = _transf_ref_current;
+    result.res_covar = Eigen::Matrix3s::Identity();
+  }
+
+  // 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;
+}
+
+//Legacy code
 icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_last_transf)
 {
     // Uncomment to enable debug messages from the CSM library

src/icp.h

 #ifndef ICP_H_
 #define ICP_H_
 
-#include <chrono>
-#include <random>
 #include "laser_scan.h"
+#include <chrono>
 #include <csm/csm_all.h>
+#include <random>
 // using namespace CSM;
 
 namespace laserscanutils{
@@ -33,12 +33,19 @@ class ICP
         ICP();
         ~ICP();
 
-    static icpOutput align(const LaserScan &_last_ls, const LaserScan &_reference_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_last_transf);
-
-    static void printTwoLaserData(sm_params &params);
-    static void printLaserData(LDP &laser_data);
-};
-
+            static icpOutput align(const LaserScan &_current_ls,
+                                   const LaserScan &_ref_ls,
+                                   const LaserScanParams &_current_scan_params,
+                                   const LaserScanParams &_ref_scan_params,
+                                   const icpParams &_icp_params,
+                                   Eigen::Vector3s &_transf_ref_current);
+            static icpOutput align(const LaserScan &_last_ls, const LaserScan &_reference_ls,
+                                   const LaserScanParams &scan_params, const icpParams &icp_params,
+                                   Eigen::Vector3s &_last_transf);
+
+          static void printTwoLaserData(sm_params & params);
+          static void printLaserData(LDP & laser_data);
+        };
 }
 
 #endif