diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0c2c15567a7a6f0b6e26c7fb4b7bea9b2b0a4f92..5ed94b631561753a3a8eea35a562bf73c5728e82 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,6 +26,11 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
+#find dependencies
+FIND_PACKAGE(Eigen3 3.3 REQUIRED)
+FIND_PACKAGE(csm QUIET)
+FIND_PACKAGE(falkolib QUIET)
+
#Build tests
IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 234da736165e724c57150570eefdbdc9e12ed94d..3aa65ec369a4c8ec2031fd8d539624b038a4fb64 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -2,18 +2,6 @@
MESSAGE("Starting laser_scan_utils CMakeLists ...")
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-#find dependencies.
-FIND_PACKAGE(faramotics QUIET) #faramotics is only required for some tests
-IF(faramotics_FOUND)
- #FIND_PACKAGE(GLUT REQUIRED)
- FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Tests requiring it will be built.")
-ENDIF(faramotics_FOUND)
-
-FIND_PACKAGE(Eigen3 3.3 REQUIRED)
-FIND_PACKAGE(csm QUIET)
-FIND_PACKAGE(falkolib QUIET)
-
#include directories
INCLUDE_DIRECTORIES(. /usr/local/include)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
diff --git a/src/icp.cpp b/src/icp.cpp
index e2f9245b30b17e407fec41956aea79387f8ac3aa..f09a0cec08c225c61c20d7fe5620322d735b3c7f 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -44,8 +44,8 @@ class LDWrapper
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)
+ it <= scan_params.range_max_ and
+ 0 < it and it < 100)
{
laser_data->readings[i] = it;
laser_data->valid[i] = 1;
@@ -93,7 +93,7 @@ icpOutput ICP::align(const LaserScan &_current_ls,
const LaserScanParams &_current_scan_params,
const LaserScanParams &_ref_scan_params,
const icpParams &_icp_params,
- Eigen::Vector3s &_transf_ref_current)
+ const Eigen::Vector3s &_initial_guess)
{
// enable/disable debug messages from the CSM library
sm_debug_write(_icp_params.verbose);
@@ -113,9 +113,9 @@ icpOutput ICP::align(const LaserScan &_current_ls,
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.first_guess[0] = _initial_guess(0);
+ csm_input.first_guess[1] = _initial_guess(1);
+ csm_input.first_guess[2] = _initial_guess(2);
csm_input.use_point_to_line_distance = _icp_params.use_point_to_line_distance ? 1 : 0;
csm_input.max_correspondence_dist = _icp_params.max_correspondence_dist;
@@ -179,7 +179,7 @@ icpOutput ICP::align(const LaserScan &_current_ls,
else
{
//std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
- result.res_transf = _transf_ref_current;
+ result.res_transf = _initial_guess;
result.res_covar = Eigen::Matrix3s::Identity();
}
@@ -187,9 +187,13 @@ icpOutput ICP::align(const LaserScan &_current_ls,
}
//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)
+icpOutput ICP::align(const LaserScan &_current_ls,
+ const LaserScan &_ref_ls,
+ const LaserScanParams& scan_params,
+ const icpParams &icp_params,
+ const Eigen::Vector3s &_initial_guess)
{
- return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _transf_guess);
+ return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _initial_guess);
}
void ICP::printLaserData(LDP &laser_data)
diff --git a/src/icp.h b/src/icp.h
index 0b0863de4f4db771c9c11f6ba5616325f28f5220..76e0805aa0a72920a182087c556e2f53f8fbe5ad 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -48,11 +48,6 @@ struct icpParams
double max_angular_correction_deg; // Maximum angular displacement between scans (deg)
double max_linear_correction; // Maximum translation between scans (m)
- // Stopping criteria
- int max_iterations; // maximum iterations
- double epsilon_xy; // distance change
- double epsilon_theta; // angle change
-
/** Maximum distance for a correspondence to be valid */
double max_correspondence_dist;
/** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
@@ -60,6 +55,11 @@ struct icpParams
/** Checks that find_correspondences_tricks give the right answer */
bool debug_verify_tricks;
+ // Stopping criteria
+ int max_iterations; // maximum iterations
+ double epsilon_xy; // distance change
+ double epsilon_theta; // angle change
+
// Restart algorithm
bool restart; // Enable restarting
double restart_threshold_mean_error; // Threshold for restarting
@@ -119,6 +119,53 @@ struct icpParams
bool do_compute_covariance; // Compute the matching covariance (method in http://purl.org/censi/2006/icpcov)
double cov_factor; // Factor multiplying the cov output of csm
+ void print() const
+ {
+ std::cout << "verbose: " << std::to_string(verbose) << std::endl;
+ std::cout << "use_point_to_line_distance: " << std::to_string(use_point_to_line_distance) << std::endl;
+ std::cout << "max_angular_correction_deg: " << std::to_string(max_angular_correction_deg) << std::endl;
+ std::cout << "max_linear_correction: " << std::to_string(max_linear_correction) << std::endl;
+ std::cout << "max_correspondence_dist: " << std::to_string(max_correspondence_dist) << std::endl;
+ std::cout << "use_corr_tricks: " << std::to_string(use_corr_tricks) << std::endl;
+ std::cout << "debug_verify_tricks: " << std::to_string(debug_verify_tricks) << std::endl;
+ std::cout << "max_iterations: " << std::to_string(max_iterations) << std::endl;
+ std::cout << "epsilon_xy: " << std::to_string(epsilon_xy) << std::endl;
+ std::cout << "epsilon_theta: " << std::to_string(epsilon_theta) << std::endl;
+ std::cout << "restart: " << std::to_string(restart) << std::endl;
+ std::cout << "restart_threshold_mean_error: " << std::to_string(restart_threshold_mean_error) << std::endl;
+ std::cout << "restart_dt: " << std::to_string(restart_dt) << std::endl;
+ std::cout << "restart_dtheta: " << std::to_string(restart_dtheta) << std::endl;
+ std::cout << "min_reading: " << std::to_string(min_reading) << std::endl;
+ std::cout << "max_reading: " << std::to_string(max_reading) << std::endl;
+ std::cout << "outliers_maxPerc: " << std::to_string(outliers_maxPerc) << std::endl;
+ std::cout << "outliers_adaptive_order: " << std::to_string(outliers_adaptive_order) << std::endl;
+ std::cout << "outliers_adaptive_mult: " << std::to_string(outliers_adaptive_mult) << std::endl;
+ std::cout << "outliers_remove_doubles: " << std::to_string(outliers_remove_doubles) << std::endl;
+ std::cout << "do_visibility_test: " << std::to_string(do_visibility_test) << std::endl;
+ std::cout << "do_alpha_test: " << std::to_string(do_alpha_test) << std::endl;
+ std::cout << "do_alpha_test_thresholdDeg: " << std::to_string(do_alpha_test_thresholdDeg) << std::endl;
+ std::cout << "clustering_threshold: " << std::to_string(clustering_threshold) << std::endl;
+ std::cout << "orientation_neighbourhood: " << std::to_string(orientation_neighbourhood) << std::endl;
+ std::cout << "use_ml_weights: " << std::to_string(use_ml_weights) << std::endl;
+ std::cout << "use_sigma_weights: " << std::to_string(use_sigma_weights) << std::endl;
+ std::cout << "sigma: " << std::to_string(sigma) << std::endl;
+ std::cout << "do_compute_covariance: " << std::to_string(do_compute_covariance) << std::endl;
+ std::cout << "cov_factor: " << std::to_string(cov_factor) << std::endl;
+ }
+};
+
+const icpParams icp_params_default = {
+ false, //bool verbose; // prints debug messages
+ true, 180.0, 10, // bool use_point_to_line_distance; double max_angular_correction_deg; double max_linear_correction;
+ 0.5, true, false, // double max_correspondence_dist; bool use_corr_tricks; bool debug_verify_tricks;
+ 50, 1e-4, 1e-3, // int max_iterations; double epsilon_xy; double epsilon_theta;
+ false, 0, 0, 0, // bool restart; double restart_threshold_mean_error; double restart_dt; double restart_dtheta;
+ 0.023, 60, // double min_reading, max_reading;
+ 0.9, 0.7, 1.5, false, // double outliers_maxPerc; double outliers_adaptive_order; double outliers_adaptive_mult;
+ false, false, 10, // bool outliers_remove_doubles; bool do_visibility_test; bool do_alpha_test; double do_alpha_test_thresholdDeg;
+ 0.5, 4, // double clustering_threshold; int orientation_neighbourhood;
+ false, false, 0.2, // bool use_ml_weights; bool use_sigma_weights; double sigma;
+ true, 5 // bool do_compute_covariance; double cov_factor;
};
class ICP
@@ -132,10 +179,12 @@ class ICP
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);
+ const Eigen::Vector3s &_initial_guess);
+ static icpOutput align(const LaserScan &_last_ls,
+ const LaserScan &_reference_ls,
+ const LaserScanParams &scan_params,
+ const icpParams &icp_params,
+ const Eigen::Vector3s &_initial_guess);
static void printTwoLaserData(sm_params & params);
static void printLaserData(LDP & laser_data);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8b12bba936bfd244a256b60c5c7468d0815dc620..44067523264e565cd26bae7e3296ddb1e206fa2a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -13,17 +13,21 @@ INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
############# USE THIS TEST AS AN EXAMPLE ####################
# #
# Create a specific test executable for gtest_example #
-# gnss_utils_add_gtest(gtest_example gtest_example.cpp) #
+# laser_scan_utils_add_gtest(gtest_example gtest_example.cpp)#
# target_link_libraries(gtest_example ${PROJECT_NAME}) #
# #
##############################################################
# gtest example
-gnss_utils_add_gtest(gtest_example gtest_example.cpp)
-target_link_libraries(gtest_example ${PROJECT_NAME})
+laser_scan_utils_add_gtest(gtest_example gtest_example.cpp)
+# gtest icp
+IF(csm_FOUND)
+ laser_scan_utils_add_gtest(gtest_icp gtest_icp.cpp)
+ target_link_libraries(gtest_icp ${PROJECT_NAME})
+ENDIF(csm_FOUND)
IF(falkolib_FOUND)
- gnss_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/data)
+ laser_scan_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/data)
target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME})
ENDIF(falkolib_FOUND)
\ No newline at end of file
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 3c3c48c74516488a8c4eb24c2492bd81a5edc7f9..1bfcc9c1a097aa0b8d5d685c2e292e1680b544e5 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -55,7 +55,7 @@ set_target_properties(libgtest PROPERTIES
"IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
)
-function(gnss_utils_add_gtest target)
+function(laser_scan_utils_add_gtest target)
add_executable(${target} ${ARGN})
add_dependencies(${target} libgtest)
target_link_libraries(${target} libgtest)
diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53be96da36d22c6d9cbce3ab53f5525ad22d3c79
--- /dev/null
+++ b/test/gtest_icp.cpp
@@ -0,0 +1,212 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of laser_scan_utils
+// laser_scan_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gtest/utils_gtest.h"
+
+#include "laser_scan.h"
+#include "icp.h"
+
+using namespace laserscanutils;
+
+const double By = 20;
+
+// TODO: change to a triangle. Change Inf by C in (Cx, 0)?
+
+/* Synthetic scans are created from this simple scenario with three orthogonal walls:
+ *
+ * B: (0, By)
+ * +------------------------------------------------------------------->> (to infinite)
+ * |
+ * |
+ * | ( the laser can be in this area
+ * | with whichever orientation )
+ * |
+ * |
+ * +------------------------------------------------------------------->> (to infinite)
+ * A: (0, 0)
+ *
+*/
+LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams& params)
+{
+ assert(laser_pose(0) > 0);
+ assert(laser_pose(1) > 0 and laser_pose(1) < By);
+
+ // create scan
+ int n_beams = int((params.angle_max_-params.angle_min_)/params.angle_step_)+1;
+ LaserScan scan = LaserScan(std::vector<float>(n_beams, 0));
+
+ // compute angle limits for the three segments (in non rotated reference)
+ double theta_inf=0;
+ double theta_A = atan2(-laser_pose(1), -laser_pose(0));
+ while (theta_A < 0)
+ theta_A += 2*M_PI;
+ double theta_B = atan2(By-laser_pose(1), -laser_pose(0));
+ while (theta_B < 0)
+ theta_B += 2*M_PI;
+
+ double theta = params.angle_min_ + laser_pose(2);
+ for (auto i = 0; i < n_beams; i++, theta+=params.angle_step_)
+ {
+ // Ensure theta in [0, 2*M_PI)
+ while (theta < 0)
+ theta += 2*M_PI;
+ while (theta >= 2*M_PI)
+ theta -= 2*M_PI;
+
+ // B-Inf segment
+ if (theta <= theta_B)
+ //scan.ranges_raw_[i] = 0.0;
+ scan.ranges_raw_[i] = (By-laser_pose(1)) / cos(theta - M_PI/2);
+ // A-B segment
+ else if (theta > theta_B and theta < theta_A)
+ scan.ranges_raw_[i] = laser_pose(0) / cos(theta - M_PI);
+ // A-Inf segment
+ else if (theta >= theta_A)
+ scan.ranges_raw_[i] = laser_pose(1) / cos(theta - 3*M_PI/2);
+ else
+ throw std::runtime_error("bad theta angle..!");
+
+ // max range
+ if (scan.ranges_raw_[i] > params.range_max_ or scan.ranges_raw_[i] < params.range_min_)
+ scan.ranges_raw_[i] = 0.0;
+ }
+
+ return scan;
+};
+
+TEST(TestIcp, TestSimulateScan)
+{
+ // 4 beams: 0º, 90º, 180ª, 270ª
+ LaserScanParams scan_params;
+ scan_params.angle_min_ = 0;
+ scan_params.angle_step_ = M_PI / 2;
+ scan_params.angle_max_ = 3*M_PI/2;
+ scan_params.scan_time_ = 0;
+ scan_params.range_min_ = 0;
+ scan_params.range_max_ = 1e3;
+ scan_params.range_std_dev_ = 0;
+ scan_params.angle_std_dev_ = 0;
+
+ Eigen::Vector3d laser_pose;
+ laser_pose << 1, 6, 0;
+
+ // theta = 0
+ laser_pose(2) = 0;
+ std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
+ auto scan1 = simulateScan(laser_pose, scan_params);
+
+ ASSERT_NEAR(scan1.ranges_raw_[0], 0, 1e-9);
+ //ASSERT_NEAR(scan1.ranges_raw_[1], By-laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan1.ranges_raw_[2], laser_pose(0), 1e-9);
+ ASSERT_NEAR(scan1.ranges_raw_[3], laser_pose(1), 1e-9);
+
+ // theta = 90ª
+ laser_pose(2) = M_PI/2;
+ std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
+ auto scan2 = simulateScan(laser_pose, scan_params);
+
+ //ASSERT_NEAR(scan2.ranges_raw_[0], By-laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan2.ranges_raw_[1], laser_pose(0), 1e-9);
+ ASSERT_NEAR(scan2.ranges_raw_[2], laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan2.ranges_raw_[3], 0, 1e-9);
+
+ // theta = 180ª
+ laser_pose(2) = M_PI;
+ std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
+ auto scan3 = simulateScan(laser_pose, scan_params);
+
+ ASSERT_NEAR(scan3.ranges_raw_[0], laser_pose(0), 1e-9);
+ ASSERT_NEAR(scan3.ranges_raw_[1], laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan3.ranges_raw_[2], 0, 1e-9);
+ //ASSERT_NEAR(scan3.ranges_raw_[3], By-laser_pose(1), 1e-9);
+
+ // theta = 270ª
+ laser_pose(2) = 3*M_PI/2;
+ std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
+ auto scan4 = simulateScan(laser_pose, scan_params);
+
+ ASSERT_NEAR(scan4.ranges_raw_[0], laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan4.ranges_raw_[1], 0, 1e-9);
+ //ASSERT_NEAR(scan4.ranges_raw_[2], By-laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan4.ranges_raw_[3], laser_pose(0), 1e-9);
+
+
+ // 0-2M_PI (10 degrees step)
+ scan_params.angle_min_ = 0;
+ scan_params.angle_step_ = 10 * M_PI / 180;
+ scan_params.angle_max_ = 2*M_PI-scan_params.angle_step_;
+ scan_params.scan_time_ = 0;
+ scan_params.range_min_ = 0;
+ scan_params.range_max_ = 1e3;
+ scan_params.range_std_dev_ = 0;
+ scan_params.angle_std_dev_ = 0;
+
+ laser_pose << 26, 3, 0;
+
+ std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
+ auto scan = simulateScan(laser_pose, scan_params);
+}
+
+TEST(TestIcp, TestIcp1)
+{
+ // Scan params
+ // 0-2M_PI (5 degrees step)
+ LaserScanParams scan_params;
+ scan_params.angle_min_ = 0;
+ scan_params.angle_step_ = 5 * M_PI / 180;
+ scan_params.angle_max_ = 2*M_PI-scan_params.angle_step_;
+ scan_params.scan_time_ = 0;
+ scan_params.range_min_ = 0;
+ scan_params.range_max_ = 1e2;
+ scan_params.range_std_dev_ = 0;
+ scan_params.angle_std_dev_ = 0;
+
+ Eigen::Vector3d pose_ref, pose_tar;
+ pose_ref << 2.6, 2, 0;
+ pose_tar << 13.0, 12.5, 0;
+
+ std::cout << "Creating simulated scan_ref with laser pose: " << pose_ref.transpose() << "\n";
+ auto scan_ref = simulateScan(pose_ref, scan_params);
+ std::cout << "Creating simulated scan_tar with laser pose: " << pose_tar.transpose() << "\n";
+ auto scan_tar = simulateScan(pose_tar, scan_params);
+
+ // icp
+ auto icp_params = icp_params_default;
+ icp_params.max_linear_correction = 20;
+ icp_params.print();
+
+ icp_params.verbose = true;
+ auto icp_output = ICP::align(scan_tar,
+ scan_ref,
+ scan_params,
+ icp_params,
+ (Eigen::Vector3d() << 10.4, 10.5, 0).finished());
+
+ std::cout << "resulting transformation: " << icp_output.res_transf.transpose() << "\n";
+
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}