diff --git a/.gitignore b/.gitignore
index c99a3e22087bd637b9d4de70dec55977cd422a95..07dd1e8673fdf510e0dbdb93de44891b7d519fa9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,3 +5,4 @@ lib/
 .settings/language.settings.xml
 .project
 .cproject
+.clangd
diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake
index 7ad6ce1974a96153a652df1199f1ae7885ac8ed2..953f9c5113d9139cefaaa508043f5d623072cb8d 100644
--- a/cmake_modules/Findcsm.cmake
+++ b/cmake_modules/Findcsm.cmake
@@ -61,4 +61,4 @@ if(NOT csm_FOUND)
   csm_report_not_found("Something went wrong while setting up csm.")
 endif(NOT csm_FOUND)
 # Set the include directories for csm (itself).
-set(csm_FOUND TRUE)
\ No newline at end of file
+set(csm_FOUND TRUE)
diff --git a/src/icp.cpp b/src/icp.cpp
index c137f9bfdb6095e689b646496a64bfa2ac3e1fbf..aa43c13911d56725e6bd1c40d75c73104e62830e 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -1,4 +1,6 @@
 #include "icp.h"
+#include <algorithm>
+
 using namespace laserscanutils;
 unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
 std::mt19937 generator (seed);
@@ -11,7 +13,6 @@ public:
     {
         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_;
@@ -27,9 +28,6 @@ public:
             ++i;
         }
 
-        // for(int i = 0; i < num_rays; ++i){
-        //     laser_data->theta[i] = laser_data->theta[i] * 0.0175;
-        // }
         laser_data->min_theta = laser_data->theta[0];
         laser_data->max_theta = laser_data->theta[num_rays-1];
 
@@ -56,72 +54,139 @@ ICP::~ICP()
 
 }
 
