diff --git a/README.md b/README.md
index f541e6b5b14750aaa8523c13898f0a217ec2c62c..517c3c023a933a11e2c0025f3c92a0987352a98f 100644
--- a/README.md
+++ b/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
diff --git a/src/icp.cpp b/src/icp.cpp
index f74e5d2ce40a0e21945622483522399f13a16f2b..8a18328e53bbe2dddc2f9821f37cc36851da1c49 100644
--- a/src/icp.cpp
+++ b/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;
}