Merge branch '23-icp-develop-matching-tools' into 'master'

Resolve "icp: develop matching tools"

Closes #23

See merge request mobile_robotics/laser_scan_utils!1

.gitignore

+#Ignore build, bin and lib folders
+bin/
+build/
+lib/
+.settings/language.settings.xml
+.project
+.cproject

CMakeLists.txt

@@ -14,6 +14,7 @@ PROJECT(laser_scan_utils)
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(CMAKE_INSTALL_PREFIX /usr/local)
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
 
 IF (NOT CMAKE_BUILD_TYPE)
   #SET(CMAKE_BUILD_TYPE "DEBUG")

cmake_modules/Findcsm.cmake

+FIND_PATH(
+    csm_INCLUDE_DIR
+    NAMES algos.h
+    PATHS /usr/local/include/csm)
+IF(csm_INCLUDE_DIR)
+  MESSAGE("Found csm include dirs: ${csm_INCLUDE_DIR}")
+ELSE(csm_INCLUDE_DIR)
+  MESSAGE("Couldn't find csm include dirs")
+ENDIF(csm_INCLUDE_DIR)
+
+FIND_LIBRARY(
+    csm_LIBRARY
+    NAMES libcsm.so libcsm.dylib
+    PATHS /usr/local/lib)
+IF(csm_LIBRARY)
+  MESSAGE("Found csm lib: ${csm_LIBRARY}")
+ELSE(csm_LIBRARY)
+  MESSAGE("Couldn't find csm lib")
+ENDIF(csm_LIBRARY)
+
+IF (csm_INCLUDE_DIR AND csm_LIBRARY)
+   SET(csm_FOUND TRUE)
+ ELSE(csm_INCLUDE_DIR AND csm_LIBRARY)
+   set(csm_FOUND FALSE)
+ENDIF (csm_INCLUDE_DIR AND csm_LIBRARY)
+
+IF (csm_FOUND)
+   IF (NOT csm_FIND_QUIETLY)
+      MESSAGE(STATUS "Found csm: ${csm_LIBRARY}")
+   ENDIF (NOT csm_FIND_QUIETLY)
+ELSE (csm_FOUND)
+   IF (csm_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find csm")
+   ENDIF (csm_FIND_REQUIRED)
+ENDIF (csm_FOUND)
+
+
+macro(csm_report_not_found REASON_MSG)
+  set(csm_FOUND FALSE)
+  unset(csm_INCLUDE_DIR)
+  unset(csm_LIBRARIES)
+
+  # Reset the CMake module path to its state when this script was called.
+  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+  # FindPackage() use the camelcase library name, not uppercase.
+  if (csm_FIND_QUIETLY)
+    message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN})
+  else (csm_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+    # that prevents generation, but continues configuration.
+    message(SEND_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(csm_report_not_found)
+
+if(NOT csm_FOUND)
+  csm_report_not_found("Something went wrong while setting up csm.")
+endif(NOT csm_FOUND)
+# Set the include directories for csm (itself).
+set(csm_FOUND TRUE)
\ No newline at end of file

src/CMakeLists.txt

@@ -2,7 +2,7 @@
 MESSAGE("Starting laser_scan_utils CMakeLists ...")
 CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
 
-#find dependencies. 
+#find dependencies.
 FIND_PACKAGE(faramotics QUIET) #faramotics is only required for some tests
 IF(faramotics_FOUND)
 	#FIND_PACKAGE(GLUT REQUIRED)
@@ -10,8 +10,12 @@ IF(faramotics_FOUND)
     MESSAGE("Faramotics Library FOUND: Tests requiring it will be built.")
 ENDIF(faramotics_FOUND)
 
+FIND_PACKAGE(csm QUIET)
 #include directories
 INCLUDE_DIRECTORIES(.)
+IF(csm_FOUND)
+    INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR})
+ENDIF(csm_FOUND)
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)
@@ -22,7 +26,7 @@ ENDIF(faramotics_FOUND)
 #headers
 SET(HDRS_BASE
     laser_scan_utils.h)