-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
-    // sm_debug_write(true);
-
-    LDWrapper last   = LDWrapper(_last_ls, scan_params);
-    LDWrapper origin = LDWrapper(_origin_ls, scan_params);
-
-    int num_rays = _last_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     = origin.laser_data;
-    csm_input.laser_sens    = last.laser_data;
-
-    csm_input.first_guess[0] = _last_transf(0);
-    csm_input.first_guess[1] = _last_transf(1);
-    csm_input.first_guess[2] = _last_transf(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 = _last_transf;
-        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';
+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 = _icp_params.do_compute_covariance;
+
+  csm_input.max_angular_correction_deg = _icp_params.max_angular_correction_deg;
+  csm_input.max_linear_correction = _icp_params.max_linear_correction;
+  csm_input.epsilon_xy = _icp_params.epsilon_xy;
+  csm_input.epsilon_theta = _icp_params.epsilon_theta;
+  csm_input.sigma = _icp_params.sigma;
+  csm_input.restart = _icp_params.restart;
+  csm_input.restart_threshold_mean_error = _icp_params.restart_threshold_mean_error;
+  csm_input.restart_dt = _icp_params.restart_dt;
+  csm_input.restart_dtheta = _icp_params.restart_dtheta;
+  csm_input.clustering_threshold = _icp_params.clustering_threshold;
+  csm_input.orientation_neighbourhood = _icp_params.orientation_neighbourhood;
+  csm_input.do_alpha_test = _icp_params.do_alpha_test;
+  csm_input.do_alpha_test_thresholdDeg = _icp_params.do_alpha_test_thresholdDeg;
+  csm_input.do_visibility_test = _icp_params.do_visibility_test;
+  csm_input.outliers_remove_doubles = _icp_params.outliers_remove_doubles;
+  csm_input.debug_verify_tricks = _icp_params.debug_verify_tricks;
+  csm_input.gpm_theta_bin_size_deg = _icp_params.gpm_theta_bin_size_deg;
+  csm_input.gpm_extend_range_deg = _icp_params.gpm_extend_range_deg;
+  csm_input.gpm_interval = _icp_params.gpm_interval;
+
+  // std::cout<< "=========================================================================================================================================\n";
+  // std::cout << "csm max_angular_correction_deg " << csm_input.max_angular_correction_deg << " icp max_angular_correction_deg " << _icp_params.max_angular_correction_deg << "\n";
+  // std::cout << "csm max_linear_correction " << csm_input.max_linear_correction << " icp max_linear_correction " << _icp_params.max_linear_correction << "\n";
+  // std::cout << "csm epsilon_xy " << csm_input.epsilon_xy << " icp epsilon_xy " << _icp_params.epsilon_xy << "\n";
+  // std::cout << "csm epsilon_theta " << csm_input.epsilon_theta << " icp epsilon_theta " << _icp_params.epsilon_theta << "\n";
+  // std::cout << "csm sigma " << csm_input.sigma << " icp sigma " << _icp_params.sigma << "\n";
+  // std::cout << "csm restart " << csm_input.restart << " icp restart " << _icp_params.restart << "\n";
+  // std::cout << "csm restart_threshold_mean_error " << csm_input.restart_threshold_mean_error << " icp restart_threshold_mean_error " << _icp_params.restart_threshold_mean_error << "\n";
+  // std::cout << "csm restart_dt " << csm_input.restart_dt << " icp restart_dt " << _icp_params.restart_dt << "\n";
+  // std::cout << "csm restart_dtheta " << csm_input.restart_dtheta << " icp restart_dtheta " << _icp_params.restart_dtheta << "\n";
+  // std::cout << "csm clustering_threshold " << csm_input.clustering_threshold << " icp clustering_threshold " << _icp_params.clustering_threshold << "\n";
+  // std::cout << "csm orientation_neighbourhood " << csm_input.orientation_neighbourhood << " icp orientation_neighbourhood " << _icp_params.orientation_neighbourhood << "\n";
+  // std::cout << "csm do_alpha_test " << csm_input.do_alpha_test << " icp do_alpha_test " << _icp_params.do_alpha_test << "\n";
+  // std::cout << "csm do_alpha_test_thresholdDeg " << csm_input.do_alpha_test_thresholdDeg << " icp do_alpha_test_thresholdDeg " << _icp_params.do_alpha_test_thresholdDeg << "\n";
+  // std::cout << "csm do_visibility_test " << csm_input.do_visibility_test << " icp do_visibility_test " << _icp_params.do_visibility_test << "\n";
+  // std::cout << "csm outliers_remove_doubles " << csm_input.outliers_remove_doubles << " icp outliers_remove_doubles " << _icp_params.outliers_remove_doubles << "\n";
+  // std::cout << "csm debug_verify_tricks " << csm_input.debug_verify_tricks << " icp debug_verify_tricks " << _icp_params.debug_verify_tricks << "\n";
+  // std::cout << "csm gpm_theta_bin_size_deg " << csm_input.gpm_theta_bin_size_deg << " icp gpm_theta_bin_size_deg " << _icp_params.gpm_theta_bin_size_deg << "\n";
+  // std::cout << "csm gpm_extend_range_deg " << csm_input.gpm_extend_range_deg << " icp gpm_extend_range_deg " << _icp_params.gpm_extend_range_deg << "\n";
+  // std::cout << "csm gpm_interval " << csm_input.gpm_interval << " icp gpm_interval " << _icp_params.gpm_interval << "\n";
+  // std::cout << "00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000"
+  //              "000000000000000000000000000000000\n";
+
+  //Not implemented (yet) in CSM
+  // csm_input.laser_x = _icp_params.laser_x;
+  // csm_input.laser_y = _icp_params.laser_y;
+  // csm_input.laser_theta = _icp_params.laser_theta;
+
+  csm_input.min_reading = _icp_params.min_reading;
+  csm_input.max_reading = _icp_params.max_reading;
+  csm_input.use_ml_weights = _icp_params.use_ml_weights;
+  csm_input.use_sigma_weights = _icp_params.use_sigma_weights;
+
+  // Not implemented (yet) in CSM
+  // csm_input.hsm_linear_cell_size = _icp_params.hsm_linear_cell_size;
+  // csm_input.hsm_angular_cell_size_deg = _icp_params.hsm_angular_cell_size_deg;
+  // csm_input.hsm_num_angular_hypotheses = _icp_params.hsm_num_angular_hypotheses;
+  // csm_input.hsm_xc_directions_min_distance_deg = _icp_params.hsm_xc_directions_min_distance_deg;
+  // csm_input.hsm_xc_ndirections = _icp_params.hsm_xc_ndirections;
+  // csm_input.hsm_angular_hyp_min_distance_deg = _icp_params.hsm_angular_hyp_min_distance_deg;
+  // csm_input.hsm_linear_xc_max_npeaks = _icp_params.hsm_linear_xc_max_npeaks;
+  // csm_input.hsm_linear_xc_peaks_min_distance = _icp_params.hsm_linear_xc_peaks_min_distance;
+
+  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;
+}
 
-    return result;
+//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)
+{
+    return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _transf_guess);
 }
 
 void ICP::printLaserData(LDP &laser_data)
diff --git a/src/icp.h b/src/icp.h
index d6345fc992ea5657445b02f2fdd04b5d614eacbb..389a616024efadd9af042fe8be0dd2a22495f046 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -1,10 +1,10 @@
 #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{
