From 069f1bd9d312bcde16e1b3553f57c8943153c735 Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Wed, 29 May 2019 16:23:04 +0200
Subject: [PATCH] Added scan matching functionality
---
CMakeLists.txt | 1 +
cmake_modules/Findcsm.cmake | 64 ++++++++++++++++++++++++++++++++++
src/CMakeLists.txt | 3 ++
src/icp.cpp | 68 ++++++++++++++++++++++++++++++++++---
src/icp.h | 9 ++---
5 files changed, 137 insertions(+), 8 deletions(-)
create mode 100644 cmake_modules/Findcsm.cmake
diff --git a/CMakeLists.txt b/CMakeLists.txt
index feda53e..26e0780 100644
--- a/CMakeLists.txt
+++ b/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")
diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake
new file mode 100644
index 0000000..938022e
--- /dev/null
+++ b/cmake_modules/Findcsm.cmake
@@ -0,0 +1,64 @@
+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
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e402dfd..ec68a5f 100644
--- a/src/CMakeLists.txt
+++ b/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}
diff --git a/src/icp.cpp b/src/icp.cpp
index 5ce12ce..4b4f0f4 100644
--- a/src/icp.cpp
+++ b/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;
}
diff --git a/src/icp.h b/src/icp.h
index bc672a0..98c4527 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -1,5 +1,6 @@
// #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);
};
}
--
GitLab