diff --git a/src/icp.cpp b/src/icp.cpp
index f03a0f0f62a17344f02051a53bb47916f8f9b9f9..4db9504012ebd82b71b7c3fdbbdf6f643894ec37 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -128,70 +128,7 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
 //Legacy code
 icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_transf_guess)
 {
-    // Uncomment to enable debug messages from the CSM library
-    // sm_debug_write(true);
-
-    LDWrapper current = LDWrapper(_current_ls, scan_params);
-    LDWrapper ref = LDWrapper(_ref_ls, scan_params);
-
-    int num_rays = _current_ls.ranges_raw_.size();
-
-    sm_params csm_input{};
-    sm_result csm_output{};
-
-    csm_input.min_reading   = scan_params.range_min_;
-    csm_input.max_reading   = scan_params.range_max_;
-    csm_input.sigma         = scan_params.range_std_dev_;
-
-    csm_input.laser_ref     = ref.laser_data;
-    csm_input.laser_sens    = current.laser_data;
-
-    csm_input.first_guess[0] = _transf_guess(0);
-    csm_input.first_guess[1] = _transf_guess(1);
-    csm_input.first_guess[2] = _transf_guess(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_guess;
-        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;
+    return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _transf_guess);
 }
 
 void ICP::printLaserData(LDP &laser_data)