Minor fixes

.gitignore

@@ -5,3 +5,4 @@ lib/
 .settings/language.settings.xml
 .project
 .cproject
+.clangd

cmake_modules/Findcsm.cmake

@@ -46,7 +46,7 @@ macro(csm_report_not_found REASON_MSG)
   # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
   # FindPackage() use the camelcase library name, not uppercase.
   if (csm_FIND_QUIETLY)
-    message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN})
+    message(STATUS "Ignoring csm dependency- " ${REASON_MSG} ${ARGN})
   else (csm_FIND_REQUIRED)
     message(FATAL_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
   else()

src/icp.cpp

@@ -126,15 +126,15 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
 }
 
 //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)
+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 last   = LDWrapper(_last_ls, scan_params);
-    LDWrapper origin = LDWrapper(_origin_ls, scan_params);
+    LDWrapper current = LDWrapper(_current_ls, scan_params);
+    LDWrapper ref = LDWrapper(_ref_ls, scan_params);
 
-    int num_rays = _last_ls.ranges_raw_.size();
+    int num_rays = _current_ls.ranges_raw_.size();
 
     sm_params csm_input{};
     sm_result csm_output{};
@@ -143,12 +143,12 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con
     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.laser_ref     = ref.laser_data;
+    csm_input.laser_sens    = current.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.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;
@@ -183,7 +183,7 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con
     else
     {
         std::cout << "ICP valid != 1, providing first guess transformation and identity covariance\n";
-        result.res_transf = _last_transf;
+        result.res_transf = _transf_guess;
         result.res_covar = Eigen::Matrix3s::Identity();
     }
 

src/icp.h

@@ -19,7 +19,7 @@ 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;

src/laser_scan.cpp

@@ -2,7 +2,17 @@
 
 namespace laserscanutils
 {
-    
+void LaserScanParams::print() const{
+    std::cout << "PRINTING LASERSCANPARAMS ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
+    std::cout << "angle_min_: " << angle_min_ << std::endl;
+    std::cout << "angle_max_: " << angle_max_ << std::endl;
+    std::cout << "angle_step_: " << angle_step_ << std::endl;
+    std::cout << "scan_time_: " << scan_time_ << std::endl;
+    std::cout << "range_min_: " << range_min_ << std::endl;
+    std::cout << "range_max_: " << range_max_ << std::endl;
+    std::cout << "range_std_dev_: " << range_std_dev_ << std::endl;
+    std::cout << "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;
 }
@@ -95,20 +105,20 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
         {
             //set as 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;
-            
+
             //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_ as a 2D homogeneous
             points_.col(ii_ok) << point_ref(0), point_ref(1), 1;
-            
-            //check jump. 
+
+            //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
             {
@@ -117,10 +127,10 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
             }
             else
                 jumps_mask_[ii] = false;
-            
+
             //increment ok counter
             ii_ok++;
-            
+
             //keep current range as previous for the next iteration
             prev_range = ranges_raw_[ii];
         }
@@ -131,17 +141,17 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
             points_all_.col(ii) << 0, 0, 0;
             //prev_range = 0;
         }
-       
+
        //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;
 }
@@ -323,21 +333,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);
@@ -361,4 +371,3 @@ void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<lase
 }
 
 }//namespace
-