-    
+
 SET(HDRS
     corner_finder.h
     corner_finder_inscribed_angle.h
@@ -38,7 +42,12 @@ SET(HDRS
     line_segment.h
     point_set.h
     polyline.h
-    scan_segment.h)
+    scan_segment.h
+    )
+  IF(csm_FOUND)
+    SET(HDRS ${HDRS}
+      icp.h)
+  ENDIF(csm_FOUND)
 
 #sources
 SET(SRCS
@@ -56,26 +65,37 @@ SET(SRCS
     line_segment.cpp
     point_set.cpp
     polyline.cpp
-    scan_segment.cpp)
-    
+    scan_segment.cpp
+    )
+  IF(csm_FOUND)
+    SET(SRCS ${SRCS}
+      icp.cpp)
+  ENDIF(csm_FOUND)
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
+target_link_libraries(${PROJECT_NAME} ${csm_LIBRARY})
 
 #install library
 INSTALL(TARGETS ${PROJECT_NAME}
       RUNTIME DESTINATION bin
       LIBRARY DESTINATION lib/iri-algorithms
       ARCHIVE DESTINATION lib/iri-algorithms)
-        
-#install headers         
-INSTALL(FILES ${HDRS_BASE} 
+
+#install headers
+INSTALL(FILES ${HDRS_BASE}
       DESTINATION include/iri-algorithms/laser_scan_utils)
-INSTALL(FILES ${HDRS} 
+INSTALL(FILES ${HDRS}
       DESTINATION include/iri-algorithms/laser_scan_utils)
 
 #install Find*.cmake
 INSTALL(FILES ../Findlaser_scan_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 
 #Build examples & tests
-MESSAGE("Building tests.")
-ADD_SUBDIRECTORY(test)
+IF(NOT BUILD_TESTS)
+  OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(BUILD_TESTS)
+  MESSAGE("Building tests.")
+  ADD_SUBDIRECTORY(test)
+ENDIF(BUILD_TESTS)

src/icp.cpp

+#include "icp.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_){
+                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;
+        }
+
+        // for(int i = 0; i < num_rays; ++i){
+        //     laser_data->theta[i] = laser_data->theta[i] * 0.0175;
+        // }
+        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->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()
+{
+
+}
+
+ICP::~ICP()
+{
+
+}
+
+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
+    // sm_debug_write(true);
+
+    LDWrapper last   = LDWrapper(_last_ls, scan_params);
+    LDWrapper origin = LDWrapper(_origin_ls, scan_params);
+
+    int num_rays = _last_ls.ranges_raw_.size();
+
+    sm_params csm_input{};
+    sm_result csm_output{};
+
+    csm_input.min_reading   = scan_params.range_min_;
+    csm_input.max_reading   = scan_params.range_max_;
+    csm_input.sigma         = scan_params.range_std_dev_;
+
+    csm_input.laser_ref     = origin.laser_data;
+    csm_input.laser_sens    = last.laser_data;
+
+    csm_input.first_guess[0] = _last_transf(0);
+    csm_input.first_guess[1] = _last_transf(1);
+    csm_input.first_guess[2] = _last_transf(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 = _last_transf;
+        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;
+}
+
+void ICP::printLaserData(LDP &laser_data)
+{
+    std::cout << "Laser Reading: " << laser_data->readings[0] << '\n';
+}
+
+void ICP::printTwoLaserData(sm_params &params)
+{
+
+    for (int ii=0; ii<params.laser_ref->nrays-1; ++ii)
+    {
+        std::cout << "Theta: " << params.laser_ref->theta[ii] << "; Readings: "
+                    << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
+                    << '\n';
+    }
+
+}

src/icp.h

+#ifndef ICP_H_
+#define ICP_H_
+
+#include <chrono>
+#include <random>
+#include "laser_scan.h"
+#include <csm/csm_all.h>
+// using namespace CSM;
+
+namespace laserscanutils{
+
+struct icpOutput{
+    int valid; /** 1 if the result is valid */
+    Eigen::Vector3s res_transf;
+    Eigen::Matrix3s res_covar;
+    int nvalid;     /** Number of valid correspondence in the end */
+    double error;   	/** Total correspondence error */
+};
+
+struct icpParams{
+    int use_point_to_line_distance;
+    int max_correspondence_dist;
+    int max_iterations;
+    int use_corr_tricks;
+    double outliers_maxPerc;
+    double outliers_adaptive_order;
+    double outliers_adaptive_mult;
+};
+
+class ICP
+{
+    public:
+        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 &params);
+    static void printLaserData(LDP &laser_data);
+};
+
+}
+
+#endif

src/point_set.cpp

@@ -82,7 +82,7 @@ void PointSet::computeRadius()
     radius_ = sqrt(dd2max);
 }
 
-void PointSet::computeBoundingBox(const double & _clearance)
+void PointSet::computeBoundingBox(const ScalarT & _clearance)
 {
     double cxx, cyy, cxy; //variance and covariance terms
     Eigen::MatrixXd points_o, points_c; //points wrt origin, points wrt cov eigen vectors