small changes

README.md

@@ -6,10 +6,10 @@ A C++ Toolbox with utilities for 2D laser scan processing, made at IRI (www.iri.
 
 **Canonical Scan Matching - CSM**
 
-To enable the compilation of iterative closest point algorithms, [CSM](https://github.com/AndreaCensi/csm) should be installed. Clone, compile and install:
+To enable the compilation of iterative closest point algorithms, [CSM](https://github.com/AndreaCensi/csm) should be installed. We recommend to install it from our [forked repository](https://gitlab.iri.upc.edu/labrobotica/algorithms/csm.git) to avoid several error messages. Clone, compile and install:
 ```sh
 cd <your/path>
-git clone https://github.com/AndreaCensi/csm.git
+git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/csm.git
 cd csm
 cmake .
 make

src/icp.cpp

@@ -21,48 +21,61 @@
 //--------LICENSE_END--------
 #include "icp.h"
 #include <algorithm>
+#include <unistd.h>
+#include <fcntl.h>
 
 using namespace laserscanutils;
 unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
 std::mt19937 generator (seed);
 std::uniform_real_distribution<double> dis(0.0, 1.0);
 
-class LDWrapper {
-public:
-    LDP laser_data;
-    LDWrapper(const LaserScan& scan, const LaserScanParams& scan_params)
-    {
-        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_;
-
-            if(scan_params.range_min_ <= it and it <= scan_params.range_max_ and 0 <= it and it <= 100){
-                laser_data->readings[i] = it;//*100/scan_params.range_max_;
-                laser_data->valid[i] = 1;
-            }else{
-                laser_data->readings[i] = NAN;
-                laser_data->valid[i] = 0;
+class LDWrapper
+{
+    public:
+        LDP laser_data;
+
+        LDWrapper(const LaserScan& scan, const LaserScanParams& scan_params)
+        {
+            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_;
+
+                if (scan_params.range_min_ <= it and
+                    it <= scan_params.range_max_ and
+                    0 <= it and it <= 100)
+                {
+                    laser_data->readings[i] = it;
+                    laser_data->valid[i] = 1;
+                }
+                else
+                {
+                    laser_data->readings[i] = NAN;
+                    laser_data->valid[i] = 0;
+                }
+                laser_data->cluster[i] = -1;
+                ++i;
             }
-            laser_data->cluster[i] = -1;
-            ++i;
-        }
 
-        laser_data->min_theta = laser_data->theta[0];
-        laser_data->max_theta = laser_data->theta[num_rays-1];
+            laser_data->min_theta = laser_data->theta[0];
+            laser_data->max_theta = laser_data->theta[num_rays-1];
 
-        laser_data->odometry[0] = 0.0;
-        laser_data->odometry[1] = 0.0;
-        laser_data->odometry[2] = 0.0;
+            laser_data->odometry[0] = 0.0;
+            laser_data->odometry[1] = 0.0;
+            laser_data->odometry[2] = 0.0;
 
-        laser_data->true_pose[0] = 0.0;
-        laser_data->true_pose[1] = 0.0;
-        laser_data->true_pose[2] = 0.0;
-    }
-    ~LDWrapper(){
-        ld_free(laser_data);
-    }
+            laser_data->true_pose[0] = 0.0;
+            laser_data->true_pose[1] = 0.0;
+            laser_data->true_pose[2] = 0.0;
+
+        }
+
+        ~LDWrapper()
+        {
+            ld_free(laser_data);
+        }
 };
 
 ICP::ICP()
@@ -75,10 +88,15 @@ 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) {
+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);
+  //sm_debug_write(true);
 
   LDWrapper laser_scan_current = LDWrapper(_current_ls, _current_scan_params);
   LDWrapper laser_scan_ref = LDWrapper(_ref_ls, _ref_scan_params);
@@ -128,29 +146,6 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
   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;
@@ -193,15 +188,11 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
   }
   else
   {
-    std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
+    //std::cout << "ICP NOT VALID, 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;
 }