Added scan matching functionality

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

@@ -10,8 +10,10 @@ IF(faramotics_FOUND)
     MESSAGE("Faramotics Library FOUND: Tests requiring it will be built.")
 ENDIF(faramotics_FOUND)
 
+FIND_PACKAGE(csm REQUIRED)
 #include directories
 INCLUDE_DIRECTORIES(.)
+INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR})
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)
@@ -62,6 +64,7 @@ SET(SRCS
 
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
+target_link_libraries(${PROJECT_NAME} ${csm_LIBRARY})
 
 #install library
 INSTALL(TARGETS ${PROJECT_NAME}

src/icp.cpp

@@ -2,17 +2,77 @@
 
 using namespace laserscanutils;
 
-ICPWrapper::ICPWrapper()
+class LDWrapper {
+public:
+    LDP laser_data;
+    LDWrapper(LaserScan& scan, LaserScanParams& scan_params)
+    {
+        int num_rays = scan.ranges_raw_.size();
+        laser_data = ld_alloc_new(num_rays);
+        laser_data->nrays = num_rays;
+        laser_data->min_theta = 0;
+        laser_data->max_theta = scan_params.angle_max_;
+        double delta_theta = (laser_data->max_theta - laser_data->min_theta)/num_rays;
+        int i = 0;
+        for(auto it : scan.ranges_raw_){
+            laser_data->theta[i] = laser_data->min_theta + i*delta_theta;
+            if(scan_params.range_min_ <= it and it <= scan_params.range_max_){
+                laser_data->readings[i] = it;
+                // std::cout << "Current Reading " << i << " " << it << std::endl;
+                laser_data->valid[i] = 1;
+            }else{
+                laser_data->readings[i] = -1;
+                laser_data->valid[i] = 0;
+            }
+            laser_data->cluster[i] = -1;
+            ++i;
+        }
+
+        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()
 {
 
 }
 
-ICPWrapper::~ICPWrapper()
+ICP::~ICP()
 {
 
 }
 
-icp_output ICPWrapper::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, Eigen::Vector3s &_last_transf)
+icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf)
 {
-    return icp_output();
+    LDWrapper last = LDWrapper(_last_ls, params);
+    // last->odometry[0] = _last_transf(0);
+    // last->odometry[1] = _last_transf(1);
+    // last->odometry[2] = _last_transf(2);
+    LDWrapper origin = LDWrapper(_origin_ls, params);
+    int num_rays = _last_ls.ranges_raw_.size();
+    sm_params csm_input{};
+    sm_result csm_output{};
+    csm_input.laser_ref = last.laser_data;
+    csm_input.laser_sens = origin.laser_data;
+    //TODO: min_theta and max_theta should come from LaserScanParams
+    last.laser_data->min_theta = last.laser_data->theta[0];
+    last.laser_data->max_theta = last.laser_data->theta[num_rays-1];
+    origin.laser_data->min_theta = origin.laser_data->theta[0];
+    origin.laser_data->max_theta = origin.laser_data->theta[num_rays-1];
+    sm_icp(&csm_input, &csm_output);
+    std::cout << "My solution " << csm_output.x[0] << "," << csm_output.x[1] << "," << csm_output.x[2] << std::endl;
+    icp_output result{};
+    result.res_transf(0) = csm_output.x[0];
+    result.res_transf(1) = csm_output.x[1];
+    result.res_transf(2) = csm_output.x[2];
+    return result;
 }

src/icp.h

 // #include <csm/csm_all.h>
 #include "laser_scan.h"
+#include <csm/csm_all.h>
 
 // using namespace CSM;
 
@@ -11,13 +12,13 @@ struct icp_output{
     int error_points;
 };
 
-class ICPWrapper
+class ICP
 {
     public:
-        ICPWrapper();
-        ~ICPWrapper();
+        ICP();
+        ~ICP();
 
-        icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, Eigen::Vector3s &_last_transf);
+    static icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf);
 };
 
 }