adding gtest_icp

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)

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})

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)

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);

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

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)

test/gtest_icp.cpp

+//--------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();
+}