@@ -19,12 +19,48 @@ struct icpOutput{
 
 struct icpParams{
     int use_point_to_line_distance;
-    int max_correspondence_dist;
+    double max_correspondence_dist;
     int max_iterations;
     int use_corr_tricks;
     double outliers_maxPerc;
     double outliers_adaptive_order;
     double outliers_adaptive_mult;
+
+    int do_compute_covariance;
+    double max_angular_correction_deg;
+    double max_linear_correction;
+    double epsilon_xy;
+    double epsilon_theta;
+    double sigma;
+    int restart;
+    double restart_threshold_mean_error;
+    double restart_dt;
+    double restart_dtheta;
+    double clustering_threshold;
+    int orientation_neighbourhood;
+    int do_alpha_test;
+    double do_alpha_test_thresholdDeg;
+    int do_visibility_test;
+    int outliers_remove_doubles;
+    int debug_verify_tricks;
+    double gpm_theta_bin_size_deg;
+    double gpm_extend_range_deg;
+    double gpm_interval;
+    double laser_x;
+    double laser_y;
+    double laser_theta;
+    double min_reading;
+    double max_reading;
+    int use_ml_weights;
+    int use_sigma_weights;
+    double hsm_linear_cell_size;
+    double hsm_angular_cell_size_deg;
+    double hsm_num_angular_hypotheses;
+    double hsm_xc_directions_min_distance_deg;
+    double hsm_xc_ndirections;
+    double hsm_angular_hyp_min_distance_deg;
+    double hsm_linear_xc_max_npeaks;
+    double hsm_linear_xc_peaks_min_distance;
 };
 
 class ICP
@@ -33,12 +69,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
diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 7c0a34e06ad1d27ae02a115cc8450c9f7b97e239..3747de309ea2c1b26b6f7fbe8894217fb2d26991 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -2,7 +2,17 @@
 
 namespace laserscanutils
 {
-    
+void LaserScanParams::print() const{
+    std::cout << "DBG PRINTING LASERSCANPARAMS ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
+    std::cout << "DBG angle_min_: " << angle_min_ << std::endl;
+    std::cout << "DBG angle_max_: " << angle_max_ << std::endl;
+    std::cout << "DBG angle_step_: " << angle_step_ << std::endl;
+    std::cout << "DBG scan_time_: " << scan_time_ << std::endl;
+    std::cout << "DBG range_min_: " << range_min_ << std::endl;
+    std::cout << "DBG range_max_: " << range_max_ << std::endl;
+    std::cout << "DBG range_std_dev_: " << range_std_dev_ << std::endl;
+    std::cout << "DBG angle_std_dev_: " << angle_std_dev_ << std::endl;
+}
 LaserScan::LaserScan() :
     is_raw_processed_(false)
 {
@@ -28,7 +38,7 @@ LaserScan::~LaserScan()
 
 bool LaserScan::isRawProcessed() const
 {
-    return is_raw_processed_; 
+    return is_raw_processed_;
 }
 
 bool LaserScan::checkScanCorrectness(unsigned int _idx, unsigned int _idx_range) const
@@ -40,12 +50,12 @@ bool LaserScan::checkScanCorrectness(unsigned int _idx, unsigned int _idx_range)
     //set loop bounds
     unsigned int ii_init = std::max( (unsigned int)0 , (_idx-_idx_range) );
     unsigned int ii_end = std::min( (unsigned int)(ranges_.size()-1) , (_idx+_idx_range) );
-    
+
     //proceed
     for (unsigned int ii=ii_init; ii<=ii_end; ii++ )
         if (ranges_[ii] < 0)
             return false;
-    
+
     //return
     return true;
 }
@@ -55,16 +65,16 @@ bool LaserScan::checkScanJumps(unsigned int _idx, unsigned int _idx_range) const
     //first of all check if scan has been raw processed
     if ( ! is_raw_processed_ )
         return true;
-    
+
     //set loop bounds
     unsigned int ii_init = std::max( (unsigned int)0 , (_idx-_idx_range) );
     unsigned int ii_end = std::min( (unsigned int)(ranges_.size()-1) , (_idx+_idx_range) );
-    
+
     //proceed
     for (unsigned int ii=ii_init; ii<=ii_end; ii++ )
         if ( jumps_mask_[ii] )
             return true;
-    
+
     //return
     return false;
 }
@@ -94,65 +104,55 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
 		//invalid range
         if (!isValidRange(ranges_raw_[ii], _scan_params))
         {
-        	// fix if it is a single value between two valid values
-			if (ii > 0 && ii < ranges_raw_.size() && isValidRange(ranges_raw_[ii-1], _scan_params) && isValidRange(ranges_raw_[ii+1], _scan_params))
-			{
-				ranges_[ii] = (ranges_raw_[ii-1] + ranges_raw_[ii+1])/2.0;
-			}
-			// definitely invalid range
-			else
-			{
-				ranges_[ii] = -1.;
-				jumps_mask_[ii] = true;
-				points_all_.col(ii) << 0, 0, 0;
-				//prev_range = 0;
+            //set as valid range
+            ranges_[ii] = ranges_raw_[ii];
 
-			    //increment azimuth with angle step
-				azimuth += _scan_params.angle_step_;
-				continue;
-			}
-        }
-        // valid range
-		ranges_[ii] = ranges_raw_[ii];
+            //transform the laser hit from polar to 3D euclidean homogeneous
+            point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1;
 
-		//transform the laser hit from polar to 3D euclidean homogeneous
-		point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1;
+            //apply device mounting point calibration (p_r = T * p_l)
+            point_ref = _device_T * point_laser;
 
-		//apply device mounting point calibration (p_r = T * p_l)
-		point_ref = _device_T * point_laser;
+            //set to points_all_ as a 2D homogeneous
+            points_all_.col(ii) << point_ref(0), point_ref(1), 1;
 
-		//set to points_all_ as a 2D homogeneous
-		points_all_.col(ii) << point_ref(0), point_ref(1), 1;
+            //set to points_ as a 2D homogeneous
+            points_.col(ii_ok) << point_ref(0), point_ref(1), 1;
 
-		//set to points_ as a 2D homogeneous
-		points_.col(ii_ok) << point_ref(0), point_ref(1), 1;
-
-		//check jump.
-		//Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times
-		if (fabs(ranges_[ii] - prev_range) > _jump_th) //jump condition
-		{
-			jumps_indexes_.push_back(ii_ok); //indexes over points_
-			jumps_mask_[ii] = true; //masks over ranges_
-		}
-		else
-			jumps_mask_[ii] = false;
+            //check jump.
+            //Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times
+            if (fabs(ranges_[ii] - prev_range) > _jump_th) //jump condition
+            {
+                jumps_indexes_.push_back(ii_ok); //indexes over points_
+                jumps_mask_[ii] = true; //masks over ranges_
+            }
+            else
+                jumps_mask_[ii] = false;
 
-		//increment ok counter
-		ii_ok++;
+            //increment ok counter
+            ii_ok++;
 
-		//keep current range as previous for the next iteration
-		prev_range = ranges_raw_[ii];
+            //keep current range as previous for the next iteration
+            prev_range = ranges_raw_[ii];
+        }
+        else //invalid raw value
+        {
+            ranges_[ii] = -1.;
+            jumps_mask_[ii] = true;
+            points_all_.col(ii) << 0, 0, 0;
+            //prev_range = 0;
+        }
 
