diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..c99a3e22087bd637b9d4de70dec55977cd422a95
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,7 @@
+#Ignore build, bin and lib folders
+bin/
+build/
+lib/
+.settings/language.settings.xml
+.project
+.cproject
diff --git a/CMakeLists.txt b/CMakeLists.txt
index feda53e6d5abcbbf77308df98e9dfd423563e2db..26e07801dedb4ad9366e4251a1ff1721bb2e591a 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 0000000000000000000000000000000000000000..938022e920cddb287f773480596ffe497d8a0306
--- /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 a0fbd34c48eb9ab048f3bb5faf1442f014bda7e6..ee3f8e7da535c1964752088000607a9485bb91b1 100644
--- a/src/CMakeLists.txt
+++ b/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)
diff --git a/src/icp.cpp b/src/icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c137f9bfdb6095e689b646496a64bfa2ac3e1fbf
--- /dev/null
+++ b/src/icp.cpp
@@ -0,0 +1,142 @@
+#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 ¶ms)
+{
+
+ 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';
+ }
+
+}
diff --git a/src/icp.h b/src/icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6345fc992ea5657445b02f2fdd04b5d614eacbb
--- /dev/null
+++ b/src/icp.h
@@ -0,0 +1,44 @@
+#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 ¶ms);
+ static void printLaserData(LDP &laser_data);
+};
+
+}
+
+#endif
diff --git a/src/point_set.cpp b/src/point_set.cpp
index 3f797b3e0eb30c1feae0b20ba4191e07f88fa4b4..aed39e414586b444e7bfc805a85ceb1c041c6801 100644
--- a/src/point_set.cpp
+++ b/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