From 7b35cbe4809456bad9f2adba7321dc384b53d105 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 30 Oct 2019 12:59:14 +0100
Subject: [PATCH] Added align functionality for two arbitrary laser scans from
different sensors
---
src/icp.cpp | 70 +++++++++++++++++++++++++++++++++++++++++++++++++++++
src/icp.h | 23 ++++++++++++------
2 files changed, 85 insertions(+), 8 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index c137f9b..2abc4fc 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);
@@ -56,6 +58,74 @@ 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) {
+ // 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 = 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_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;
+}
+
+//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)
{
// Uncomment to enable debug messages from the CSM library
diff --git a/src/icp.h b/src/icp.h
index d6345fc..740e28e 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{
@@ -33,12 +33,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 ¶ms);
- 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
--
GitLab