-        //increment azimuth with angle step
+       //increment azimuth with angle step
         azimuth += _scan_params.angle_step_;
     }
-    
+
     //push back the last index to jumps_indexes_, to properly close the list. This will be used by findSegments()
     jumps_indexes_.push_back(ii_ok);
-    
+
     //resize the output matrix to the number of correct points, while keeping values
     points_.conservativeResize(3, ii_ok);
-    
+
     //raise the flag
     is_raw_processed_ = true;
 }
@@ -345,21 +345,21 @@ void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<lase
     //in case ths input scan is not yet raw processed, do it
     if (!is_raw_processed_)
         ranges2xy(_scan_params, Eigen::Matrix4s::Identity(), _jump_th_segment);
-    
-    //run over all jumps (except the last, which indicates the closing index) 
+
+    //run over all jumps (except the last, which indicates the closing index)
     for (auto jumps_it = jumps_indexes_.begin(); jumps_it != std::prev(jumps_indexes_.end()); jumps_it++)
     {
         //new segment in the list
         _segment_list.push_back(ScanSegment());
 
-        //check how many points 
+        //check how many points
         num_points = (*std::next(jumps_it)) - (*jumps_it);
 
         //fill points
         _segment_list.back().points_.resize(3, num_points);
         for (unsigned int ii = 0; ii < num_points; ii++)
             _segment_list.back().points_.col(ii) << this->points_.col((*jumps_it) + ii);
-        
+
         //set segment attributes
         _segment_list.back().idx_first_ = (*jumps_it);
         _segment_list.back().idx_last_ = (*jumps_it + num_points -1);
@@ -383,4 +383,3 @@ void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<lase
 }
 
 }//namespace
-