From c9d690204a7319f39931abdf49d9d0e6430712c5 Mon Sep 17 00:00:00 2001 From: PepMS Date: Wed, 29 May 2019 08:40:59 +0200 Subject: [PATCH 01/27] icp: added files icp.h icp.cpp --- src/icp.cpp | 0 src/icp.h | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/icp.cpp create mode 100644 src/icp.h diff --git a/src/icp.cpp b/src/icp.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/icp.h b/src/icp.h new file mode 100644 index 0000000..e69de29 -- GitLab From 98e77627861f0f7b5f2ba76813f99eea3dd1d7a7 Mon Sep 17 00:00:00 2001 From: PepMS Date: Wed, 29 May 2019 11:31:31 +0200 Subject: [PATCH 02/27] icp: added ICPWrapper added class ICPWrapper and method matchPC() that has to be linked with CSM library --- src/CMakeLists.txt | 20 +++++++++++--------- src/icp.cpp | 21 +++++++++++++++++++++ src/icp.h | 15 +++++++++++++++ 3 files changed, 47 insertions(+), 9 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a0fbd34..e402dfd 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) @@ -22,7 +22,7 @@ ENDIF(faramotics_FOUND) #headers SET(HDRS_BASE laser_scan_utils.h) - + SET(HDRS corner_finder.h corner_finder_inscribed_angle.h @@ -38,7 +38,8 @@ SET(HDRS line_segment.h point_set.h polyline.h - scan_segment.h) + scan_segment.h + icp.h) #sources SET(SRCS @@ -56,8 +57,9 @@ SET(SRCS line_segment.cpp point_set.cpp polyline.cpp - scan_segment.cpp) - + scan_segment.cpp + icp.cpp) + # create the shared library ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS}) @@ -66,11 +68,11 @@ 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 diff --git a/src/icp.cpp b/src/icp.cpp index e69de29..277536b 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -0,0 +1,21 @@ +#include "icp.h" + +namespace laserscanutils{ + +ICPWrapper::ICPWrapper() +{ + +} + +ICPWrapper::~ICPWrapper() +{ + +} + +void ICPWrapper::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, Eigen::Vector3s &_last_transf, + Eigen::Vector3s &_res_transf, int &num_points, int &error_points) +{ + +} + +} diff --git a/src/icp.h b/src/icp.h index e69de29..59a9616 100644 --- a/src/icp.h +++ b/src/icp.h @@ -0,0 +1,15 @@ +// #include +#include "laser_scan.h" + +// using namespace CSM; +using namespace laserscanutils; + +class ICPWrapper +{ + public: + ICPWrapper(); + ~ICPWrapper(); + + void matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, Eigen::Vector3s &_last_transf, + Eigen::Vector3s &_res_transf, int &num_points, int &error_points); +}; -- GitLab From 2ff8c13bcd3330967d99cbfbd744556d7d607c88 Mon Sep 17 00:00:00 2001 From: Joaquim Casals Date: Wed, 29 May 2019 12:05:42 +0200 Subject: [PATCH 03/27] Modified function definition --- src/icp.cpp | 5 ++--- src/icp.h | 9 ++++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 277536b..adcdb27 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -12,10 +12,9 @@ ICPWrapper::~ICPWrapper() } -void ICPWrapper::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, Eigen::Vector3s &_last_transf, - Eigen::Vector3s &_res_transf, int &num_points, int &error_points) +icp_output ICPWrapper::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, Eigen::Vector3s &_last_transf) { - + return icp_output{}; } } diff --git a/src/icp.h b/src/icp.h index 59a9616..5fd3d7b 100644 --- a/src/icp.h +++ b/src/icp.h @@ -3,13 +3,16 @@ // using namespace CSM; using namespace laserscanutils; - +struct icp_output{ + Eigen::Vector3s res_transf; + int num_points; + int error_points; +}; class ICPWrapper { public: ICPWrapper(); ~ICPWrapper(); - void matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, Eigen::Vector3s &_last_transf, - Eigen::Vector3s &_res_transf, int &num_points, int &error_points); + icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, Eigen::Vector3s &_last_transf); }; -- GitLab From 4ce0741e78aa747385abf627bb1175ef0342ce6c Mon Sep 17 00:00:00 2001 From: PepMS Date: Wed, 29 May 2019 12:21:49 +0200 Subject: [PATCH 04/27] icp: small fix related to namespaces --- src/icp.cpp | 6 ++---- src/icp.h | 7 ++++++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index adcdb27..5ce12ce 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -1,6 +1,6 @@ #include "icp.h" -namespace laserscanutils{ +using namespace laserscanutils; ICPWrapper::ICPWrapper() { @@ -14,7 +14,5 @@ ICPWrapper::~ICPWrapper() icp_output ICPWrapper::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, Eigen::Vector3s &_last_transf) { - return icp_output{}; -} - + return icp_output(); } diff --git a/src/icp.h b/src/icp.h index 5fd3d7b..bc672a0 100644 --- a/src/icp.h +++ b/src/icp.h @@ -2,12 +2,15 @@ #include "laser_scan.h" // using namespace CSM; -using namespace laserscanutils; + +namespace laserscanutils{ + struct icp_output{ Eigen::Vector3s res_transf; int num_points; int error_points; }; + class ICPWrapper { public: @@ -16,3 +19,5 @@ class ICPWrapper icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, Eigen::Vector3s &_last_transf); }; + +} -- GitLab From 069f1bd9d312bcde16e1b3553f57c8943153c735 Mon Sep 17 00:00:00 2001 From: Joaquim Casals Date: Wed, 29 May 2019 16:23:04 +0200 Subject: [PATCH 05/27] 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 _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 #include "laser_scan.h" +#include // 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 From 268d516d1fd27450273a14217083273c9ee023a7 Mon Sep 17 00:00:00 2001 From: PepMS Date: Thu, 30 May 2019 09:58:45 +0200 Subject: [PATCH 06/27] Minor Changes --- src/icp.cpp | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 4b4f0f4..81e5d47 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -28,13 +28,13 @@ public: ++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; + // laser_data->odometry[0] = 1/0.0; + // laser_data->odometry[1] = 1/0.0; + // laser_data->odometry[2] = 1/0.0; + // + // laser_data->true_pose[0] = 1/0.0; + // laser_data->true_pose[1] = 1/0.0; + // laser_data->true_pose[2] = 1/0.0; } ~LDWrapper(){ ld_free(laser_data); @@ -61,18 +61,27 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar 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; + + 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 = true; + //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; + std::cout << "Result: " << csm_output.valid << '\n'; + // 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; } -- GitLab From f85169463ac8b0f006191aeebcb8705a3dc066dd Mon Sep 17 00:00:00 2001 From: PepMS Date: Thu, 30 May 2019 15:05:39 +0200 Subject: [PATCH 07/27] WIP --- src/icp.cpp | 52 ++++++++++++++++++++++++++-------------------------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 81e5d47..a7c938e 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -9,16 +9,13 @@ public: { 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; + 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; - // std::cout << "Current Reading " << i << " " << it << std::endl; laser_data->valid[i] = 1; }else{ laser_data->readings[i] = -1; @@ -28,13 +25,16 @@ public: ++i; } - // laser_data->odometry[0] = 1/0.0; - // laser_data->odometry[1] = 1/0.0; - // laser_data->odometry[2] = 1/0.0; - // - // laser_data->true_pose[0] = 1/0.0; - // laser_data->true_pose[1] = 1/0.0; - // laser_data->true_pose[2] = 1/0.0; + 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); @@ -53,30 +53,30 @@ ICP::~ICP() icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf) { - LDWrapper last = LDWrapper(_last_ls, params); - // last->odometry[0] = _last_transf(0); - // last->odometry[1] = _last_transf(1); - // last->odometry[2] = _last_transf(2); + sm_debug_write(true); + + LDWrapper last = LDWrapper(_last_ls, params); 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 = origin.laser_data; - csm_input.laser_sens = last.laser_data; + csm_input.min_reading = params.range_min_; + csm_input.max_reading = params.range_max_; + + 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 = true; - //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]; + csm_input.use_point_to_line_distance = true; sm_icp(&csm_input, &csm_output); - std::cout << "Result: " << csm_output.valid << '\n'; + // std::cout << "Result: " << csm_output.valid << '\n'; // 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]; -- GitLab From a7930fe814438aa0234ec9ed883389ab2d1104a5 Mon Sep 17 00:00:00 2001 From: Joaquim Casals Date: Thu, 30 May 2019 15:55:52 +0200 Subject: [PATCH 08/27] Using random inicialization for debugging purposes --- src/icp.cpp | 15 +++++++++++---- src/icp.h | 2 ++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index a7c938e..07bbd60 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -1,6 +1,8 @@ #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 dis(0.0, 1.0); class LDWrapper { public: @@ -69,9 +71,14 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar 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.first_guess[0] = _last_transf(0); + // csm_input.first_guess[1] = _last_transf(1); + // csm_input.first_guess[2] = _last_transf(2); + + csm_input.first_guess[0] = dis(generator)*3; + csm_input.first_guess[1] = dis(generator)*3; + double aux = dis(generator); + csm_input.first_guess[2] = -1.14*(1-aux) + 1.14*aux; csm_input.use_point_to_line_distance = true; diff --git a/src/icp.h b/src/icp.h index 98c4527..25e5f4b 100644 --- a/src/icp.h +++ b/src/icp.h @@ -1,4 +1,6 @@ // #include +#include +#include #include "laser_scan.h" #include -- GitLab From a2733fa231dfa0e4ed7be8ea06d0cc37175597a5 Mon Sep 17 00:00:00 2001 From: PepMS Date: Fri, 31 May 2019 12:37:28 +0200 Subject: [PATCH 09/27] sm: matchICP() is working after some tuning See comments in the code and in the csm library --- src/icp.cpp | 39 ++++++++++++++++++++++++++++++++++----- src/icp.h | 3 +++ 2 files changed, 37 insertions(+), 5 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index a7c938e..3656a17 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -18,7 +18,7 @@ public: laser_data->readings[i] = it; laser_data->valid[i] = 1; }else{ - laser_data->readings[i] = -1; + laser_data->readings[i] = NAN; laser_data->valid[i] = 0; } laser_data->cluster[i] = -1; @@ -53,7 +53,8 @@ ICP::~ICP() icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf) { - sm_debug_write(true); + // Uncomment to enable debug messages from the CSM library + // sm_debug_write(true); LDWrapper last = LDWrapper(_last_ls, params); LDWrapper origin = LDWrapper(_origin_ls, params); @@ -73,15 +74,43 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar csm_input.first_guess[1] = _last_transf(1); csm_input.first_guess[2] = _last_transf(2); - csm_input.use_point_to_line_distance = true; + csm_input.use_point_to_line_distance = 1; + + csm_input.max_correspondence_dist = 1; + csm_input.max_iterations = 20; + + csm_input.use_corr_tricks = 1; + csm_input.outliers_maxPerc = 0.9; + csm_input.outliers_adaptive_order = 0.7; + csm_input.outliers_adaptive_mult = 1.5; sm_icp(&csm_input, &csm_output); - // std::cout << "Result: " << csm_output.valid << '\n'; - // 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]; + 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; iinrays-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 index 98c4527..ef9dee6 100644 --- a/src/icp.h +++ b/src/icp.h @@ -19,6 +19,9 @@ class ICP ~ICP(); static icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf); + + static void printTwoLaserData(sm_params ¶ms); + static void printLaserData(LDP &laser_data); }; } -- GitLab From 26da6eb1c3d7c16fe58537ccbf7860b0a5b991e9 Mon Sep 17 00:00:00 2001 From: PepMS Date: Fri, 31 May 2019 12:40:29 +0200 Subject: [PATCH 10/27] conflicts: solved conficts after pull --- src/icp.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index d58d811..52b5a45 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -72,14 +72,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar 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.first_guess[0] = dis(generator)*3; - csm_input.first_guess[1] = dis(generator)*3; - double aux = dis(generator); - csm_input.first_guess[2] = -1.14*(1-aux) + 1.14*aux; + 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 = 1; @@ -98,9 +93,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar result.res_transf(1) = csm_output.x[1]; result.res_transf(2) = csm_output.x[2]; - 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'; + // 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; } -- GitLab From ebc3cb778233fc3a227c8604702c1dc62da8ac01 Mon Sep 17 00:00:00 2001 From: Joaquim Casals Date: Tue, 11 Jun 2019 15:01:46 +0200 Subject: [PATCH 11/27] Added icp parameters as a parameter to the ICP matching function --- src/icp.cpp | 20 +++++++++----------- src/icp.h | 14 +++++++++++--- 2 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 52b5a45..b22a1ae 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -53,7 +53,7 @@ ICP::~ICP() } -icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf) +icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf) { // Uncomment to enable debug messages from the CSM library // sm_debug_write(true); @@ -76,19 +76,17 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar csm_input.first_guess[1] = _last_transf(1); csm_input.first_guess[2] = _last_transf(2); - csm_input.use_point_to_line_distance = 1; - - csm_input.max_correspondence_dist = 1; - csm_input.max_iterations = 20; - - csm_input.use_corr_tricks = 1; - csm_input.outliers_maxPerc = 0.9; - csm_input.outliers_adaptive_order = 0.7; - csm_input.outliers_adaptive_mult = 1.5; + 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; sm_icp(&csm_input, &csm_output); - icp_output result{}; + icpOutput 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]; diff --git a/src/icp.h b/src/icp.h index f0c5a97..939565d 100644 --- a/src/icp.h +++ b/src/icp.h @@ -8,19 +8,27 @@ namespace laserscanutils{ -struct icp_output{ +struct icpOutput{ Eigen::Vector3s res_transf; int num_points; int error_points; }; - +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 icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf); + static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf); static void printTwoLaserData(sm_params ¶ms); static void printLaserData(LDP &laser_data); -- GitLab From c199ff5a610981b074aedabbe4962ca8bfb785eb Mon Sep 17 00:00:00 2001 From: PepMS Date: Wed, 26 Jun 2019 15:11:32 +0200 Subject: [PATCH 12/27] Added gitignore --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8ae397c --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +#Ignore build, bin and lib folders +bin/ +build/ +lib/ -- GitLab From 9f4262add8bd40c9925b10147fbf079d4668d8b1 Mon Sep 17 00:00:00 2001 From: PepMS Date: Wed, 26 Jun 2019 17:39:57 +0200 Subject: [PATCH 13/27] Output struct changed --- src/icp.cpp | 3 +++ src/icp.h | 6 ++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index b22a1ae..a2ca9a9 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -91,6 +91,9 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara result.res_transf(1) = csm_output.x[1]; result.res_transf(2) = csm_output.x[2]; + result.nvalid = csm_output.nvalid; + result.error = csm_output.error; + // 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'; diff --git a/src/icp.h b/src/icp.h index 939565d..eb58cac 100644 --- a/src/icp.h +++ b/src/icp.h @@ -10,9 +10,10 @@ namespace laserscanutils{ struct icpOutput{ Eigen::Vector3s res_transf; - int num_points; - int error_points; + 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; @@ -22,6 +23,7 @@ struct icpParams{ double outliers_adaptive_order; double outliers_adaptive_mult; }; + class ICP { public: -- GitLab From 1cc6ff5852d9b2c9baa1255ce739201fa84ee08b Mon Sep 17 00:00:00 2001 From: PepMS Date: Wed, 3 Jul 2019 07:10:21 +0200 Subject: [PATCH 14/27] Minor changes --- src/icp.cpp | 5 ++++- src/icp.h | 6 ++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index a2ca9a9..617a09b 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -92,7 +92,10 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara result.res_transf(2) = csm_output.x[2]; result.nvalid = csm_output.nvalid; - result.error = csm_output.error; + result.error = csm_output.error; + + std::cout << "ICP: add covariance!" << '\n'; + // result.res_covar = Eigen::Zeros(); // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n'; // std::cout << "Number of iterations: " << csm_output.iterations << '\n'; diff --git a/src/icp.h b/src/icp.h index eb58cac..fb222aa 100644 --- a/src/icp.h +++ b/src/icp.h @@ -3,15 +3,17 @@ #include #include "laser_scan.h" #include - +#include Date: Wed, 3 Jul 2019 07:11:59 +0200 Subject: [PATCH 15/27] Include guards --- src/icp.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/icp.h b/src/icp.h index fb222aa..48ec48d 100644 --- a/src/icp.h +++ b/src/icp.h @@ -1,4 +1,6 @@ -// #include +#ifndef ICP_H_ +#define ICP_H_ + #include #include #include "laser_scan.h" @@ -13,7 +15,7 @@ struct icpOutput{ Eigen::Vector3s res_transf; Eigen::Matrix3s res_covar; int nvalid; /** Number of valid correspondence in the end */ - double error; /** Total correspondence error */ + double error; /** Total correspondence error */ }; struct icpParams{ -- GitLab From 0b9326930706baa7bdf4af42f1cd03095758addb Mon Sep 17 00:00:00 2001 From: PepMS Date: Wed, 3 Jul 2019 09:12:02 +0200 Subject: [PATCH 16/27] minor bug --- src/icp.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/icp.h b/src/icp.h index 48ec48d..a4c2861 100644 --- a/src/icp.h +++ b/src/icp.h @@ -5,7 +5,6 @@ #include #include "laser_scan.h" #include -#include Date: Wed, 3 Jul 2019 14:05:34 +0200 Subject: [PATCH 17/27] Covar: Hardocded. Need to be correctly map from gsl --- src/icp.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 617a09b..cf83798 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -94,8 +94,8 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara result.nvalid = csm_output.nvalid; result.error = csm_output.error; - std::cout << "ICP: add covariance!" << '\n'; - // result.res_covar = Eigen::Zeros(); + + result.res_covar = Eigen::Matrix3s::Identity()*0.1; // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n'; // std::cout << "Number of iterations: " << csm_output.iterations << '\n'; -- GitLab From 284d3dcc37cdc7448d98eb1284d59ca79caf9c13 Mon Sep 17 00:00:00 2001 From: jcasals Date: Mon, 22 Jul 2019 09:55:24 +0200 Subject: [PATCH 18/27] Getting Censi's covariance and transforming it to Eigen --- src/icp.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/icp.cpp b/src/icp.cpp index cf83798..55a92f4 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -84,6 +84,8 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara 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{}; @@ -95,7 +97,9 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara result.error = csm_output.error; - result.res_covar = Eigen::Matrix3s::Identity()*0.1; + 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); // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n'; // std::cout << "Number of iterations: " << csm_output.iterations << '\n'; -- GitLab From f35100e1a5c1d2b760d9366e496df22e6ba17372 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= Date: Sat, 3 Aug 2019 20:40:32 +0200 Subject: [PATCH 19/27] Add option for building tests --- .gitignore | 3 +++ src/CMakeLists.txt | 10 ++++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 8ae397c..c99a3e2 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,6 @@ bin/ build/ lib/ +.settings/language.settings.xml +.project +.cproject diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ec68a5f..041a112 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -82,5 +82,11 @@ INSTALL(FILES ${HDRS} 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) -- GitLab From 54d6e1612fa0a780511b94d34a299b068679835c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= Date: Sat, 3 Aug 2019 20:41:00 +0200 Subject: [PATCH 20/27] Work around call to GSL function --- src/icp.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/icp.cpp b/src/icp.cpp index 55a92f4..936e196 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -99,12 +99,16 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara 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); + 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 // 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'; + std::cout << __FILE__ << __LINE__ << "cov: " << result.res_covar << std::endl; + return result; } -- GitLab From b996b95a89a5f69a1af6024c7888c3ddc5ec0694 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= Date: Sat, 3 Aug 2019 20:41:26 +0200 Subject: [PATCH 21/27] Use ScalarT not double --- src/point_set.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/point_set.cpp b/src/point_set.cpp index 3f797b3..aed39e4 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 -- GitLab From 3fc3e4b4dafb5ae20a0ac1962df23011ff7286f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= Date: Wed, 7 Aug 2019 10:56:04 +0200 Subject: [PATCH 22/27] Minor cosmetic changes. --- src/icp.cpp | 31 +++++++++++++++---------------- src/icp.h | 2 +- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 936e196..0987553 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -53,38 +53,39 @@ ICP::~ICP() } -icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf) +icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& scan_params, 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, params); - LDWrapper origin = LDWrapper(_origin_ls, params); + 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 = params.range_min_; - csm_input.max_reading = params.range_max_; + 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.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.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; + csm_input.do_compute_covariance = 1; sm_icp(&csm_input, &csm_output); @@ -107,8 +108,6 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara // std::cout << "Number of iterations: " << csm_output.iterations << '\n'; // std::cout << "Error: " << csm_output.error << '\n'; - std::cout << __FILE__ << __LINE__ << "cov: " << result.res_covar << std::endl; - return result; } diff --git a/src/icp.h b/src/icp.h index a4c2861..603b2c9 100644 --- a/src/icp.h +++ b/src/icp.h @@ -33,7 +33,7 @@ class ICP ICP(); ~ICP(); - static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf); + static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& scan_params, icpParams &icp_params, Eigen::Vector3s &_last_transf); static void printTwoLaserData(sm_params ¶ms); static void printLaserData(LDP &laser_data); -- GitLab From 465d09022d5b30676886f208efc6901a82e34317 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= Date: Wed, 7 Aug 2019 13:19:30 +0200 Subject: [PATCH 23/27] Rename matchPC --> align --- src/icp.cpp | 4 ++-- src/icp.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 0987553..6536fc6 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -7,7 +7,7 @@ std::uniform_real_distribution dis(0.0, 1.0); class LDWrapper { public: LDP laser_data; - LDWrapper(LaserScan& scan, LaserScanParams& scan_params) + LDWrapper(const LaserScan& scan, const LaserScanParams& scan_params) { int num_rays = scan.ranges_raw_.size(); laser_data = ld_alloc_new(num_rays); @@ -53,7 +53,7 @@ ICP::~ICP() } -icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& scan_params, icpParams &icp_params, Eigen::Vector3s &_last_transf) +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); diff --git a/src/icp.h b/src/icp.h index 603b2c9..d6345fc 100644 --- a/src/icp.h +++ b/src/icp.h @@ -33,7 +33,7 @@ class ICP ICP(); ~ICP(); - static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& scan_params, icpParams &icp_params, Eigen::Vector3s &_last_transf); + 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); -- GitLab From d61637c51b37819a6c7964c4ee7bfc57ae26513c Mon Sep 17 00:00:00 2001 From: jcasals Date: Wed, 4 Sep 2019 16:08:08 +0200 Subject: [PATCH 24/27] Added conversion of laser_data->theta to radians --- src/icp.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/icp.cpp b/src/icp.cpp index 6536fc6..4cd599a 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -27,6 +27,9 @@ public: ++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]; -- GitLab From 50498f347a212e6b01f28a7dd97eed03b6636e24 Mon Sep 17 00:00:00 2001 From: jcasals Date: Tue, 1 Oct 2019 10:57:30 +0200 Subject: [PATCH 25/27] Made CSM optional --- src/CMakeLists.txt | 19 ++++++++++++++----- src/icp.cpp | 1 + 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 041a112..ee3f8e7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,10 +10,12 @@ IF(faramotics_FOUND) MESSAGE("Faramotics Library FOUND: Tests requiring it will be built.") ENDIF(faramotics_FOUND) -FIND_PACKAGE(csm REQUIRED) +FIND_PACKAGE(csm QUIET) #include directories INCLUDE_DIRECTORIES(.) -INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR}) +IF(csm_FOUND) + INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR}) +ENDIF(csm_FOUND) IF(Ceres_FOUND) INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) ENDIF(Ceres_FOUND) @@ -41,7 +43,11 @@ SET(HDRS point_set.h polyline.h scan_segment.h - icp.h) + ) + IF(csm_FOUND) + SET(HDRS ${HDRS} + icp.h) + ENDIF(csm_FOUND) #sources SET(SRCS @@ -60,8 +66,11 @@ SET(SRCS point_set.cpp polyline.cpp scan_segment.cpp - icp.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}) diff --git a/src/icp.cpp b/src/icp.cpp index 4cd599a..b41958d 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -88,6 +88,7 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con 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); -- GitLab From 835e4803b4258549679608e66b6e2fb8a50fb042 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Tue, 1 Oct 2019 18:14:52 +0200 Subject: [PATCH 26/27] returning initial guess if valid != 1 --- src/icp.cpp | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index 4cd599a..32a4184 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -93,19 +93,28 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con sm_icp(&csm_input, &csm_output); icpOutput 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]; - result.nvalid = csm_output.nvalid; + result.valid = csm_output.valid; result.error = csm_output.error; - - 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 + 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'; -- GitLab From 1b566f72d0dd19dcd9ef66ec7749b9e89eb0c2cb Mon Sep 17 00:00:00 2001 From: jcasals Date: Thu, 24 Oct 2019 15:19:12 +0200 Subject: [PATCH 27/27] Removed shortcut to set angles to radians; now it is properly set at config time --- src/icp.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/icp.cpp b/src/icp.cpp index c6b1b08..c137f9b 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -27,9 +27,9 @@ public: ++i; } - for(int i = 0; i < num_rays; ++i){ - laser_data->theta[i] = laser_data->theta[i] * 0.0175; - } + // 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]; -- GitLab