diff --git a/.gitignore b/.gitignore
index c164a29ddb05af0133f9ceedc983dd5ec4ad84d9..4f9c828fc38f880196ae198c5298798f83909e84 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,6 @@
#Ignore build, bin and lib folders
bin/
-build/
+build*/
lib/
.settings/language.settings.xml
.project
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5ed94b631561753a3a8eea35a562bf73c5728e82..4d5bb47d08ba7981bbd0728ccc58bacd24e724b3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,23 +1,17 @@
-
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-if(COMMAND cmake_policy)
- cmake_policy(SET CMP0005 NEW)
- cmake_policy(SET CMP0003 NEW)
-
-endif(COMMAND cmake_policy)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
# The project name and the type of project
PROJECT(laser_scan_utils)
+# Paths
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")
+set(INCLUDE_INSTALL_DIR include/iri-algorithms)
+set(LIB_INSTALL_DIR lib/)
+# Build type
IF (NOT CMAKE_BUILD_TYPE)
- #SET(CMAKE_BUILD_TYPE "DEBUG")
SET(CMAKE_BUILD_TYPE "RELEASE")
ENDIF (NOT CMAKE_BUILD_TYPE)
message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
@@ -26,50 +20,205 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
-#find dependencies
-FIND_PACKAGE(Eigen3 3.3 REQUIRED)
-FIND_PACKAGE(csm QUIET)
-FIND_PACKAGE(falkolib QUIET)
+#Set compiler according C++11 support
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+if(COMPILER_SUPPORTS_CXX11)
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+else()
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
-#Build tests
+# Options
IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
-#Build demos
IF(NOT BUILD_DEMOS)
OPTION(BUILD_DEMOS "Build demos" ON)
ENDIF(NOT BUILD_DEMOS)
-#Set compiler according C++11 support
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-else()
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+if(BUILD_TESTS)
+ # Enables testing for this directory and below.
+ # Note that ctest expects to find a test file in the build directory root.
+ # Therefore, this command should be in the source directory root.
+ #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
+ enable_testing()
endif()
-ADD_SUBDIRECTORY(src)
+# ============ DEPENDENCIES ============
+FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
+FIND_PACKAGE(falkolib QUIET)
+find_package(PkgConfig)
+pkg_check_modules(csm QUIET csm )
+link_directories(${csm_LIBRARY_DIRS})
+if(csm_FOUND)
+ if(NOT csm_LIBRARY_DIRS)
+ message("csm_LIBRARY_DIRS is empty")
+ else()
+ #Â keep only the files in a lib folder: by default the "." is in csm_LIBRARY_DIRS,
+ # which is not acceptable for target_link_directories
+ message(csm_LIBRARY_DIRS:)
+ foreach (my_entry IN LISTS csm_LIBRARY_DIRS)
+ message("csm_LIBRARY_DIRS has ${my_entry}")
+ endforeach()
+ list(FILTER csm_LIBRARY_DIRS INCLUDE REGEX "lib")
+ foreach (my_entry IN LISTS csm_LIBRARY_DIRS)
+ message("csm_LIBRARY_DIRS has ${my_entry}")
+ endforeach()
+ endif()
+endif()
+
+# ============ INCLUDES ============
+INCLUDE_DIRECTORIES("include")
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
+
+
+#headers
+SET(HDRS_BASE
+ include/laser_scan_utils/laser_scan_utils.h)
+
+SET(HDRS
+ include/laser_scan_utils/corner_finder.h
+ include/laser_scan_utils/corner_finder_inscribed_angle.h
+ include/laser_scan_utils/corner_point.h
+ include/laser_scan_utils/grid_2d.h
+ include/laser_scan_utils/grid_cluster.h
+ include/laser_scan_utils/laser_scan.h
+ include/laser_scan_utils/laser_scan_with_params.h
+ include/laser_scan_utils/line_finder.h
+ include/laser_scan_utils/line_finder_hough.h
+ include/laser_scan_utils/line_finder_iterative.h
+ include/laser_scan_utils/line_finder_jump_fit.h
+ include/laser_scan_utils/line_segment.h
+ include/laser_scan_utils/point_set.h
+ include/laser_scan_utils/polyline.h
+ include/laser_scan_utils/scan_segment.h
+ include/laser_scan_utils/loop_closure_base.h
+ include/laser_scan_utils/scene_base.h
+ )
+ IF(csm_FOUND)
+ SET(HDRS ${HDRS}
+ include/laser_scan_utils/icp.h)
+ ENDIF(csm_FOUND)
+
+ IF(falkolib_FOUND)
+ SET(HDRS ${HDRS}
+ include/laser_scan_utils/corner_falko_2d.h
+ include/laser_scan_utils/loop_closure_falko.h
+ include/laser_scan_utils/scene_falko.h
+ include/laser_scan_utils/match_loop_closure_scene.h)
+ ENDIF(falkolib_FOUND)
+
+#sources
+SET(SRCS
+ src/corner_finder.cpp
+ src/corner_finder_inscribed_angle.cpp
+ src/corner_point.cpp
+ src/grid_2d.cpp
+ src/grid_cluster.cpp
+ src/laser_scan.cpp
+ src/laser_scan_with_params.cpp
+ src/line_finder.cpp
+ src/line_finder_hough.cpp
+ src/line_finder_iterative.cpp
+ src/line_finder_jump_fit.cpp
+ src/line_segment.cpp
+ src/point_set.cpp
+ src/polyline.cpp
+ src/scan_segment.cpp
+ )
+ IF(csm_FOUND)
+ SET(SRCS ${SRCS}
+ src/icp.cpp)
+ ENDIF(csm_FOUND)
+
+ IF(falkolib_FOUND)
+ SET(SRCS ${SRCS}
+ src/corner_falko_2d.cpp)
+ ENDIF(falkolib_FOUND)
+
+
+# create the shared library
+ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
+
+#Link the created libraries
+#=============================================================
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC Eigen3::Eigen)
+
+IF(csm_FOUND)
+ MESSAGE(STATUS "CSM lib found")
+ TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC ${csm_LIBRARIES})
+ target_include_directories(${PROJECT_NAME} PUBLIC ${csm_INCLUDE_DIRS})
+ if(${CMAKE_VERSION} VERSION_LESS "3.13.0")
+ link_directories(${PROJECT_NAME} PUBLIC ${csm_LIBRARY_DIRS})
+ else()
+ target_link_directories(${PROJECT_NAME} PUBLIC ${csm_LIBRARY_DIRS})
+ endif()
+ENDIF(csm_FOUND)
+IF(falkolib_FOUND)
+ MESSAGE(STATUS "FALKO lib found:")
+ TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC ${falkolib_LIBRARY})
+ target_include_directories(${PROJECT_NAME} PUBLIC ${falkolib_INCLUDE_DIRS})
+ENDIF(falkolib_FOUND)
+
+
+#Build tests
+#========================================
+IF(BUILD_DEMOS)
+ MESSAGE("Building examples.")
+ ADD_SUBDIRECTORY(examples)
+ENDIF(BUILD_DEMOS)
#Build tests
IF(BUILD_TESTS)
- # Enables testing for this directory and below.
- # Note that ctest expects to find a test file in the build directory root.
- # Therefore, this command should be in the source directory root.
- #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
- MESSAGE("Building tests.")
- enable_testing()
- set(_LASER_SCAN_UTILS_ROOT_DIR ${CMAKE_SOURCE_DIR})
- add_subdirectory(test)
+ MESSAGE("Building tests.")
+ set(_LASER_SCAN_UTILS_ROOT_DIR ${CMAKE_SOURCE_DIR})
+ add_subdirectory(test)
ENDIF(BUILD_TESTS)
-FIND_PACKAGE(Doxygen)
+#install library
+#=============================================================
+INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION ${LIB_INSTALL_DIR}
+ ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
+)
+install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/${PROJECT_NAME}/cmake)
+
+
+#Â Configure the package installation
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+ ${CMAKE_SOURCE_DIR}/cmake_modules/${PROJECT_NAME}Config.cmake.in
+ ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake
+ INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PROJECT_NAME}/cmake
+ PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
+)
+
+install(
+ FILES
+ ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake
+ DESTINATION
+ ${LIB_INSTALL_DIR}/${PROJECT_NAME}/cmake
+)
+
+# Specifies include directories to use when compiling the plugin target
+# This way, include_directories does not need to be called in plugins depending on this one
+target_include_directories(${PROJECT_NAME} INTERFACE
+ $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
+)
+
+#install headers
+INSTALL(FILES ${HDRS_BASE}
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME})
+INSTALL(FILES ${HDRS}
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME})
+
+export(PACKAGE ${PROJECT_NAME})
+
+FIND_PACKAGE(Doxygen MODULE)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
IF (IRI_DOC_DIR)
diff --git a/Findlaser_scan_utils.cmake b/Findlaser_scan_utils.cmake
deleted file mode 100644
index 7404338873ac262162217c06ebe8a2463fed0901..0000000000000000000000000000000000000000
--- a/Findlaser_scan_utils.cmake
+++ /dev/null
@@ -1,28 +0,0 @@
-FIND_PATH(
- laser_scan_utils_INCLUDE_DIRS
- NAMES laser_scan_utils.h
- PATHS /usr/local/include/iri-algorithms/laser_scan_utils)
-#change INCLUDE_DIRS to its parent directory
-get_filename_component(laser_scan_utils_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS} DIRECTORY)
-
-FIND_LIBRARY(
- laser_scan_utils_LIBRARY
- NAMES laser_scan_utils
- PATHS /usr/lib /usr/local/lib)
-
-IF (laser_scan_utils_INCLUDE_DIRS AND laser_scan_utils_LIBRARY)
- SET(laser_scan_utils_FOUND TRUE)
- SET(laser_scan_utils_INCLUDE_DIR ${laser_scan_utils_INCLUDE_DIRS})
- SET(laser_scan_utils_LIBRARIES ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_INCLUDE_DIRS AND laser_scan_utils_LIBRARY)
-
-IF (laser_scan_utils_FOUND)
- IF (NOT laser_scan_utils_FIND_QUIETLY)
- MESSAGE(STATUS "Found laser_scan_utils: ${laser_scan_utils_LIBRARY}")
- ENDIF (NOT laser_scan_utils_FIND_QUIETLY)
-ELSE (laser_scan_utils_FOUND)
- IF (laser_scan_utils_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find laser_scan_utils")
- ENDIF (laser_scan_utils_FIND_REQUIRED)
-ENDIF (laser_scan_utils_FOUND)
-
diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake
deleted file mode 100644
index 953f9c5113d9139cefaaa508043f5d623072cb8d..0000000000000000000000000000000000000000
--- a/cmake_modules/Findcsm.cmake
+++ /dev/null
@@ -1,64 +0,0 @@
-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 "Ignoring csm dependency - " ${REASON_MSG} ${ARGN})
- elseif(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)
diff --git a/cmake_modules/laser_scan_utilsConfig.cmake.in b/cmake_modules/laser_scan_utilsConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..699253a8747f367e40732cfbab17ded58218e1b5
--- /dev/null
+++ b/cmake_modules/laser_scan_utilsConfig.cmake.in
@@ -0,0 +1,9 @@
+set(@PROJECT_NAME@_VERSION 0.0.1)
+
+
+@PACKAGE_INIT@
+
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake")
+
+check_required_components(@PROJECT_NAME@)
\ No newline at end of file
diff --git a/src/examples/CMakeLists.txt b/examples/CMakeLists.txt
similarity index 100%
rename from src/examples/CMakeLists.txt
rename to examples/CMakeLists.txt
diff --git a/src/examples/polyline_demo.cpp b/examples/polyline_demo.cpp
similarity index 98%
rename from src/examples/polyline_demo.cpp
rename to examples/polyline_demo.cpp
index c9399bd57ba81d736605c8a6b0f0fa5fead0668b..634f8eb915d4859916dc88399417c147f88ba32d 100644
--- a/src/examples/polyline_demo.cpp
+++ b/examples/polyline_demo.cpp
@@ -27,8 +27,8 @@
*/
//LaserScanUtils includes
-#include "line_finder_iterative.h"
-#include "laser_scan.h"
+#include "laser_scan_utils/line_finder_iterative.h"
+#include "laser_scan_utils/laser_scan.h"
//std includes
#include <ctime>
diff --git a/src/corner_falko_2d.h b/include/laser_scan_utils/corner_falko_2d.h
similarity index 100%
rename from src/corner_falko_2d.h
rename to include/laser_scan_utils/corner_falko_2d.h
diff --git a/src/corner_finder.h b/include/laser_scan_utils/corner_finder.h
similarity index 100%
rename from src/corner_finder.h
rename to include/laser_scan_utils/corner_finder.h
diff --git a/src/corner_finder_inscribed_angle.h b/include/laser_scan_utils/corner_finder_inscribed_angle.h
similarity index 100%
rename from src/corner_finder_inscribed_angle.h
rename to include/laser_scan_utils/corner_finder_inscribed_angle.h
diff --git a/src/corner_point.h b/include/laser_scan_utils/corner_point.h
similarity index 100%
rename from src/corner_point.h
rename to include/laser_scan_utils/corner_point.h
diff --git a/src/grid_2d.h b/include/laser_scan_utils/grid_2d.h
similarity index 100%
rename from src/grid_2d.h
rename to include/laser_scan_utils/grid_2d.h
diff --git a/src/grid_cluster.h b/include/laser_scan_utils/grid_cluster.h
similarity index 100%
rename from src/grid_cluster.h
rename to include/laser_scan_utils/grid_cluster.h
diff --git a/src/icp.h b/include/laser_scan_utils/icp.h
similarity index 100%
rename from src/icp.h
rename to include/laser_scan_utils/icp.h
diff --git a/src/laser_scan.h b/include/laser_scan_utils/laser_scan.h
similarity index 100%
rename from src/laser_scan.h
rename to include/laser_scan_utils/laser_scan.h
diff --git a/src/laser_scan_utils.h b/include/laser_scan_utils/laser_scan_utils.h
similarity index 100%
rename from src/laser_scan_utils.h
rename to include/laser_scan_utils/laser_scan_utils.h
diff --git a/src/laser_scan_with_params.h b/include/laser_scan_utils/laser_scan_with_params.h
similarity index 100%
rename from src/laser_scan_with_params.h
rename to include/laser_scan_utils/laser_scan_with_params.h
diff --git a/src/line_finder.h b/include/laser_scan_utils/line_finder.h
similarity index 100%
rename from src/line_finder.h
rename to include/laser_scan_utils/line_finder.h
diff --git a/src/line_finder_hough.h b/include/laser_scan_utils/line_finder_hough.h
similarity index 100%
rename from src/line_finder_hough.h
rename to include/laser_scan_utils/line_finder_hough.h
diff --git a/src/line_finder_iterative.h b/include/laser_scan_utils/line_finder_iterative.h
similarity index 100%
rename from src/line_finder_iterative.h
rename to include/laser_scan_utils/line_finder_iterative.h
diff --git a/src/line_finder_jump_fit.h b/include/laser_scan_utils/line_finder_jump_fit.h
similarity index 100%
rename from src/line_finder_jump_fit.h
rename to include/laser_scan_utils/line_finder_jump_fit.h
diff --git a/src/line_segment.h b/include/laser_scan_utils/line_segment.h
similarity index 100%
rename from src/line_segment.h
rename to include/laser_scan_utils/line_segment.h
diff --git a/src/loop_closure_base.h b/include/laser_scan_utils/loop_closure_base.h
similarity index 100%
rename from src/loop_closure_base.h
rename to include/laser_scan_utils/loop_closure_base.h
diff --git a/src/loop_closure_falko.h b/include/laser_scan_utils/loop_closure_falko.h
similarity index 100%
rename from src/loop_closure_falko.h
rename to include/laser_scan_utils/loop_closure_falko.h
diff --git a/src/match_loop_closure_scene.h b/include/laser_scan_utils/match_loop_closure_scene.h
similarity index 100%
rename from src/match_loop_closure_scene.h
rename to include/laser_scan_utils/match_loop_closure_scene.h
diff --git a/src/point_set.h b/include/laser_scan_utils/point_set.h
similarity index 100%
rename from src/point_set.h
rename to include/laser_scan_utils/point_set.h
diff --git a/src/polyline.h b/include/laser_scan_utils/polyline.h
similarity index 100%
rename from src/polyline.h
rename to include/laser_scan_utils/polyline.h
diff --git a/src/scan_segment.h b/include/laser_scan_utils/scan_segment.h
similarity index 100%
rename from src/scan_segment.h
rename to include/laser_scan_utils/scan_segment.h
diff --git a/src/scene_base.h b/include/laser_scan_utils/scene_base.h
similarity index 100%
rename from src/scene_base.h
rename to include/laser_scan_utils/scene_base.h
diff --git a/src/scene_falko.h b/include/laser_scan_utils/scene_falko.h
similarity index 100%
rename from src/scene_falko.h
rename to include/laser_scan_utils/scene_falko.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index 3aa65ec369a4c8ec2031fd8d539624b038a4fb64..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,120 +0,0 @@
-
-MESSAGE("Starting laser_scan_utils CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#include directories
-INCLUDE_DIRECTORIES(. /usr/local/include)
-INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
-IF(csm_FOUND)
- INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR})
-ENDIF(csm_FOUND)
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-IF(falkolib_FOUND)
- MESSAGE(STATUS "FALKO lib found:")
- MESSAGE(STATUS " falkolib_INCLUDE_DIRS ${falkolib_INCLUDE_DIRS}")
- MESSAGE(STATUS " falkolib_LIBRARY_DIRS ${falkolib_LIBRARY_DIRS}")
- MESSAGE(STATUS " falkolib_LIBRARIES ${falkolib_LIBRARIES}")
- add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
- INCLUDE_DIRECTORIES(${falkolib_INCLUDE_DIRS})
- LINK_DIRECTORIES(${falkolib_LIBRARY_DIRS})
-ENDIF(falkolib_FOUND)
-
-#headers
-SET(HDRS_BASE
- laser_scan_utils.h)
-
-SET(HDRS
- corner_finder.h
- corner_finder_inscribed_angle.h
- corner_point.h
- grid_2d.h
- grid_cluster.h
- laser_scan.h
- laser_scan_with_params.h
- line_finder.h
- line_finder_hough.h
- line_finder_iterative.h
- line_finder_jump_fit.h
- line_segment.h
- point_set.h
- polyline.h
- scan_segment.h
- loop_closure_base.h
- scene_base.h
-
- )
- IF(csm_FOUND)
- SET(HDRS ${HDRS}
- icp.h)
- ENDIF(csm_FOUND)
-
- IF(falkolib_FOUND)
- SET(HDRS ${HDRS}
- corner_falko_2d.h
- loop_closure_falko.h
- scene_falko.h
- match_loop_closure_scene.h)
- ENDIF(falkolib_FOUND)
-
-#sources
-SET(SRCS
- corner_finder.cpp
- corner_finder_inscribed_angle.cpp
- corner_point.cpp
- grid_2d.cpp
- grid_cluster.cpp
- laser_scan.cpp
- laser_scan_with_params.cpp
- line_finder.cpp
- line_finder_hough.cpp
- line_finder_iterative.cpp
- line_finder_jump_fit.cpp
- line_segment.cpp
- point_set.cpp
- polyline.cpp
- scan_segment.cpp
- )
- IF(csm_FOUND)
- SET(SRCS ${SRCS}
- icp.cpp)
- ENDIF(csm_FOUND)
-
- IF(falkolib_FOUND)
- SET(SRCS ${SRCS}
- corner_falko_2d.cpp)
- ENDIF(falkolib_FOUND)
-
-
-
-# create the shared library
-ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
-IF(csm_FOUND)
- target_link_libraries(${PROJECT_NAME} ${csm_LIBRARY})
-ENDIF(csm_FOUND)
-
-IF(falkolib_FOUND)
- target_link_libraries(${PROJECT_NAME} ${falkolib_LIBRARY})
-ENDIF(falkolib_FOUND)
-
-# Examples
-IF(BUILD_DEMOS)
- MESSAGE("Building examples.")
- ADD_SUBDIRECTORY(examples)
-ENDIF(BUILD_DEMOS)
-
-#install library
-INSTALL(TARGETS ${PROJECT_NAME}
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib
- ARCHIVE DESTINATION lib)
-
-#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/laser_scan_utils)
-INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms/laser_scan_utils)
-
-#install Find*.cmake
-INSTALL(FILES ../Findlaser_scan_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp
index 9928d30bfcaf1b58d33aa210a64708bd9a437451..3b98f1eb63e2c8347ef0ebbb89f59768c8a4d48e 100644
--- a/src/corner_falko_2d.cpp
+++ b/src/corner_falko_2d.cpp
@@ -26,7 +26,7 @@
* \author: spujol
*/
-#include "corner_falko_2d.h"
+#include "laser_scan_utils/corner_falko_2d.h"
namespace laserscanutils {
diff --git a/src/corner_finder.cpp b/src/corner_finder.cpp
index f973bb0ea525939658e177884947df2ba792be90..d4d5f6d8facdfe0927bfb0e7dbf4abb144adc67b 100644
--- a/src/corner_finder.cpp
+++ b/src/corner_finder.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "corner_finder.h"
+#include "laser_scan_utils/corner_finder.h"
namespace laserscanutils
{
diff --git a/src/corner_finder_inscribed_angle.cpp b/src/corner_finder_inscribed_angle.cpp
index d5fef2be4071102af2d901a9bd9ccd1ac6eedb1d..3783b8dd4181e0ef62b0a24c468e1585850e0991 100644
--- a/src/corner_finder_inscribed_angle.cpp
+++ b/src/corner_finder_inscribed_angle.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "corner_finder_inscribed_angle.h"
+#include "laser_scan_utils/corner_finder_inscribed_angle.h"
namespace laserscanutils
{
diff --git a/src/corner_point.cpp b/src/corner_point.cpp
index fa74cfd165d50a7ff7f0c9f857e3e86b2b88cf2e..d966f381f4edd320080bbdca2418ca63d15dc6ab 100644
--- a/src/corner_point.cpp
+++ b/src/corner_point.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "corner_point.h"
+#include "laser_scan_utils/corner_point.h"
//open namespace
namespace laserscanutils
diff --git a/src/deprecated/clustering.cpp b/src/deprecated/clustering.cpp
deleted file mode 100644
index 9083e3678704126e7e739586365a9fe5417dc580..0000000000000000000000000000000000000000
--- a/src/deprecated/clustering.cpp
+++ /dev/null
@@ -1,872 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/******* DEPRECATED *******/
-
-#include "clustering.h"
-
-#include <fstream> // std::ofstream
-
-
-
-void laserscanutils::LaserDetectionParams::print() const
-{
- std::cout << "--------------------------------------------" << std::endl
- << "-- Laser Detection Algorithm Parameters --" << std::endl
- << "--------------------------------------------" << std::endl
- << " * Maximum Distance Between Points: " << jump_thr << std::endl
- << " * Minimum Number of Points for a Cluster: " << min_num_cluster_points << std::endl
- << " * Maximum Number of Points for a Cluster: " << max_num_cluster_points << std::endl
- << " * Segmnet Window Size: " << this->segment_window_size << std::endl
- << " * K_sigmas to tolerate std_dev in line: " << this->k_sigmas << std::endl
- << " * theta max to concatenate segments: " << this->theta_max_parallel << std::endl;
-
-}
-
-
-
-
-Eigen::MatrixXs laserscanutils::loadScanMatrix()
-{
- std::vector<std::vector<float> > values;
- std::ifstream fin("/home/vvaquero/testOneScan2.txt");
- for (std::string line; std::getline(fin, line); )
- {
- std::replace(line.begin(), line.end(), ',', ' ');
- std::istringstream in(line);
- values.push_back(
- std::vector<float>(std::istream_iterator<float>(in),
- std::istream_iterator<float>()));
- }
-
- unsigned int rows = values.size();
- unsigned int columns = 0;
-
- for (unsigned int k = 0; k < rows; k++)
- {
- if (values[k].size() > columns)
- columns = values[k].size();
- }
-
- std::cout << "Matrix Size = " << rows << "x" << columns <<std::endl;
- Eigen::MatrixXs result(rows,columns);
-
- for (unsigned int i = 0; i < rows; i++)
- for (unsigned int j = 0; j < columns; j++)
- {
- result(i,j) = values[i][j];
- //std::cout << "(" << result(i,j) << ") " ;
- }
- return result;
-}
-
-
-void laserscanutils::exportStaticData2Matlab(const char * _file_name,
- const LaserDetectionParams & _alg_params)
-{
- std::ofstream fileMatlab;
- fileMatlab.open (_file_name, std::ofstream::out); // | std::ofstream::app);
-
- // FILE HEADER
- fileMatlab << "% ***** Autogenerated File - vvg ***** \n\n";
- fileMatlab << " clc,\n clear all,\n close all \n\n\n";
-
- // Algorithm Params
- fileMatlab << "% Algorithm Params \n";
- fileMatlab << "alg_detection_params = struct("
- " 'jump_thr', " << _alg_params.jump_thr << ","
- " 'k_sigmas', " << _alg_params.k_sigmas << ","
- " 'segment_window_size', " << _alg_params.segment_window_size << ","
- " 'theta_max_parallel', " << _alg_params.theta_max_parallel << ","
- "'min_num_cluster_points', " << _alg_params.min_num_cluster_points << ","
- "'max_num_cluster_points', " << _alg_params.max_num_cluster_points << "); \n\n";
-
- // Algorithm Stats skeleton
- fileMatlab << "% Algorithm Stats skeleton \n" ;
- fileMatlab << "alg_detection_stats = struct([]); \n\n";
-
- // Global Odometry skeleton
- fileMatlab << "% Global Odometry skeleton\n" ;
- fileMatlab << "global_odom = struct([]); \n\n";
-
-
- // Object List skeleton
- fileMatlab << "% Object List skeleton \n" ;
- fileMatlab << "object_list = struct('lineList',[]);\n";
- fileMatlab << "object_list.lines = cell([1]);\n\n";
- //fileMatlab << "object_list.lineList = struct([]);\n\n";
-
-
- // Scan Matrix Homogeneous Coordinates
- fileMatlab << "% Scan Matrix In Homogeneous Coordinates \n" ;
- fileMatlab << "scanMatrixHomo = struct([]);\n\n";
-
- // Rotation Matrix
- fileMatlab << "% Rotation Matrix \n" ;
- fileMatlab << "alfa = pi/2; \n";
- fileMatlab << "rotateM = [cos(alfa), -sin(alfa); sin(alfa), -cos(alfa)]; \n";
-
-
- fileMatlab.close();
-
-}
-
-void laserscanutils::exportData2Matlab(std::string & _file_name,
- const Eigen::MatrixXs & _points,
- LaserDetectionStats & _stats,
- unsigned int _scanNumber)
-{
- std::ofstream fileMatlab;
-
- std::string new_name;
- int division = _scanNumber/100;
-// if (division == 0)
-// {
- int cociente = _scanNumber/100;
- new_name = _file_name + std::to_string(division);
- new_name += ".m";
- fileMatlab.open (new_name, std::ofstream::out | std::ofstream::app);
-// }
-
- // FILE HEADER
- fileMatlab << "% ***** Autogenerated File - vvg ***** \n\n";
- //fileMatlab << " clc,\n clear all,\n close all \n\n\n";
-
-
- // Algorithm Status Variables
- fileMatlab << "% Algorithm Status Variables \n";
- fileMatlab << "alg_detection_stats(" << _scanNumber << ").filteredTotalPoints = " << _stats.filteredTotalPoints << "; \n";
- fileMatlab << "alg_detection_stats(" << _scanNumber << ").numberOfClusters = " << _stats.numberOfClusters << "; \n";
- fileMatlab << "alg_detection_stats(" << _scanNumber << ").numberOfValidClusters = " << _stats.numberOfValidClusters << "; \n";
- fileMatlab << "alg_detection_stats(" << _scanNumber << ").scanTotalPoints = " << _stats.scanTotalPoints << "; \n";
- fileMatlab << "alg_detection_stats(" << _scanNumber << ").cluster_indxs = [";
- for (unsigned int cluster = 0; cluster < _stats.cluster_indxs_.size(); cluster++)
- {
- std::pair<int,int> tempCluster = _stats.cluster_indxs_[cluster];
- fileMatlab << tempCluster.first << "; " << tempCluster.second << "; ";
- }
- fileMatlab << "]; \n";
-
- fileMatlab << "alg_detection_stats(" << _scanNumber << ").cluster_indxs = reshape( alg_detection_stats(" << _scanNumber <<
- ").cluster_indxs,[2,length(alg_detection_stats(" << _scanNumber << ").cluster_indxs)/2]); \n\n";
-
-
- // Scanner Points in Homogeneous coordinates
- fileMatlab << "% Writing Scanner Points in Homogeneous coordinates \n";
- fileMatlab << "scanMatrixHomo(" << _scanNumber << ").scan = [";
- for (unsigned int row = 0; row < 3; row++)
- {
- for (unsigned int col = 0; col < _stats.filteredTotalPoints; col++)
- {
- fileMatlab << _points(row,col) << " ";
- }
- fileMatlab << ";\n";
- }
- fileMatlab << "]; \n\n";
-
-
-
-// // Global Odometry
- fileMatlab << "% Global Odometry \n";
- fileMatlab << "global_odom(" << _scanNumber << ").position = [" << _stats.odom_pos_[0] << ", " <<
- _stats.odom_pos_[1] << ", " <<
- _stats.odom_pos_[2] << "]; \n";
- fileMatlab << "global_odom(" << _scanNumber << ").orientation = [" << _stats.odom_ori_[0] << ", " <<
- _stats.odom_ori_[1] << ", " <<
- _stats.odom_ori_[2] << ", " <<
- _stats.odom_ori_[3] << "]; \n";
-
-
-// // Rotating Points Matrix
-// fileMatlab << "% Rotating Points Matrix \n";
-// fileMatlab << "scanMatrixHomo(" << _scanNumber << ").scanRot = scanMatrixHomo(1:2, :); \n" ;
-// fileMatlab << "scanMatrixHomo(" << _scanNumber << ").scanRot = rotateM*scanMatrixHomo(" << _scanNumber << ").scanRot; \n\n";
-
-
-// // Drawing Rotated Scan
-// fileMatlab << "% Drawing Rotated Scan \n";
-// fileMatlab << "scatter(scanMatrixRot(1,:), scanMatrixRot(2,:),30,'filled'); axis equal; grid on; \n\n";
-
-
-
- // Printing Full Objects
- fileMatlab << "% Printing Full Objects \n";
-
- fileMatlab << "% Object n\n";
- std::list<laserscanutils::Object>::iterator object_it = _stats.object_list_.begin();
- for (; object_it != _stats.object_list_.end(); object_it++)
- {
- laserscanutils::Object temp_object = *object_it;
-
- // Object
- fileMatlab << "object_list(" << _scanNumber << ").numberOfPoints(" << std::distance(_stats.object_list_.begin(),object_it)+1 << ") = "
- << temp_object.num_points_<< ";\n";
-
- fileMatlab << "object_list(" << _scanNumber << ").first(" << std::distance(_stats.object_list_.begin(),object_it)+1 << ") = "
- << temp_object.first_ << ";\n";
-
- fileMatlab << "object_list(" << _scanNumber << ").last(" << std::distance(_stats.object_list_.begin(),object_it)+1 << ") = "
- << temp_object.last_ << ";\n";
-
- fileMatlab << "object_list(" << _scanNumber << ").tracked(" << std::distance(_stats.object_list_.begin(),object_it)+1 << ") = "
- << temp_object.associated_ << ";\n";
-
-// fileMatlab << "object_list(" << _scanNumber << ").lineList(" << std::distance(_stats.object_list_.begin(),object_it)+1 << ") = [";
- fileMatlab << "object_list(" << _scanNumber << ").lineList = [object_list(" << _scanNumber << ").lineList; ";
- for (std::list<laserscanutils::Line>::iterator line_it = temp_object.line_list_.begin(); line_it != temp_object.line_list_.end(); line_it++)
- {
- laserscanutils::Line temp_line = *line_it;
-
- //std::cout << temp_line.point_first_.transpose() << "," << temp_line.point_last_.transpose() << ", " << temp_line.error_ << std::endl;
- fileMatlab << temp_line.point_first_(0) << ", " << temp_line.point_first_(1) << ", " << temp_line.point_first_(2) << ", " <<
- temp_line.point_last_(0) << ", " << temp_line.point_last_(1) << ", " << temp_line.point_last_(2) << ", " <<
- temp_line.error_ << ", " << temp_line.vector_(0) << ", " << temp_line.vector_(1) << ", " << temp_line.vector_(2) <<";\n";
- }
- fileMatlab << "];\n";
-
- fileMatlab << "object_list(" << _scanNumber << ").lines{" << std::distance(_stats.object_list_.begin(),object_it)+1 << ",1} = [";
-
- for (std::list<laserscanutils::Line>::iterator line_it = temp_object.line_list_.begin(); line_it != temp_object.line_list_.end(); line_it++)
- {
- laserscanutils::Line temp_line = *line_it;
-
- //std::cout << temp_line.point_first_.transpose() << "," << temp_line.point_last_.transpose() << ", " << temp_line.error_ << std::endl;
- fileMatlab << temp_line.point_first_(0) << ", " << temp_line.point_first_(1) << ", " << temp_line.point_first_(2) << ", " <<
- temp_line.point_last_(0) << ", " << temp_line.point_last_(1) << ", " << temp_line.point_last_(2) << ", " <<
- temp_line.error_ << ", " << temp_line.vector_(0) << ", " << temp_line.vector_(1) << ", " << temp_line.vector_(2) <<";\n";
- }
- fileMatlab << "];\n";
-
- }
-
-
- fileMatlab.close();
-}
-
-
-
-
-
-
-
-
-
-
-//****************************************************
-//********** preFilterScan
-//****************************************************
-void laserscanutils::preFilterScan(const laserscanutils::ScanParams & _params,
- const std::vector<float> & _ranges,
- Eigen::MatrixXs & _points,
- laserscanutils::LaserDetectionStats &_stats)
-{
- std::cout << "[PreFiltering SCAN... ";
-
-// std::cout << "Points Matrix = " << _points.rows() << "x" << _points.cols() << std::endl;
-
-// std::cout << "Matrix:" << std::endl << _points << std::endl;
-
- unsigned int matPosition = 0;
- float act_range = -1;
- float act_theta = 0.0;
-
- for(unsigned int i=0; i<_ranges.size(); ++i)
- {
- act_range = _ranges[i];
-
- if ( (act_range >=_params.range_min_) && (act_range <= _params.range_max_) && !(std::isnan(act_range)) && !(std::isinf(act_range)) )
- {
- act_theta = _params.angle_min_+ _params.angle_step_*(i);
-
- _points.col(matPosition) << act_range*cos(act_theta), act_range*sin(act_theta), 1;
-
- matPosition++;
- }
- else
- {
- //std::cout << "Invalid Point: " << scan.ranges[i] << " - index: " << i << std::endl;
- }
-
-
- } // END FOR scan ranges
-
- // Filling stats
- _stats.scanTotalPoints = _ranges.size();
- _stats.filteredTotalPoints = matPosition;
-
- //std::cout << "..DONE] " << std::endl;
-}
-
-
-
-
-
-
-////****************************************************
-////********** findClusters
-////****************************************************
-//void laserscanutils::findClusters(const laserscanutils::LaserObjectDetAlgParams &_alg_params,
-// const Eigen::MatrixXs & _points,
-// LaserDetectionStats & _stats)
-//{
-
-// _stats.cluster_indxs_.clear();
-// _stats.object_list_.clear(); // Assure the object list for the scan is empty.
-
-// std::pair<int,int> seg; // saves the first and last point for each segment
-// laserscanutils::Object temp_object; // objects detected initialization.
-
-// float points_distance = 0.0;
-// int count_num_cluster = 0;
-// int count_num_valid_cluster = 0;
-
-
-// // Initial Segment
-//// count_num_cluster = 1;
-// seg.first = 0;
-// float sq_threshold = pow(_alg_params.jump_thr,2.0);
-
-// //for (unsigned int col = 1; col < _points.cols(); col++)
-// for (unsigned int col = 1; col < _stats.filteredTotalPoints; col++)
-// {
-// points_distance = ( pow( ( _points(0,col) -_points(0,col-1) ), 2.0 ) + pow( ( _points(1,col) - _points(1,col-1) ), 2.0) );
-
-// if ( points_distance <= sq_threshold) // same segment points
-// {
-// // Same segment -> TODO counter?
-// }
-
-// else // if ( col != _points.cols() ) // new segment
-// {
-// //std::cout << "P " << pos << " - A - (" << pointA.x << "-" << pointA.y << ") - B - (" << pointB.x << "-" << pointB.y << ") -> distance: " << points_distance << " More Than th" << std:endl;
-
-// seg.second = col-1; // end of the previous segment
-
-// // check length of the cluster
-// if ( ( (seg.second - seg.first)+1 >= _alg_params.min_num_cluster_points ) && ( (seg.second - seg.first)+1 <= _alg_params.max_num_cluster_points) )
-// {
-// _stats.cluster_indxs_.push_back(seg);
-
-// // fulfill and add object
-// temp_object.first_ = seg.first;
-// temp_object.last_ = seg.second;
-// temp_object.num_points_ = seg.second - seg.first +1;
-// _stats.object_list_.push_back(temp_object);
-
-// count_num_valid_cluster++; // count cluster as valid
-// //std::cout << "CLUSTER IS VALID" << std::endl;
-// }
-// // TODO: else: save the invalid cluster in other vector of bad clusters to further analysis
-// count_num_cluster++; // count any cluster
-
-// seg.first = col;
-// }
-
-// }
-// // last point close last segment
-//// seg.second = _points.cols()-1;
-// seg.second = _stats.filteredTotalPoints-1;
-// if (seg.second - seg.first >= _alg_params.min_num_cluster_points && seg.second - seg.first <= _alg_params.max_num_cluster_points)
-// {
-// _stats.cluster_indxs_.push_back(seg);
-
-// // fulfill and add object
-// temp_object.first_ = seg.first;
-// temp_object.last_ = seg.second;
-// temp_object.num_points_ = seg.second - seg.first +1;
-// _stats.object_list_.push_back(temp_object);
-
-// count_num_valid_cluster++; // count cluster as valid
-// //std::cout << "CLUSTER IS VALID" << std::endl;
-// }
-// count_num_cluster++; // count any cluster
-
-
-// // Stats
-// _stats.numberOfClusters = count_num_cluster;
-// _stats.numberOfValidClusters = count_num_valid_cluster;
-
-// std::cout << "numberOfValidClusters = " << count_num_valid_cluster << std::endl;
-// std::cout << "Number of OBJECTS prepared = " << _stats.object_list_.size() << std::endl;
-
-
-
-//}
-
-
-
-
-
-//void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _params,
-// const laserscanutils::LaserDetectionParams & _alg_params,
-// const Eigen::MatrixXs & _cluster,
-// const unsigned int initIndex,
-// std::list<laserscanutils::Line> & _line_list)
-//{
-// laserscanutils::Line line;
-// std::list<Line>::iterator line_it1, line_it2;
-// ScalarT theta;
-
-// // cleaning line_list
-// _line_list.clear();
-
-// //init random generator
-// std::default_random_engine generator(1);
-// std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
-
-// //std::cout << "TEST!!!!: = _cluster.cols() = " << _cluster.cols() << std::endl;
-
-// unsigned int ii = _alg_params.segment_window_size-1;
-// bool last = false;
-// //while ( (ii<_cluster.cols()) && (last != true) )
-// while (last != true)
-// {
-// if (ii >= _cluster.cols())
-// {
-// //std::cout << " LAST " << std::endl;
-// ii = _cluster.cols()-1;
-// last = true;
-// }
-// unsigned int i_from = ii - _alg_params.segment_window_size + 1;
-
-// //std::cout << "Fitting Line: from " << i_from << " to " << ii << "...";
-// fitLine(_cluster.block(0,i_from, 3, _alg_params.segment_window_size), line);
-
-// //if error below stdev, add line to result set
-// if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
-// {
-// //std::cout << "=> YES!" << std::endl;
-// line.first_ = i_from;
-// line.last_ = ii;
-// line.point_first_ = _cluster.col(line.first_);
-// line.point_last_ = _cluster.col(line.last_);
-// line.np_ = line.last_ - line.first_ + 1;
-
-// _line_list.push_back(line);
-// }
-// else
-// {
-// //std::cout << " NO! - Error = " << line.error_ << " > " << _params.range_std_dev_*_alg_params.k_sigmas << std::endl;
-// //std::cout << " List of points: " ;
-//// for (unsigned int po = i_from; po < ii; po++)
-//// {
-//// std::cout << "[" << _cluster(0,po) << ", " << _cluster(1,po) << "], ";
-//// }
-//// std::cout << std::endl;
-// }
-// ii = ii+rand_window_overlapping(generator);
-// }
-
-// std::cout << "Lines fitted: " << _line_list.size();
-
-
-// // concatenating lines
-// line_it1 = _line_list.begin();
-
-// while ( line_it1 != _line_list.end() )
-// {
-// line_it2 = line_it1;
-// line_it2 ++;
-// while (line_it2 != _line_list.end())
-// {
-// //compute angle between lines 1 and 2
-// theta = laserscanutils::angleBetweenLines(*line_it1, *line_it2);
-
-//// std::cout << "LineIt1 = " << std::distance(_line_list.begin(),line_it1) << " vs LineIt2 = " << std::distance(_line_list.begin(), line_it2) << std::endl;
-//// std::cout << "cos_theta: " << cos(theta) << " -> theta: " << theta << std::endl;
-
-// //Check angle threshold
-// if ( theta < _alg_params.theta_max_parallel ) //fabs(theta) not required since theta>0
-// {
-// //compute a new line with all involved points
-// Line new_line;
-// fitLine(_cluster.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line);
-//// std::cout << " Checking Concatenation error of : " << line_it1->first_ << " vs " << line_it2->last_ << std::endl;
-//// std::cout << " New Line error : " << new_line.error_ << " <=? " << _params.range_std_dev_*_alg_params.k_sigmas << std::endl;
-
-
-// //check if error below a threshold to effectively concatenate
-// if ( new_line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
-// {
-// //std::cout << " => lines concatenated";
-
-// //update line1 as the concatenation of l1 and l2
-// new_line.first_ = line_it1->first_;
-// new_line.last_ = line_it2->last_;
-// new_line.point_first_ = line_it1->point_first_;
-// new_line.point_last_ = line_it2->point_last_;
-// new_line.np_ = new_line.last_ - new_line.first_ + 1;
-// *line_it1 = new_line;
-
-// //remove l2 from the list
-// line_it2 = _line_list.erase(line_it2);
-
-// //std::cout << " - line resultant: from "<< new_line.first_ << " to " << new_line.last_ << std::endl;
-// //<< new_line.vector_.transpose() << " ERROR: " << new_line.error_ << std::endl << std::endl;
-
-// //in that case do not update iterators to allow concatenation of the new l1 with the next line in the list, if any
-// // update_iterators = false;
-// }
-// else
-// {
-// line_it2++;
-// //std::cout << " <-- no concatenation...error too big" << std::endl;
-// }
-// }
-// else
-// {
-// line_it2++;
-// //std::cout << " <-- no concatenation... theta too big " << std::endl;
-// }
-
-// } // end while it 2
-
-// // Eliminating intermediate lines
-// std::list<Line>::iterator temp_line_it = line_it1;
-// line_it1++;
-// while ( (temp_line_it->last_ >= line_it1->first_) && (line_it1 != _line_list.end()))
-// {
-// //std::cout << " linea intermedia!" << std::endl;
-// line_it1 = _line_list.erase(line_it1);
-//// line_it1++;
-// }
-
-
-// } // end while it 1
-
-// // globalizing first and last point (in terms of the whole scan)
-// for (auto list_it1 = _line_list.begin(); list_it1 != _line_list.end(); list_it1++)
-// {
-// //std::cout << " INSIDE FIND LINES #." << std::distance(_line_list.begin(),list_it1) << std::endl;
-// list_it1->first_ += initIndex;
-// list_it1->last_ += initIndex;
-// //list_it1->print();
-// //std::cout << std::endl << std::endl;
-// }
-
-// std::cout << " --> Lines After Concatenation: " << _line_list.size() << std::endl << std::endl;
-
-
-//}
-
-
-
-
-
-
-
-
-
-
-//unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::ScanParams & _params,
-// const ExtractCornerParams & _alg_params,
-// const std::list<laserscanutils::Line> & _line_list,
-// std::list<laserscanutils::Corner> & _corner_list)
-//{
-// //local variables
-// ScalarT theta;
-// Corner corner;
-// std::list<Line>::const_iterator line_it1, line_it2, line_it3, last_line;
-// std::list<laserscanutils::Corner>::iterator corner_it1, corner_it2;
-// ScalarT max_distance_sq(_alg_params.line_params_.jump_dist_ut_*_alg_params.line_params_.jump_dist_ut_); //init max distance as the squared value to avoid sqroot on comparisons
-
-// //std::cout << "CORNER DETECTION FROM LINES: " << std::endl;
-// // _alg_params.print();
-
-// line_it1 = _line_list.begin();
-
-// if (_line_list.size() <2)
-// {
-// //std::cout << "DEBUG: --> NOT ENOUGH LINES IN CLUSTER TO EXTRACT CORNERS" << std::endl;
-// return 0;
-// }
-
-// while ( line_it1 != _line_list.end()--)
-// {
-// line_it2 = line_it1;
-// line_it2 ++;
-
-
-// // std::cout << std::endl << "line 1: last idx: " << (int)line_it1->last_ << std::endl;
-// // std::cout << "line 2: first idx: " << (int)line_it2->first_ << std::endl;
-// // std::cout << "substraction " << static_cast<int>(line_it2->first_) - (int)line_it1->last_ << std::endl;
-
-// // std::cout << "line 1: point first: " << line_it1->point_first_.transpose() << std::endl;
-// // std::cout << "line 1: point last: " << line_it1->point_last_.transpose() << std::endl;
-// // std::cout << "line 2: point first: " << line_it2->point_first_.transpose() << std::endl;
-// // std::cout << "line 2: point last: " << line_it2->point_last_.transpose() << std::endl;
-
-// //compute angle between lines 1 and 2
-// theta = angleBetweenLines(*line_it1, *line_it2);
-// std::cout << "Angle Between Lines #" << std::distance(_line_list.begin(),line_it1) << " - "
-// << std::distance(_line_list.begin(),line_it2) << " = " << theta;
-
-// //Check angle threshold
-// if ( theta > _alg_params.theta_min_ )
-// {
-// std::cout << " ... Corner! : " << std::endl;
-// // Find the corner as the cross product between lines (intersection of lines in homog)
-// corner.pt_ = (line_it1->vector_).cross(line_it2->vector_);
-
-// // normalize corner point to set last component to 1
-// corner.pt_ = corner.pt_ / corner.pt_(2);
-
-// // Check if the corner is close to both lines ends. TODO: Move this check before computing intersection
-// if ( (line_it1->point_last_ - corner.pt_).head(2).squaredNorm() < max_distance_sq
-// &&
-// (line_it2->point_first_ - corner.pt_).head(2).squaredNorm() < max_distance_sq )
-// {
-// std::cout << " Corners is close enough: " << std::endl;
-
-// //vector from corner to first point of l1
-// Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
-
-// //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
-// corner.orientation_ = atan2(v1(1),v1(0));
-
-// //Compute corner aperture with line_it1->first, corner and line_it2->last
-// corner.aperture_ = cornerAperture(line_it1->point_first_, corner.pt_, line_it2->point_last_);
-
-// //set other descriptors
-// corner.line_1_ = *line_it1;
-// corner.line_2_ = *line_it2;
-
-// //add the corner to the list
-// _corner_list.push_back(corner);
-
-// // debug: print
-// corner.print();
-// }
-// }
-// else
-// //std::cout << " ... No Corner! : " << std::endl;
-
-// line_it2 ++;
-// line_it1 ++;
-
-
-// }
-// //std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
-
-//// // Erase duplicated corners
-//// corner_it1 = _corner_list.begin();
-//// while ( corner_it1 != _corner_list.end() )
-//// {
-//// corner_it2 = corner_it1;
-//// corner_it2 ++;
-//// while (corner_it2 != _corner_list.end())
-//// {
-//// // Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) !
-//// if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq )
-//// {
-//// // Keep the one with bigger product of number of points of each line
-//// (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
-//// corner_it2 = _corner_list.erase(corner_it2);
-//// }
-//// else
-//// corner_it2 ++;
-//// }
-//// corner_it1 ++;
-//// }
-// //std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
-
-// // for(std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin(); corner_it1 != _corner_list.end(); corner_it1++)
-// // corner_it1->print();
-
-// return _corner_list.size();
-//}
-
-
-
-
-
-
-
-// NOT USED!!!!
-// VV - Converts ranges from a laserScan to homogeneous coordinates and save it to a matrix of same size than ranges size, returning index till valid values.
-unsigned int laserscanutils::scanProcess(const ScanParams & _params,
- const std::vector<float> & _ranges,
- const float _jump_threshold,
- Eigen::MatrixXs & _points,
- std::vector<unsigned int> & _jumps_id_)
-{
-
- _jumps_id_.clear();
-
- unsigned int matPosition = 0;
- float prev_range = -1;
- float prev_theta = 0;
- float act_range = -1;
- float act_theta = 0.0;
-
- float distance = 0;
-
- for(unsigned int i=0; i<_ranges.size(); ++i)
- {
- act_range = _ranges[i];
- if ( (act_range >=_params.range_min_) && (act_range <= _params.range_max_) && !(std::isnan(act_range)) && !(std::isinf(act_range)) )
- {
- act_theta = _params.angle_min_+ _params.angle_step_*(i);
-
- _points.col(matPosition) << act_range*cos(act_theta), act_range*sin(act_theta), 1; //row0-> x coordinate, row1->y coordinate, row2->1;
- matPosition++;
-
- // Calculating end of clusters
- if (prev_range != -1)
- {
- // polar distances = r2² + r1² - 2*r1*r2*cos(theta2-theta1)
- distance = act_range*act_range + prev_range*prev_range - (2*prev_range*act_range*(cos(prev_theta-act_theta)));
-
- if (distance > _jump_threshold && 1==1) // TODO Add option of measure number of invalid point in between.
- _jumps_id_.push_back(matPosition);
- }
- prev_range = act_range; // updates
- prev_theta = act_theta;
-
- }
- else
- {
- //std::cout << "Invalid Point: " << _ranges[i] << " - index: " << i << std::endl;
- }
-
- }//END for
-
- return matPosition-1; // Remap the vector to the Matrix - Accounting for 0
-
-
-}
-
-
-
-
-
-
-
-// VV - test to check results in txt
-void laserscanutils::printMatrixAndClusters2File(std::string &_file_name,
- const unsigned int _num_points,
- Eigen::MatrixXs & _points,
- std::vector< std::pair<int,int> > _cluster_indxs_)
-{
- std::ofstream filePoints;
- std::string namePoints = _file_name.append("_points.txt");
- filePoints.open(namePoints , std::ofstream::out | std::ofstream::app);
-
- for (unsigned int row = 0; row < 3; row++)
- {
- for (unsigned int col = 0; col < _num_points; col++)
- {
- filePoints << _points(row,col) << ",";
- }
- filePoints << "\n";
- }
- filePoints.close();
-
-
- std::ofstream fileSegments;
- fileSegments.open (_file_name.append("_clusters.txt"), std::ofstream::out | std::ofstream::app);
- for (auto segnum_it = _cluster_indxs_.begin(); segnum_it != _cluster_indxs_.end(); segnum_it++)
- {
- std::pair<int,int> tem_inds = *segnum_it;
- fileSegments << tem_inds.first << ", " << tem_inds.second << "\n";
- }
- fileSegments.close();
-}
-
-
-
-// VV - test to check results in txt
-void laserscanutils::printLinesToFile(std::string &_file_name, std::list<laserscanutils::Line> & _line_list)
-{
- std::ofstream fileLines;
- fileLines.open(_file_name , std::ofstream::out | std::ofstream::app);
-
- for (std::list<laserscanutils::Line>::iterator line_it = _line_list.begin(); line_it != _line_list.end(); line_it++)
- {
- laserscanutils::Line temp_line = *line_it;
-
- std::cout << temp_line.point_first_.transpose() << "," << temp_line.point_last_.transpose() << ", " << temp_line.error_ << std::endl;
- fileLines << temp_line.point_first_(0) << ", " << temp_line.point_first_(1) << ", " << temp_line.point_first_(2) << ", " <<
- temp_line.point_last_(0) << ", " << temp_line.point_last_(1) << ", " << temp_line.point_last_(2) << ", " <<
- temp_line.error_ << "\n";
- }
- fileLines << "0, 0, 0, 0, 0, 0, 0\n";
- fileLines.close();
-
-}
-
-
-
-
-
-// VV - test to check results in txt
-void laserscanutils::printMatrix2FileJumpsVersion(char *_file_name, const unsigned int _num_points,
- Eigen::MatrixXs & _points,
- std::vector<unsigned int> & _jumps_id_)
-{
- std::ofstream filePoints;
- filePoints.open (std::strcat(_file_name, "_points.txt") , std::ofstream::out | std::ofstream::app);
-
- // TO PRINT IN ONE ROW
-// for (int numpoint = 0; numpoint < _num_points; numpoint++)
-// {
-// filePoints << _points.col(numpoint) << "\n";
-// }
-// filePoints.close();
-
- for (unsigned int row = 0; row < 3; row++)
- {
- for (unsigned int col = 0; col < _num_points; col++)
- {
- filePoints << _points(row,col) << ",";
- }
- filePoints << "\n";
- }
- filePoints.close();
-
-
- std::ofstream fileSegments;
- fileSegments.open (std::strcat(_file_name, "_points.txt"), std::ofstream::out | std::ofstream::app);
- for (unsigned int segnum = 1; segnum<_jumps_id_.size(); segnum++)
- {
- if (segnum == 1)
- fileSegments << "Segment Number: 0 - from 0 " << " to " << _jumps_id_[segnum] << "\n";
- else
- fileSegments << "Segment Number: "<< segnum << " - from " << _jumps_id_[segnum-1] << " to " << _jumps_id_[segnum] << "\n";
- }
- fileSegments.close();
- //std::cout << "PRINTING OUT " << std::endl;
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/deprecated/clustering.h b/src/deprecated/clustering.h
deleted file mode 100644
index d26446fa71349859bec770c8f6763ecb8bade05d..0000000000000000000000000000000000000000
--- a/src/deprecated/clustering.h
+++ /dev/null
@@ -1,213 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/******* DEPRECATED *******/
-
-#ifndef CLUSTERING_H
-#define CLUSTERING_H
-
-//laserscanutils
-#include "laser_scan_utils.h"
-#include "entities.h"
-#include "scan_basics.h"
-#include "corner_detector.h"
-
-
-#include <algorithm>
-#include <fstream>
-#include <iostream>
-#include <iterator>
-#include <sstream>
-#include <string>
-#include <vector>
-#include <eigen3/Eigen/Dense>
-//#include <algorithm> // std::min
-
-//#include <fstream> // File Printing for tests
-
-//#include "segment.h"
-
-
-
-
-namespace laserscanutils
-{
-
- /** \brief Set of tunning parameters
- *
- * Set of tunning parameters for corner extraction
- *
- */
- struct LaserDetectionParams
- {
- //members
- float jump_thr; //max distance between points
- float min_num_cluster_points; //minimum number of points for a cluster
- float max_num_cluster_points; //maximum number of points for a cluster
-
- unsigned int segment_window_size; //number of points to fit lines in the first pass
- ScalarT theta_max_parallel; //maximum theta between consecutive segments to fuse them in a single line.
- ScalarT k_sigmas;//How many std_dev are tolerated to count that a point is supporting a line
-
-
-
- void print() const;
- };
-
- struct LaserDetectionStats // TO BE REMOVED [TBR] -> Substitute = ObjectDetector.h: LaserScanProcessed
- {
- // Mid variables for the algorithm, storing information needed for the correct working
-
- unsigned int scanTotalPoints; //Total number of points in the raw scan
- unsigned int filteredTotalPoints; //Total number of valid points after pre filter
- unsigned int numberOfClusters; //Total number of clusters found in the scan
- unsigned int numberOfValidClusters; //Total number of valid clusters found in the scan
- std::vector< std::pair<int,int> > cluster_indxs_; // Contains initial and final columns (of the filtered homogeneous matrix) for clusters in scan.
- std::list<laserscanutils::Line> line_list_; //[TBR] Contains all the lines found in the scan (GLOBAL).
- std::list<laserscanutils::Object> object_list_; // Contains the different objects detected (clusters with associate information) in a scan.
-
- std::vector<float> odom_pos_; // [X,Y,Z] position of the global odometry "nav_msg: pose.pose.position"
- std::vector<float> odom_ori_; // [X,Y,Z,W] orientation of the global odometry. "nav_msg: pose.pose.orientation"
-
-
- };
-
-
-
- // TEST
- Eigen::MatrixXs loadScanMatrix();
-
-
- void exportStaticData2Matlab(const char * _file_name,
- const LaserDetectionParams & _alg_params);
-
- /** \brief Test Porpuses to create a Matlab script with all the variables.
- **/
- void exportData2Matlab(std::string &_file_name, const Eigen::MatrixXs & _points,
- LaserDetectionStats &_stats, unsigned int _scanNumber);
-
-
-
- /** \brief Filters the valid ranges on a scan and converts them to homogeneous coordinates.
- *
- * Valid ranges are filtered according to scan parameters.
- * Converts ranges from a laserScan to homogeneous coordinates and save it to a matrix of same size than ranges size, returning index till valid values.
- *
- * \param _params is the laser_scan_utils structure of the laser parameters.
- * \param _ranges is the raw scan measures. Vector of float ranges.
- * \param _points is the returned points from the conversion of ranges to homogeneous coordinates
- * \param _stats is the statistics structure where number of valid points is saved under _stats.filteredTotalPoints
- *
- **/
- void preFilterScan(const laserscanutils::ScanParams & _params,
- const std::vector<float> & _ranges,
- Eigen::MatrixXs & _points,
- laserscanutils::LaserDetectionStats &_stats);
-
-
-
-// /** \brief Find the clusters in the valid points from the scan.
-// *
-// * Check distance between valid points in homogeneous coordinates. If it is more than the threshold, considers it as a different cluster.
-// *
-// * \param _alg_params is the structure of algorithm params for the Laser Detection Algorithm.
-// * \param _points is the returned points from the conversion of ranges to homogeneous coordinates
-// * \param _stats is the statistics structure (TODO: What is used here? - Number of Clusters)
-// **/
-// void findClusters(const LaserDetectionParams & _alg_params,
-// const Eigen::MatrixXs & _points,
-// LaserDetectionStats & _stats);
-
-
-
-
-
-
-// void findLinesInCluster(const laserscanutils::ScanParams & _params,
-// const LaserDetectionParams &_alg_params,
-// const Eigen::MatrixXs &_cluster,
-// const unsigned int initIndex, //
-// std::list<laserscanutils::Line> &_line_list);
-
-
-
-
- unsigned int findCornersInClusterLines(const laserscanutils::ScanParams & _params,
- const ExtractCornerParams & _alg_params,
- const std::list<laserscanutils::Line> & _line_list,
- std::list<laserscanutils::Corner> & _corner_list);
-
-
-
-
-
-
- /** \brief Converts ranges from a laserScan to homogeneous coordinates. Check validity of the ranges and look cluster jumps.
- *
- * Converts ranges from a laserScan to homogeneous coordinates and save it to a matrix of same size than ranges size, returning index till valid values.
- * Valid ranges are filtered and at the same time cluster jumps are checked.
- * TODO (jumpThreshold)
- *
- * \param _params is the laser_scan_utils structure of the laser parameters.
- * \param _ranges is the raw scan measures. Vector os ranges.
- * \param _jump_threshold is the jump to diferenciate between clusters -> TODO in algorithm parameters
- * \param _points is the returned points from the conversion of ranges to homogeneous coordinates
- * \param _jumps_id_ is the vector containing in which column of the _points matrix exist a cluster jump.
- **/// NOT USED!!
- unsigned int scanProcess(const ScanParams & _params,
- const std::vector<float> & _ranges,
- const float _jump_threshold,
- Eigen::MatrixXs & _points,
- std::vector<unsigned int> & _jumps_id_);
-
-
-
-
- void printMatrixAndClusters2File(std::string &_file_name,
- const unsigned int _num_points,
- Eigen::MatrixXs & _points,
- std::vector<std::pair<int, int> > _cluster_indxs_);
-
-
-
- void printLinesToFile(std::string &_file_name,
- std::list<laserscanutils::Line> & _line_list);
-
-
-
- // VV - Old, needs jumps array.
- void printMatrix2FileJumpsVersion(char *_file_name, const unsigned int _num_points,
- Eigen::MatrixXs & _points,
- std::vector<unsigned int> & _jumps_id_);
-
-
-
-
-
- /** \brief Analyse clusters and extract the geometry primitives - line, corner...
- *
- * TODO
- *
- **/
-// unsigned int extractGeomPrimitives(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Corner> & _corner_list);
-
-}
-#endif// CLUSTERING_H
diff --git a/src/deprecated/corner_detector.cpp b/src/deprecated/corner_detector.cpp
deleted file mode 100644
index 540a7a170cba461be12a0004dd26b300f028b9e5..0000000000000000000000000000000000000000
--- a/src/deprecated/corner_detector.cpp
+++ /dev/null
@@ -1,579 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#include "corner_detector.h"
-
-void laserscanutils::ExtractCornerParams::print() const
-{
- line_params_.print();
- std::cout << "Extract Corner Algorithm Parameters : " << std::endl
- << " theta_min_: " << theta_min_ << std::endl
- << " max_distance_: " << max_distance_ << std::endl;
-}
-
-unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params,
- const laserscanutils::ExtractCornerParams & _alg_params,
- const std::list<laserscanutils::Line> & _line_list,
- std::list<laserscanutils::Corner> & _corner_list)
-{
- //local variables
- ScalarT cos_theta, theta;
- Corner corner;
- std::list<Line>::const_iterator line_it1, line_it2, last_line;
- std::list<laserscanutils::Corner>::iterator corner_it1, corner_it2;
- bool update_iterators;
- ScalarT max_distance_sq(_alg_params.max_distance_*_alg_params.max_distance_); //init max distance as the squared value to avoid sqroot on comparisons
-
- //STEP 1: find corners over the line list
- line_it1 = _line_list.begin();
- while ( line_it1 != _line_list.end() )
- {
- line_it2 = line_it1;
- line_it2 ++;
- while (line_it2 != _line_list.end())
- {
-
- // check if last of line1 and first of line2 are within the threshold
- if ( ( (int)line_it2->first_ - (int)line_it1->last_ ) <= (int)_alg_params.line_params_.concatenate_ii_ut_ )
- {
- //compute angle between lines 1 and 2
- theta = angleBetweenLines(*line_it1, *line_it2);
-
- //Check angle threshold
- if ( theta > _alg_params.theta_min_ )
- {
- // Find the corner as the cross product between lines (intersection)
- corner.pt_ = (line_it1->vector_).cross(line_it2->vector_);
-
- // normalize corner point to set last component to 1
- corner.pt_ = corner.pt_ / corner.pt_(2);
-
- // Check if the corner is close to both lines ends.
- if ( (line_it1->point_last_ - corner.pt_).head(2).squaredNorm() < max_distance_sq
- &&
- (line_it2->point_first_ - corner.pt_).head(2).squaredNorm() < max_distance_sq )
- {
-
- //vector from corner to first point of l1
- Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
- Eigen::Vector2s v2 = line_it2->point_last_.head(2) - corner.pt_.head(2);
-
- //compute corner orientation as the angle of the bisector w.r.t local frame
- corner.orientation_ = ( atan2(v1(1),v1(0)) + atan2(v2(1),v2(0)) ) / 2;
-
- //Compute corner aperture with line_it1->first, corner and line_it2->last
- corner.aperture_ = cornerAperture(line_it1->point_first_, corner.pt_, line_it2->point_last_);
-
- //set other descriptors
- corner.line_1_ = *line_it1;
- corner.line_2_ = *line_it2;
-
- //add the corner to the list
- _corner_list.push_back(corner);
- }
- }
- }
- else
- {
- break;
- }
-
- line_it2 ++;
- }
-
- line_it1 ++;
- }
- // std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
-
- // Erase duplicated corners
- corner_it1 = _corner_list.begin();
- while ( corner_it1 != _corner_list.end() )
- {
- corner_it2 = corner_it1;
- corner_it2 ++;
- while (corner_it2 != _corner_list.end())
- {
- // Check if two corners are close enough with similar orientation and aperture
- if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq &&
- abs( pi2pi( corner_it1->aperture_ - corner_it2->aperture_) ) < _alg_params.theta_min_ &&
- abs( pi2pi( corner_it1->orientation_ - corner_it2->orientation_) ) < _alg_params.theta_min_)
- {
- // Keep the one with bigger product of number of points of each line
- (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
- corner_it2 = _corner_list.erase(corner_it2);
- }
- else
- corner_it2 ++;
- }
- corner_it1 ++;
- }
- // std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
-
- return _corner_list.size();
-}
-
-unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params,
- const laserscanutils::ExtractCornerParams & _alg_params,
- const std::vector<float> & _ranges,
- std::list<laserscanutils::Corner> & _corner_list)
-{
- std::list<laserscanutils::Line> line_list;
- unsigned int nl;
-
- //extract lines and fill the list
- nl = extractLines(_params, _alg_params.line_params_, _ranges, line_list);
-// std::cout << "line_list.size(): " << line_list.size() << std::endl;
-
- //If more than one line, call to extract corners with the line list as input parameter
- if ( nl > 1)
- {
- return extractCorners(_params, _alg_params, line_list, _corner_list); //return detected corners and fill _corner_list
- }
- else
- return 0;
-
-}
-
-
-laserscanutils::ScalarT laserscanutils::cornerAperture(const Eigen::Vector3s & _p1,
- const Eigen::Vector3s & _c,
- const Eigen::Vector3s & _p2)
-{
- //TODO: use lines as inputs instead of _p1 and _p2 -> better estimation of the aperture value
-
- //buid lines from c to p1 and p2
- Eigen::Vector3s l1 = _c.cross(_p1);
- Eigen::Vector3s l2 = _c.cross(_p2);
-
- //normalize lines
- l1 = l1 / ( sqrt( l1(0)*l1(0)+l1(1)*l1(1) ) );
- l2 = l2 / ( sqrt( l2(0)*l2(0)+l2(1)*l2(1) ) );
-
- //compute angle
- ScalarT aux = l1(0)*l2(0) + l1(1)*l2(1);
- if (fabs(aux) > 1.) aux = aux / fabs(aux); //limit here to +-1 to avoid complex numbers due to numerical precision when acos(>1). Anyway, corners will be far from this situation
- ScalarT alpha = acos(aux); //alpha in [0,pi]
-
- //build matrix M and compute its determinant
- Eigen::Matrix3s M;
- M << _p1,_c, _p2;
- ScalarT detM = M.determinant();
-
- //if det<0, return the complementary angle
- if ( detM < 0 ) return ( 2*M_PI - alpha );
- else return alpha;
-}
-
-/*
-laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2)
-{
- ScalarT theta = acos( (line1.vector_.head(2)).dot(line2.vector_.head(2)) / ( line1.vector_.head(2).norm()*line2.vector_.head(2).norm() ) );
-
- //theta is in [0,PI]
- if (theta > M_PI/2)
- theta = M_PI - theta;//assures theta>0 AND either theta close to 0 or close to PI will always map to theta close to 0.
-
- return theta;
-}*/
-
-
-/*
-unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<ScalarT> & _ranges, std::list<laserscanutils::Line> & _line_list)
-{
- //local variables
- Eigen::MatrixXs points
- ScalarT cos_theta, theta;
- Line line;
- std::list<Line>::iterator line_it1, line_it2;
- std::list<unsigned int> jumps;
- bool update_iterators;
- unsigned int ii_from;
- ScalarT max_distance_sq(_alg_params.max_distance*_alg_params.max_distance); //init to squared value to avoid root() at comparisons
-
- //init random generator
-// std::default_random_engine generator(1);
-// std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
-
- //laserPolar2xy(_params, _ranges, _alg_params.max_distance, points, jumps);
- ranges2xy(_params, _ranges, _alg_params.max_distance, points, jumps);
-// std::cout << "jumps: " << jumps.size() << std::endl;
-// std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
-
-
- // STEP 1: find line segments running over the scan
- //for (unsigned int ii = _alg_params.segment_window_size-1; ii<points.cols(); ii++)//ii=ii+rand_window_overlapping(generator) )
- for (unsigned int ii=0, ii_from=0; ii<points.cols(); ii++)//ii=ii+rand_window_overlapping(generator) )
- {
- //unsigned int i_from = ii - _alg_params.segment_window_size + 1;
-
- //if current point (ii) still close to ii_from, jump to next ii
- if ( (points.col(ii_from)-points.col(ii)).squaredNorm() < max_distance_sq ) continue; //jump to for loop again
-
- //advance ii_from up to being closer than max_distance_sq
- while ( (points.col(ii_from+1)-points.col(ii)).squaredNorm() > max_distance_sq ) ii_from ++;
-
- // update the jump to be checked
- while (!jumps.empty() && ii_from > jumps.front()) jumps.pop_front();
-
- // check if there is a jump inside the window (not fitting if it is the case)
- if ( (jumps.front() > ii_from) && (jumps.front() <= ii) ) continue;
-
- //Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
- fitLine(points.block(0,ii_from, 3, _alg_params.segment_window_size), line);
-
- //if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
- if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
- {
- line.first_ = i_from;
- line.last_ = ii;
- line.point_first_ = points.col(line.first_);
- line.point_last_ = points.col(line.last_);
- line.np_ = line.last_ - line.first_ + 1;
- _line_list.push_back(line);
- }
- }
-// std::cout << "Lines fitted: " << _line_list.size() << std::endl;
-
- //In case just less than two lines found, return with 0 corners
- if ( _line_list.size() < 2 ) return 0;
-
- //STEP 2: concatenate lines
- line_it1 = _line_list.begin();
- while ( line_it1 != _line_list.end() )
- {
- line_it2 = line_it1;
- line_it2 ++;
- while (line_it2 != _line_list.end())
- {
-// std::cout << std::endl << "line 1: first idx: " << (int)line_it1->last_ << std::endl;
-// std::cout << "line 2: first idx: " << (int)line_it2->first_ << std::endl;
-// std::cout << "substraction " << (int)line_it2->first_ - (int)line_it1->last_ << std::endl;
-
- // check if last of line1 and first of line2 are within the threshold
- if ( ( (int)line_it2->first_ - (int)line_it1->last_ ) <= (int)_alg_params.max_beam_distance )
- {
-// std::cout << "line 1: point first: " << points.col(line_it1->first_).transpose() << std::endl;
-// std::cout << "line 1: point last: " << points.col(line_it1->last_).transpose() << std::endl;
-// std::cout << "line 2: point first: " << points.col(line_it2->first_).transpose() << std::endl;
-// std::cout << "line 2: point last: " << points.col(line_it2->last_).transpose() << std::endl;
-
- //compute angle between lines 1 and 2
- theta = angleBetweenLines(*line_it1, *line_it2);
-
- //Check angle threshold
- if ( theta < _alg_params.theta_max_parallel ) //fabs(theta) not required since theta>0
- {
- //compute a new line with all involved points
- Line new_line;
- fitLine(points.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line);
-
- //std::cout << "new_line.error: " << new_line.error_ << std::endl;
-
- //check if error below a threshold to effectively concatenate
- if ( new_line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
- {
- //update line1 as the concatenation of l1 and l2
- new_line.first_ = line_it1->first_;
- new_line.last_ = line_it2->last_;
- new_line.point_first_ = line_it1->point_first_;
- new_line.point_last_ = line_it2->point_last_;
- new_line.np_ = new_line.last_ - new_line.first_ + 1;
- *line_it1 = new_line;
-
- //remove l2 from the list
- line_it2 = _line_list.erase(line_it2);
-
-// std::cout << "lines concatenated" << std::endl;
-// std::cout << "line 1: " << std::endl << line_it1->first << std::endl <<
-// line_it1->last << std::endl << line_it1->vector_.transpose() << std::endl << line_it1->error << std::endl;
-// std::cout << "line 2: " << std::endl << line_it2->first << std::endl <<
-// line_it2->last << std::endl << line_it2->vector_.transpose() << std::endl << line_it2->error << std::endl;
-// std::cout << "line resultant: "<< std::endl << new_line.first << std::endl <<
-// new_line.last << std::endl << new_line.vector_.transpose() << std::endl << new_line.error << std::endl;
- }
- else
- line_it2 ++;
- }
- else
- line_it2 ++;
- }
- else
- break;
- }
- line_it1 ++;
- }
- //std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
-
- return _line_list.size();
-}
-*/
-
-
-/*
-//unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<Eigen::Vector4s> & _corner_list)
-//unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<laserscanutils::Corner> & _corner_list)
-unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Corner> & _corner_list)
-{
- //local variables
- //Eigen::MatrixXs points(3,_ranges.size());
- ScalarT azimuth, cos_theta, theta;
- //Eigen::Vector3s corner;
- Corner corner;
- Line line;
- std::list<Line> line_list;
- std::list<Line>::iterator line_it1, line_it2;
- std::queue<unsigned int> jumps;
- bool update_iterators;
- ScalarT max_distance_sq(_alg_params.max_distance_*_alg_params.max_distance_); //init max distance as the squared value to avoid sqroot on comparisons
-
- std::cout << "CORNER DETECTION FROM RANGES: " << std::endl;
- //_alg_params.print();
-
- //init random generator
- std::default_random_engine generator(1);
- std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
-
- //STEP 0: convert range polar data to cartesian points, and keep range jumps
-// for (unsigned int ii = 0; ii<_ranges.size(); ii++)
-// {
-// //range 2 polar
-// azimuth = _params.angle_max_ - _params.angle_step_ * ii;
-// points.col(ii) << _ranges(ii)*cos(azimuth), _ranges(ii)*sin(azimuth), 1; //points.row0-> x coordinate, points.row1->y coordinate
-//
-// //check range jump and keep it
-// if (ii > 0 && fabs(_ranges(ii)-_ranges(ii-1)) > _alg_params.max_distance_) jumps.push(ii);
-// }
- Eigen::MatrixXs points = laserPolar2xy(_params, _ranges, _alg_params.max_distance_, jumps);
- std::cout << "jumps: " << jumps.size() << std::endl;
- std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
-
- // STEP 1: find line segments running over the scan
- for (unsigned int ii = _alg_params.segment_window_size-1; ii<points.cols(); ii=ii+rand_window_overlapping(generator) )
- {
- unsigned int i_from = ii - _alg_params.segment_window_size + 1;
-
- // update the jump to be checked
- while (!jumps.empty() && i_from > jumps.front())
- jumps.pop();
-
- // check if there is a jump inside the window (not fitting if it is the case)
- if (jumps.front() > i_from && jumps.front() <= ii)
- continue;
-
- //Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
- //fitLine(i_from, ii, points, line);
- fitLine(points.block(0,i_from, 3, ii-i_from+1), line);
-
- //if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
- if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
- {
- line.first_ = i_from;
- line.last_ = ii;
- line.point_first_ = points.col(line.first_);
- line.point_last_ = points.col(line.last_);
- line.np_ = line.last_ - line.first_ + 1;
- line_list.push_back(line);
- }
- }
- std::cout << "Lines fitted: " << line_list.size() << std::endl;
-
- //In case just less than two lines found, return with 0 corners
- if ( line_list.size() < 2 ) return 0;
-
- //STEP 2: concatenate lines
- line_it1 = line_list.begin();
- line_it2 = line_it1;
- line_it2 ++;
- while ( line_it1 != line_list.end() )
- {
- //if no concatenation found, iterators will run forward
- update_iterators = false;
-
- // check if last of line1 and first of line2 are within the threshold
- if ( ( line_it2->first_ - line_it1->last_ ) <= _alg_params.line_params_.concatenation_ii_threshold_ )
- {
- //compute angle between lines 1 and 2
- theta = angleBetweenLines(*line_it1, *line_it2);
-// std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << "theta: " << theta << std::endl <<
-// "*index_it1: " << *index_it1 << std::endl << "*index_it2: " << *index_it2 << std::endl;
-// " (*line_it1).dot(*line_it2): " << (*line_it1).dot(*line_it2) << std::endl <<
-// " (*line_it1).norm()*(*line_it2).norm(): " << (*line_it1).norm()*(*line_it2).norm() << std::endl;
-
- //Check angle threshold
- if ( theta < _alg_params.theta_max_parallel ) //fabs(theta) not required since theta>0
- {
- //compute a new line with all involved points
- Line new_line;
- fitLine(points.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line);
-
- //check if error below a threshold to effectively concatenate
- if ( new_line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
- {
- //update line1 as the concatenation of l1 and l2
- new_line.first_ = line_it1->first_;
- new_line.last_ = line_it2->last_;
- new_line.point_first_ = points.col(new_line.first_);
- new_line.point_last_ = points.col(new_line.last_);
- new_line.np_ = new_line.last_ - new_line.first_ + 1;
- *line_it1 = new_line;
-
- //remove l2 from the list
- line_it2 = line_list.erase(line_it2);
-
- //in that case do not update iterators to allow concatenation of the new l1 with the next line in the list, if any
- // update_iterators = false;
-
-// std::cout << "lines concatenated" << std::endl;
-// std::cout << "line 1: " << std::endl << line_it1->first_ << std::endl <<
-// line_it1->last_ << std::endl << line_it1->vector_.transpose() << std::endl << line_it1->error_ << std::endl;
-// std::cout << "line 2: " << std::endl << line_it2->first_ << std::endl <<
-// line_it2->last_ << std::endl << line_it2->vector_.transpose() << std::endl << line_it2->error_ << std::endl;
-// std::cout << "line resultant: "<< std::endl << new_line.first_ << std::endl <<
-// new_line.last_ << std::endl << new_line.vector_.transpose() << std::endl << new_line.error_ << std::endl;
- }
- }
- }
- else
- update_iterators = false;
-
- //update iterators
- if (update_iterators)
- {
- line_it1 ++;
- line_it2 = line_it1;
- line_it2 ++;
- }
- }
- std::cout << "Lines after concatenation: " << line_list.size() << std::endl;
-
- //STEP 3: find corners over the line list
- line_it1 = line_list.begin();
- line_it2 = line_it1;
- line_it2 ++;
- while ( line_it1 != line_list.end() )
- {
- //if no concatenation found, iterators will run forward
- update_iterators = true;
-
- // check if last of line1 and first of line2 are within the threshold
- if ( ( line_it2->first_ - line_it1->last_ ) <= _alg_params.line_params_.concatenation_ii_threshold_ )
- {
- //compute angle between lines 1 and 2
- theta = angleBetweenLines(*line_it1, *line_it2);
-// cos_theta = (line_it1->vector_.head(2)).dot(line_it2->vector_.head(2)) / ( line_it1->vector_.head(2).norm()*line_it2->vector_.head(2).norm() );
-// theta = acos (cos_theta); //theta is in [0,PI]
-// if (theta > M_PI/2) theta = M_PI - theta;//assures theta>0 AND either theta close to 0 or close to PI will always map to theta close to 0.
-
-// std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << "theta: " << theta << std::endl <<
-// "line 1: " << line_it1->first << "-" << line_it1->last << std::endl <<
-// "line 2: " << line_it2->first << "-" << line_it2->last << std::endl <<
-// " (*line_it1).dot(*line_it2): " << (line_it1->vector_).dot(line_it2->vector_) << std::endl <<
-// " (*line_it1).norm()*(*line_it2).norm(): " << (line_it1->vector_).norm()*(line_it2->vector_).norm() << std::endl;
-
- //Check angle threshold
- if ( theta > _alg_params.theta_min_ ) //fabs(theta) not required since theta>0
- {
- // Find the corner ad the cross product between lines (intersection)
- //corner = (line_it1->vector_).cross(line_it2->vector_);
- corner.pt_ = (line_it1->vector_).cross(line_it2->vector_);
-
- // normalize corner point to set last component to 1
- corner.pt_ = corner.pt_ / corner.pt_(2);
-
- // Check if the corner is close to both lines ends. TODO: Move this check before computing intersection
- if ( ( (points.col(line_it1->last_) - corner.pt_).head(2).squaredNorm() < max_distance_sq )
- &&
- ( (points.col(line_it2->first_) - corner.pt_).head(2).squaredNorm() < max_distance_sq ) )
- {
-
- //vector from corner to first point of l1
- Eigen::Vector2s v1 = points.col(line_it1->first_).head(2) - corner.pt_.head(2);
-
- //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
- corner.orientation_ = atan2(v1(1),v1(0));
-
- //Compute corner aperture with line_it1->first, corner and line_it2->last
- corner.aperture_ = cornerAperture(points.col(line_it1->first_), corner.pt_, points.col(line_it2->last_));
-
- //set other descriptors
- corner.line_1_ = *line_it1;
- corner.line_2_ = *line_it2;
-// corner.np_l1_ = line_it1->last_ - line_it1->first_;//>0 assured due to scanning order
-// corner.np_l2_ = line_it2->last_ - line_it2->first_;//>0 assured due to scanning order
-// corner.error_l1_ = line_it1->error_;
-// corner.error_l2_ = line_it2->error_;
-
- //add the corner to the list
- _corner_list.push_back(corner);
- }
- }
-
- //update iterators //TODO: QUESTION: Are we checking each line against all ? Why if they are find consecutively?
- if (line_it2 != line_list.end() )
- {
- line_it2 ++;
- update_iterators = false;
- }
- }
-
- //update iterators
- if (update_iterators)
- {
- line_it1 ++;
- line_it2 = line_it1;
- line_it2 ++;
- }
-
- }
- std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
-
- // Erase duplicated corners
- if ( _corner_list.size() > 1 )
- {
- // concatenate lines
- std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin();
- std::list<laserscanutils::Corner>::iterator corner_it2 = corner_it1;
- corner_it2 ++;
- while ( corner_it2 != _corner_list.end() )
- {
- // Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) !
- if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq )
- {
- // Keep the one with bigger product of number of points of each line
- (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
- corner_it2 = _corner_list.erase(corner_it2);
- }
- else
- {
- corner_it1 ++;
- corner_it2 = corner_it1;
- corner_it2 ++;
- }
- }
- }
- std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
- for(std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin(); corner_it1 != _corner_list.end(); corner_it1++) corner_it1->print();
-
- return _corner_list.size();
-}
-*/
-
diff --git a/src/deprecated/corner_detector.h b/src/deprecated/corner_detector.h
deleted file mode 100644
index 4ddf5bf645b49711cb3f6648d69ede00af0ae4ba..0000000000000000000000000000000000000000
--- a/src/deprecated/corner_detector.h
+++ /dev/null
@@ -1,107 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#ifndef CORNERS_H_
-#define CORNERS_H_
-
-//std
-#include <iostream>
-#include <list>
-#include <queue>
-#include <random>
-
-// Eigen ingludes
-#include <eigen3/Eigen/Geometry>
-
-//laserscanutils
-#include "laser_scan_utils.h"
-#include "scan_basics.h"
-#include "entities.h"
-#include "line_detector.h"
-
-namespace laserscanutils
-{
- /** \brief Set of tunning parameters
- *
- * Set of tunning parameters for corner extraction
- *
- */
- struct ExtractCornerParams
- {
- //members
-// unsigned int segment_window_size; //number of points to fit lines in the first pass
-// ScalarT theta_min; //minimum theta between consecutive segments to detect corner. PI/6=0.52
-// ScalarT theta_max_parallel; //maximum theta between consecutive segments to fuse them in a single line.
-// ScalarT k_sigmas;//How many std_dev are tolerated to count that a point is supporting a line
-// unsigned int max_beam_distance;//max number of beams of distance between lines to consider corner or concatenation
-// ScalarT max_distance;//max distance between line ends to consider corner or concatenation
-
- ScalarT theta_min_; //minimum theta between consecutive segments to detect corner. PI/6=0.52
- ScalarT max_distance_;//max distance between line extremes to consider a corner
- ExtractLineParams line_params_; //algorithm parameters for line extraction, whcih is executed previously to corner extraction
-
- //just a print method
- void print() const;
- };
-
- /** \brief Extract corners from a given lines list. Result as a list of Corners
- *
- * Extract corners from a given lines list.
- * Returns corners as a list of laserscanutils::Corners
- *
- */
- unsigned int extractCorners(const laserscanutils::ScanParams & _params,
- const laserscanutils::ExtractCornerParams & _alg_params,
- const std::list<laserscanutils::Line> & _line_list,
- std::list<laserscanutils::Corner> & _corner_list);
-
- /** \brief Extract corners from a given scan. Result as a list of Corners
- *
- * Extract corners from a given scan.
- * Returns corners as a list of laserscanutils::Corners
- *
- */
- unsigned int extractCorners(const laserscanutils::ScanParams & _params,
- const laserscanutils::ExtractCornerParams & _alg_params,
- const std::vector<float> & _ranges,
- std::list<laserscanutils::Corner> & _corner_list);
-
- /** \brief Returns corner aperture
- *
- * Computes corner aperture in 2D, given two extreme points and the corner location
- * REQUIRES: three points in homogeneous coordinates (last component set to 1):
- * _p1: first point of the first line supporting a corner
- * _c: corner point
- * _p2: last point of the second line supporting a corner
- * ASSUMES:
- * - _p1, _c and _p2 are not collinear since they probably come from a corner detector
- * - _p1, _c and _p2 are ordered as a scan can view them, scanning clockwise from the top view point
- * ENSURES:
- * A return angle in [0,2pi]
- **/
- ScalarT cornerAperture(const Eigen::Vector3s & _p1,
- const Eigen::Vector3s & _c,
- const Eigen::Vector3s & _p2);
-
-}
-#endif
-
diff --git a/src/deprecated/entities.cpp b/src/deprecated/entities.cpp
deleted file mode 100644
index 4dd266543d0aa71770f4ab81be39a0207730a4a8..0000000000000000000000000000000000000000
--- a/src/deprecated/entities.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-//std
-#include "iostream"
-
-//laserscanutils
-#include "entities.h"
-
-void laserscanutils::Line::print() const
-{
- std::cout << "Line Parameters : " << std::endl
-// << " (a,b,c): " << vector_.transpose() << std::endl
-// << " error: " << error_ << std::endl
-// << " first,last: " << first_ << " , " << last_ << std::endl
-// << " first point: " << point_first_.transpose() << std::endl
-// << " last point: " << point_last_.transpose() << std::endl
- << "\tnumber of points: " << np_ << std::endl
- << "\trange: " << range_ << std::endl
- << "\ttheta: " << theta_ << std::endl;
-}
-
-float laserscanutils::Line::length()
-{
- return(std::sqrt( std::pow( (this->point_last_(0)-this->point_first_(0)), 2 ) + std::pow( (this->point_last_(1)-this->point_first_(1)), 2) ) );
-}
-
-
-
-void laserscanutils::Corner::print() const
-{
- std::cout << "Corner Parameters : " << std::endl
- << " corner point: " << pt_.transpose() << std::endl
- << " orientation: " << orientation_ << std::endl
- << " aperture: " << aperture_ << std::endl;
-// std::cout << "Line 1: ";
-// line_1_.print();
-// std::cout << "Line 2: ";
-// line_2_.print();
-}
-
-
-
-
-void laserscanutils::Object::print() const
-{
- std::cout <<
- std::endl << "---------------------------------------------" << std::endl
- << "--- Object Detected Information ---" << std::endl
- << "---------------------------------------------" << std::endl
- << " * Initial Global Point : \t" << this->first_ << std::endl
- << " * Final Global Point : \t" << this->last_ << std::endl
- << " * Number of Points: \t" << this->num_points_ << std::endl
- << " * Associated?: \t\t" << this->associated_<< std::endl
- << " * Structured?: \t\t" << this->structured_ << std::endl
- << " * Bounding Box Center : \t" << this->object_center_.transpose() << std::endl
- << " * Bounding Box Size : \t\t" << this->size_x_ << " x " << this->size_y_ << std::endl
- << " * Bounding Box Orientation :" << this->obj_orientation_ << std::endl
- << " * Polyline: " << std::endl << "\t\t" << this->polyline_points_ << std::endl
- << " * Lines: " << std::endl;
-
- if (this->line_list_.size() >= 1)
- {
- for (auto it1 = this->line_list_.begin(); it1 != this->line_list_.end(); it1++)
- {
- std::cout << " - Line #." << std::distance(this->line_list_.begin(),it1) << std::endl;
- it1->print();
- std::cout << std::endl;
- }
- }
- else
- std::cout << " --> No Lines in this Object. " << std::endl;
-
- std::cout << " * Corners: " << std::endl;
- if (this->corner_list_.size() >= 1)
- {
- std::cout << "object Corner line SIZE: " << this->corner_list_.size() << std::endl;
- for (auto it1 = this->corner_list_.begin(); it1 != this->corner_list_.end(); it1++)
- {
- std::cout << " - Corner #." << std::distance(this->corner_list_.begin(),it1) << std::endl;
- it1->print();
- std::cout << std::endl;
- }
- }
- else
- std::cout << " --> No corners in this Object. " << std::endl;
-
-
-}
-
-float laserscanutils::Object::getArea()
-{
- return this->size_x_*this->size_y_;
-}
-
-float laserscanutils::Object::getPerimeter()
-{
- return ( (this->size_x_*2) + (this->size_y_*2) );
-}
diff --git a/src/deprecated/entities.h b/src/deprecated/entities.h
deleted file mode 100644
index 0e0ae3835af4f1de6f6fc60e802feee691cbb44e..0000000000000000000000000000000000000000
--- a/src/deprecated/entities.h
+++ /dev/null
@@ -1,157 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#ifndef ENTITIES_H_
-#define ENTITIES_H_
-
-//laserscanutils
-#include "laser_scan_utils.h"
-
-//std
-#include <iostream>
-#include <list>
-
-namespace laserscanutils
-{
- /** \brief Line entity represents straight segments on the plane
- *
- * Line entity represents straight segments on the plane
- * Several parameterizations can coexist, but no coherency check between them is performed.
- * Not all parameterizations are required.
- *
- **/
- struct Line
- {
- Eigen::Vector3s vector_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0 TODO: rename to abc_
- unsigned int first_; //corresponding index to the scan ranges of the first point used TODO: rename to idx_first_
- unsigned int last_; //corresponding index to the scan ranges of the last point used TODO: rename to idx_last_
- ScalarT error_; //sum of all distances from used points to line when fitting
- Eigen::Vector3s point_first_; //homogeneous coordinates of the starting 2D point
- Eigen::Vector3s point_last_; //homogeneous coordinates of the ending 2D point
- unsigned int np_; // number of scan points supporting this line (through fitting, Hough voting, ...)
- ScalarT range_; //range component in polar parameterization.
- ScalarT theta_; //theta component in polar parameterization.
-
- //just a print method
- void print() const;
-
- //returns length
- float length();
- };
-
- struct Corner
- {
- Corner()
- {
- this->pt_ = {0,0,0};
- this->aperture_ = 0;
- this->orientation_ = 0;
- from_object_ = -1;
-
- }
-
- Eigen::Vector3s pt_; //homogeneous coordinates of the 2D point, [meters]
- ScalarT orientation_; //orientation angle from the line1 to x axis, in [-pi,pi]
- ScalarT aperture_; //angle aperture of the corner in [0,2pi]. Aperture angle defined passing thorugh the shadow area of the scan (not crossing rays)
- Line line_1_; // line of the corner
- Line line_2_; // line of the corner
-
- // test
- ScalarT from_object_; // ID of the object that belongs to (-1 if not associated).
-
- //just a print method
- void print() const;
- };
-
-
- struct Object
- {
- // Constructor
- Object()
- {
- this->num_points_ = 0;
- this->first_ = 0;
- this->last_ = 0;
- this->associated_ = false;
- this->structured_ = false;
-
- this->id_ = -1;
-
- this->ref_centroid_point_ = {0.0, 0.0, 0.0};
- this->ref_act_point_ = {0.0, 0.0, 0.0};
- this->ref_old_point_ = {0.0, 0.0, 0.0};
-
- this->min_x_ = 1000.0;
- this->min_y_ = 1000.0;
- this->max_x_ = -1000.0;
- this->max_y_ = -1000.0;
- this->obj_orientation_ = 0.0;
- this->object_center_ << 0.0, 0.0;
- this->size_x_ = 0.0;
- this->size_y_ = 0.0;
-
-
- this->ref_type_ = -1;
- this->ref_position_ = -1; // TBR??? Position of the corner/line in the respective list...
- this->asso_to_prev_obj_in_pos_ = -1; // TBR??? Position of the corner/line in the respective list...
-
- }
-
- bool associated_;
- bool structured_;
- unsigned int num_points_; // number of points of the cluster
- unsigned int first_; //index of the Homogeneous Matrix column for the first point used (GLOBAL)
- unsigned int last_; //index of the Homogeneous Matrix column for the last point used (GLOBAL)
- std::list<laserscanutils::Line> line_list_; // list of lines contained in the cluster (Refs Points are LOCAL)
- std::list<laserscanutils::Corner> corner_list_; // list of corners contained in the cluster (Refs Points are LOCAL)
-
- ScalarT id_; // Id of the object. -1 if unknown or MOBILE; other number if pre-associated.
-
- unsigned int asso_to_prev_obj_in_pos_; // For tracking help, this object has been associated to the object in position
- // "asso_to_obj_in_pos_" from the previous scan. (-1 if not)
-
- // Description
- Eigen::Vector3s ref_centroid_point_;
- Eigen::Vector3s ref_act_point_;
- Eigen::Vector3s ref_old_point_;
-
- float min_x_;
- float min_y_;
- float max_x_;
- float max_y_;
- float obj_orientation_;
- float size_x_;
- float size_y_;
- Eigen::Vector2s object_center_;
- Eigen::MatrixXs polyline_points_; //<float,2,5>
-
- int ref_type_; // Type of the reference. 8 = corner; 4 = init_line ; 2 = end_line; 1 = centroid. -1 = unkonwn;
- int ref_position_; // [TODO] The position of the referenced corner (or line) in its corresponding list;
-
-
- void print() const;
- float getArea();
- float getPerimeter();
-
- };
-}
-#endif
diff --git a/src/deprecated/examples/CMakeLists.txt b/src/deprecated/examples/CMakeLists.txt
deleted file mode 100644
index 6937773858fe77f5d4ce3b0347e08bfd3bb80c73..0000000000000000000000000000000000000000
--- a/src/deprecated/examples/CMakeLists.txt
+++ /dev/null
@@ -1,13 +0,0 @@
-# corner extraction test
-IF(faramotics_FOUND)
- ADD_EXECUTABLE(test_extract_corners test_extract_corners.cpp)
- TARGET_LINK_LIBRARIES(test_extract_corners ${GLUT_glut_LIBRARY} ${pose_state_time_LIBRARIES} ${faramotics_LIBRARIES} ${PROJECT_NAME})
-ENDIF(faramotics_FOUND)
-
-# corner aperture test
-ADD_EXECUTABLE(test_corner_aperture test_corner_aperture.cpp)
-TARGET_LINK_LIBRARIES(test_corner_aperture ${PROJECT_NAME})
-
-# Object Detector test
-#ADD_EXECUTABLE(test_laser_detector test_laser_detector.cpp)
-#TARGET_LINK_LIBRARIES(test_laser_detector ${PROJECT_NAME})
diff --git a/src/deprecated/examples/testOneScan.txt b/src/deprecated/examples/testOneScan.txt
deleted file mode 100644
index 859c61e67dbc2064d77ae68b39e2372aa80ceeb6..0000000000000000000000000000000000000000
--- a/src/deprecated/examples/testOneScan.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-2.47857,2.40308,2.46936,2.49425,2.58269,2.6075,2.6976,2.67785,2.75212,2.81579,2.88573,2.93293,2.9802,3.02752,3.07489,3.12231,3.20072,3.26732,3.31537,3.36345,3.43738,3.51185,3.56706,3.58244,3.68457,3.78771,3.81647,3.88631,3.91461,4.01289,4.09067,4.14022,4.21861,4.29739,4.38391,4.42651,4.50619,4.61629,4.6969,4.71678,4.85908,4.94062,4.96786,5.07315,5.21855,5.26954,5.38442,5.49995,5.55117,5.67566,5.78439,5.86879,6.01984,6.10489,6.19005,6.34295,6.42867,6.58285,6.66897,6.85886,6.94558,7.13698,7.22426,7.41708,7.50468,7.69889,7.78679,8.018,8.17831,8.3029,8.53652,8.66171,8.89682,9.13282,9.36953,9.50533,9.77104,10.0097,10.2865,10.5641,10.8424,11.1214,11.4011,11.6813,12.0002,12.3675,12.8314,13.1143,13.6283,13.8832,14.409,15.6434,16.1826,16.6058,17.1172,17.6488,18.3473,19.0567,19.7473,20.6452,21.5048,22.4048,23.5033,24.6522,25.8219,27.3595,76.0194,75.946,30.8241,75.9103,75.8487,75.8609,75.8273,75.6581,76.332,76.1013,91.4294,91.4046,91.4127,91.3939,91.3481,91.4349,146.41,145.369,145.355,54.1523,49.9382,46.2363,44.6039,42.9663,42.9377,42.9911,42.8599,42.9149,42.89,42.8329,42.9128,42.8853,42.8537,42.8924,42.834,42.7902,42.8156,42.7908,42.8072,42.7463,42.6903,42.7922,42.7359,42.7112,42.7083,
--4.25007,-4.03921,-4.06938,-4.03067,-4.09332,-4.05381,-4.11453,-4.00768,-4.04203,-4.05894,-4.08321,-4.0741,-4.06448,-4.05435,-4.04369,-4.03252,-4.06009,-4.071,-4.05782,-4.04411,-4.0604,-4.0757,-4.06745,-4.01379,-4.0564,-4.09752,-4.05703,-4.05969,-4.01846,-4.04806,-4.05513,-4.03323,-4.03844,-4.04258,-4.05244,-4.02076,-4.02193,-4.04838,-4.04711,-3.99305,-4.04126,-4.03666,-3.98713,-3.99935,-4.04063,-4.00705,-4.02074,-4.03273,-3.99627,-4.01116,-4.01278,-3.99591,-4.02233,-4.00254,-3.98158,-4.00211,-3.97818,-3.99457,-3.96762,-3.99997,-3.96972,-3.9969,-3.96337,-3.98534,-3.94841,-3.96519,-3.92486,-3.95404,-3.94475,-3.9159,-3.93539,-3.90182,-3.91469,-3.92377,-3.92897,-3.88873,-3.89825,-3.89258,-3.89719,-3.89729,-3.89281,-3.88376,-3.87014,-3.85191,-3.84129,-3.84021,-3.86182,-3.82248,-3.84359,-3.78501,-3.79356,-3.97291,-3.95986,-3.91015,-3.87323,-3.83194,-3.81625,-3.79061,-3.74914,-3.73326,-3.69521,-3.64886,-3.6175,-3.5744,-3.51421,-3.48057,-8.99749,-8.31744,-3.10383,-6.97518,-6.30254,-5.63744,-4.96998,-4.29618,-3.66649,-2.99003,3.59227,4.39048,5.19079,5.99027,6.78833,7.59765,18.6256,22.3744,23.6725,13.251,13.1476,13.04,13.0008,12.9314,13.3325,13.7616,14.1331,14.5676,14.9778,15.3786,15.8314,16.2478,16.6649,17.1123,17.5239,17.9434,18.395,18.8284,19.2833,19.7063,20.134,20.6405,21.075,21.5282,21.9962,
-1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
diff --git a/src/deprecated/examples/test_corner_aperture.cpp b/src/deprecated/examples/test_corner_aperture.cpp
deleted file mode 100644
index f90baf9a144990a569b008c103493626d07c3f05..0000000000000000000000000000000000000000
--- a/src/deprecated/examples/test_corner_aperture.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-//std includes
-#include <iostream>
-
-//laserscanutils
-#include "entities.h"
-#include "corner_detector.h"
-
-//main
-int main(int argc, char** argv)
-{
- std::cout << "\n ========= Corner aperture test ===========\n";
-
- //declare two points and a corner in between
- Eigen::Vector3s p1; p1 << 2,4,1;
- Eigen::Vector3s p2; p2 << 4,1,1;
- Eigen::Vector3s c; c << 4,4,1;
-
- //compute aperture
- laserscanutils::ScalarT ap = laserscanutils::cornerAperture(p1,c,p2);
- std::cout << "Aperture: " << ap << std::endl;
-
- //exit
- return 0;
-}
diff --git a/src/deprecated/examples/test_extract_corners.cpp b/src/deprecated/examples/test_extract_corners.cpp
deleted file mode 100644
index 05e9947e312552d0b1845185a58d149751e2897b..0000000000000000000000000000000000000000
--- a/src/deprecated/examples/test_extract_corners.cpp
+++ /dev/null
@@ -1,189 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-//std includes
-#include <iostream>
-#include <list>
-#include <random>
-
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-//laserscanutils
-#include "entities.h"
-#include "corner_detector.h"
-
-//function to travel around each model
-void motionCampus(unsigned int ii, Cpose3d & pose)
-{
- if (ii<=40)
- {
- //pose.rt.setEuler( pose.rt.head()-2*M_PI/180., pose.rt.pitch(), pose.rt.roll() );
- pose.pt(0) = pose.pt(0) + 0.1;
- }
- if ( (ii>40) && (ii<=80) )
- {
- pose.pt(0) = pose.pt(0) + 0.1;
- pose.rt.setEuler(pose.rt.head(), pose.rt.pitch(), pose.rt.roll() + 0.01);
- }
- if ( (ii>80) && (ii<=120) )
- {
- pose.pt(0) = pose.pt(0) + 0.1;
- pose.rt.setEuler(pose.rt.head(), pose.rt.pitch(), pose.rt.roll() - 0.015);
- }
- if ( (ii>120) && (ii<=170) )
- {
- pose.rt.setEuler( pose.rt.head()+1.8*M_PI/180., pose.rt.pitch(), pose.rt.roll() );
- pose.moveForward(0.2);
- }
- if ( (ii>170) && (ii<=220) )
- {
- pose.rt.setEuler( pose.rt.head()-1.8*M_PI/180., pose.rt.pitch(), pose.rt.roll()-0.05*M_PI/180. );
- }
- if ( (ii>220) && (ii<=310) )
- {
- pose.pt(0) = pose.pt(0) + 0.1;
- }
- if ( (ii>310) && (ii<=487) )
- {
- pose.rt.setEuler( pose.rt.head()-1.*M_PI/180., pose.rt.pitch(), pose.rt.roll()+0.1*M_PI/180. );
- pose.moveForward(0.1);
- }
- if ( (ii>487) && (ii<=582) )
- {
- pose.moveForward(0.2);
- }
- if ( (ii>582) && (ii<=700) )
- {
- pose.pt(2) = pose.pt(2) + 0.001;
- pose.rt.setEuler( pose.rt.head()-1.*M_PI/180., pose.rt.pitch(), pose.rt.roll());
- pose.moveForward(0.1);
- }
-}
-
-int main(int argc, char** argv)
-{
- std::cout << "\n ========= Corner extraction test ===========\n";
-
- // INITIALIZATION ============================================================================================
-
- if (argc != 2)
- {
- std::cout << "Invalid number of arguments!" << std::endl;
- std::cout << "Call test as: test_extract_corners [full_path_file_name]" << std::endl;
- std::cout << "EXIT PROGRAM" << std::endl;
- return -1;
- }
-
- // Faramotics stuff
- CdynamicSceneRender *myRender;
- CrangeScan2D *myScanner;
- Cpose3d viewPoint;
- Cpose3d devicePose(2,8,0.2,0,0,0);
- vector<float> myScan;
- string modelFileName(argv[1]);
-
- //laserscanutils
- laserscanutils::ScanParams scan_params;
- scan_params.angle_min_ = -M_PI/2;
- scan_params.angle_max_ = M_PI/2;
- scan_params.angle_step_ = M_PI/720;
- scan_params.scan_time_ = 0.01;//not relevant
- scan_params.range_min_ = 0.1;
- scan_params.range_max_ = 30;
- scan_params.range_std_dev_ = 0.01;
- scan_params.print();
-
- laserscanutils::ExtractCornerParams alg_params;
- alg_params.line_params_.jump_dist_ut_ = 1.0;
- alg_params.line_params_.jump_angle_ut_ = 3;
- alg_params.line_params_.window_length_ = 0.5;
- alg_params.line_params_.min_window_points_ = 5;
- alg_params.line_params_.k_sigmas_ut_ = 3;
- alg_params.line_params_.concatenate_ii_ut_ = 5;
- alg_params.line_params_.concatenate_angle_ut_ = 0.1;
- alg_params.theta_min_ = 0.4;
- alg_params.max_distance_ = 1.0;
- alg_params.print();
-
- //init random generators
- std::default_random_engine generator(1);
- std::normal_distribution<laserscanutils::ScalarT> laser_range_noise(0.001, scan_params.range_std_dev_);
-
- //glut initialization
- faramotics::initGLUT(argc, argv);
-
- //create a viewer for the 3D model and scan points
- myRender = new CdynamicSceneRender(1200,700,90*M_PI/180,90*700.0*M_PI/(1200.0*180.0),0.2,100);
- myRender->loadAssimpModel(modelFileName,true); //with wireframe
-
- //create scanner and load 3D model
- myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);//HOKUYO_UTM30LX_180DEG
- myScanner->loadAssimpModel(modelFileName);
-
- // START TRAJECTORY ============================================================================================
- for (uint step=1; step < 1000; step++)
- {
- std::cout << std::endl << "New iter: " << step << std::endl;
-
- // moves the device position
- motionCampus(step, devicePose);
-
- //compute scan
- myScan.clear();
- myScanner->computeScan(devicePose,myScan);
-
- //draws the device frame, scan hits and depth image
- myRender->drawPoseAxis(devicePose);
- myRender->drawScan(devicePose,myScan,180.*M_PI/180.,90.*M_PI/180.); //draw scan
-
- // extract corners
- std::list<laserscanutils::Corner> corner_list;
- laserscanutils::extractCorners(scan_params, alg_params, myScan, corner_list);
- std::cout << "corner_list.size(): " << corner_list.size() << std::endl;
-
- //draw corners
- std::vector<double> corner_vector(corner_list.size()*2);
- unsigned int ii = 0;
- for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++, ii=ii+2)
- {
- corner_vector[ii] = corner_it->pt_(0);
- corner_vector[ii+1] = corner_it->pt_(1);
- }
- myRender->drawCorners(devicePose,corner_vector);
-
- //Set view point and render the scene. Locate visualization view point, somewhere behind the device
- viewPoint.setPose(devicePose);
- viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
- viewPoint.moveForward(-5);
- myRender->setViewPoint(viewPoint);
- myRender->render();
- }
-
- //delete things
- delete myRender;
- delete myScanner;
-
- //exit
- return 0;
-}
diff --git a/src/deprecated/examples/test_laser_detector.cpp b/src/deprecated/examples/test_laser_detector.cpp
deleted file mode 100644
index 6cea73820e2081e48463d015152e097ced9bec37..0000000000000000000000000000000000000000
--- a/src/deprecated/examples/test_laser_detector.cpp
+++ /dev/null
@@ -1,283 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include <algorithm>
-#include <fstream>
-#include <iostream>
-#include <iterator>
-#include <sstream>
-#include <string>
-#include <vector>
-#include <eigen3/Eigen/Dense>
-
-//#include <graphics.h>
-#include <GL/gl.h>
-#include <GL/glut.h>
-#include <GL/glu.h>
-
-//laserscanutils
-#include "entities.h"
-#include "clustering.h"
-#include "object_detector.h"
-
-
-
-void readOneScanRanges(const char * _file_name, std::vector<float> & ranges_read)
-{
- // "/home/vvaquero/iri-lab/matlab/laser_detection_mat/TestOneScanRanges.txt"
- std::ifstream inputFile(_file_name); // input file stream with name
-
- ranges_read.clear();
-
- std::string lineTest;
- std::getline( inputFile, lineTest );
-
- // use a stringstream to separate the fields out of the line
- std::stringstream ss( lineTest);
- std::string field;
- while (getline( ss, field, ',' ))
- {
- // for each field convert it to a double
- std::stringstream fs( field );
- double f = 0.0; // (default value is 0.0)
- fs >> f;
-
- ranges_read.push_back( f );
- }
- inputFile.close();
-
-
- // printing
-// std::cout << " Vector Ranges [ " ;
-// for (unsigned int k = 0; k < 10; k++)
-// {
-// std::cout << ranges_read[k] << ", ";
-// }
-// std::cout << " ] " << std::endl;
-}
-
-
-void initParameters(laserscanutils::LaserDetectionParams & _parameters)
-{
- _parameters.jump_thr = 0.9;
- _parameters.max_num_cluster_points = 200;
- _parameters.min_num_cluster_points = 5;
- _parameters.segment_window_size = 6;
- _parameters.theta_max_parallel = 0.1;
- _parameters.k_sigmas = 3;
-
- //_parameters.print();
-}
-
-
-
-int main()
-{
-/* //TEST For graphical output
- glutInit(&argc, argv);
- glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
- glutInitWindowSize(800,600);
- glutCreateWindow("Hello World");
-
- setup();
- glutDisplayFunc(display);
- glutMainLoop();
- */
-
- // ***** INITIALIZATION ***** //
-
- laserscanutils::LaserDetectionParams alg_parameters; // structure with the parameters to tune the algorithm
-// laserscanutils::LaserDetectionStats alg_stats; // structure with the internal variables of the algorithm
- laserscanutils::LaserScanProcessed alg_stats; // structure with the internal variables of the algorithm
- initParameters(alg_parameters);
- alg_parameters.print();
-
-
- // get laser Scan Parameters from real laser rosbag
- // Laser 2D Scan Parameters :
- // Angle min: -1.0472
- // Angle max: 0.610865
- // Angle step: 0.00436332
- // Scan time: 6.95332e-310
- // Range min: 0.1
- // Range max: 200
- // Range std dev: 6.91555e-310
- laserscanutils::ScanParams scan_params;
- scan_params.angle_min_ = -1.0472;
- scan_params.angle_max_ = 0.610865;
- scan_params.angle_step_ = 0.00436332;
- scan_params.scan_time_ = 0.01;//not relevant
- scan_params.range_min_ = 0.1;
- scan_params.range_max_ = 200;
- scan_params.range_std_dev_ = 0.01;
- scan_params.print();
-
- laserscanutils::ExtractCornerParams algExtractCornerParams;
- algExtractCornerParams.line_params_.jump_dist_ut_ = 500;
- algExtractCornerParams.line_params_.jump_angle_ut_ = 3;
- algExtractCornerParams.line_params_.window_sz_ = 5;
- algExtractCornerParams.line_params_.k_sigmas_ut_ = 3;
- algExtractCornerParams.line_params_.concatenate_ii_ut_ = 5;
- algExtractCornerParams.line_params_.concatenate_angle_ut_ = 0.1;
- algExtractCornerParams.theta_min_ = 0.01;
- algExtractCornerParams.max_distance_ = 0.9;
- algExtractCornerParams.print();
-
- // ***** READING LASER POINTS FOR TEST ***** //
- std::vector<std::vector<float> > values; // temp for reading the file values
- std::ifstream fin("/home/vvaquero/iri-lab/matlab/laser_detection_mat/testOneScanMatrix.txt"); // input file stream with name
-
- for (std::string line; std::getline(fin, line); )
- {
- std::replace(line.begin(), line.end(), ',', ' ');
- std::istringstream in(line);
- values.push_back(
- std::vector<float>(std::istream_iterator<float>(in),
- std::istream_iterator<float>()));
- }
-
- unsigned int rows = values.size();
- unsigned int columns = 0;
-
- for (unsigned int k = 0; k < rows; k++)
- {
- if (values[k].size() > columns)
- columns = values[k].size();
- }
-
- std::cout << "Matrix Size = " << rows << "x" << columns <<std::endl;
- Eigen::MatrixXs result(rows,columns);
-
- for (unsigned int i = 0; i < rows; i++)
- for (unsigned int j = 0; j < columns; j++)
- {
- result(i,j) = values[i][j];
- }
-
- alg_stats.scanTotalPoints_ = 380;
- alg_stats.filteredTotalPoints_ = columns;
-
- //std::cout << "Matrix = " << std::endl << result <<std::endl;
-
-
- // |
- // ->
- // Till here, everything is like a scan has been received and prefiltered
-
-
-
- // Finding clusters on the scan -> fulfills "stats.cluster_idxs", cleared at the begining.
- //laserscanutils::findClusters(alg_parameters, result,alg_stats);
-
-
- laserscanutils::extractClustersInScan(alg_parameters, result,alg_stats);
-
- // DEBUG: Printing Cluster Information.
- std::cout << "Clusters: " << std::endl <<
- " TotalFound = " << alg_stats.number_of_clusters_ << std::endl <<
- " Valid Ones = " << alg_stats.number_of_valid_clusters_ << std::endl <<
- " Indexes = " ;
- for (unsigned int j = 0; j < alg_stats.cluster_indxs_.size(); j++)
- {
- std::pair<int,int> tempSeg = alg_stats.cluster_indxs_[j];
- std::cout << "(" << tempSeg.first << "," << tempSeg.second << ") | " ;
- }
- std::cout << std::endl;
-
-
-
- std::list<laserscanutils::Line>::iterator list_it1;
-
- alg_stats.line_list_.clear();
-
-
-
- //for (auto pair_it = alg_stats.cluster_indxs_.begin(); pair_it != alg_stats.cluster_indxs_.end(); pair_it++)
- for (auto object_it = alg_stats.object_list_.begin(); object_it != alg_stats.object_list_.end(); object_it++)
- {
- // temporal object
- laserscanutils::Object temp_obj = *object_it;
- //std::pair<int,int> tempSeg = *pair_it;
- //std::list<laserscanutils::Line> line_list_temp; // temporal //TODO: put on alg_stats structure.
-
- //std::cout << std::endl << "-------" << std::endl << " SEGMENT FROM : " << tempSeg.first << " to " << tempSeg.second << std::endl;
- std::cout << std::endl << "-------" << std::endl << " ObjectCluster FROM : " << object_it->first_ << " to " << object_it->last_ << std::endl;
-
- Eigen::MatrixXs tempCluster = result.block(0,object_it->first_, 3, object_it->last_-object_it->first_+1);
-
- //std::cout << "tempCluster = " << std::endl << tempCluster << std::endl;
-
-
- // LINES
- laserscanutils::findLinesInCluster(scan_params,alg_parameters, tempCluster, object_it->first_ , object_it->line_list_);
-
- // concat line_list_temp with to the general line_list
- std::cout << " Temp Lines in cluster= " << object_it->line_list_.size() << std::endl;
-
- //alg_stats.object_list_.push_back(temp_obj);
-
- //alg_stats.line_list_.splice(alg_stats.line_list_.end(),object_it->line_list_,object_it->line_list_.begin(),object_it->line_list_.end());
- alg_stats.line_list_.insert(alg_stats.line_list_.end(),object_it->line_list_.begin(), object_it->line_list_.end());
- std::cout << " Final Lines in total = " << alg_stats.line_list_.size() << std::endl;
-
-
- // CORNERS
- std::cout << "Extracting the Corners of each Cluster/Object" << std::endl;
- laserscanutils::findCornersInClusterLines(scan_params, algExtractCornerParams, object_it->line_list_, object_it->corner_list_);
-
- }
-
-
-
-
-
-
- // DEBUG: Printing global lines information
- std::cout << std::endl << std::endl << "GLOBAL LINE LIST" << std::endl;
- for (list_it1 = alg_stats.line_list_.begin(); list_it1 != alg_stats.line_list_.end(); list_it1++)
- {
- std::cout << " #." << std::distance(alg_stats.line_list_.begin(),list_it1) << std::endl;
- list_it1->print();
- std::cout << std::endl << std::endl;
- }
- // DEBUG: END Printing global lines information
-
-
-
- // DEBUG: Printing ALL objects information
- for (auto object_it1 = alg_stats.object_list_.begin(); object_it1 != alg_stats.object_list_.end(); object_it1++)
- {
- std::cout << " #." << std::distance(alg_stats.object_list_.begin(),object_it1) << std::endl;
- object_it1->print();
- std::cout << std::endl << std::endl;
- }
- // DEBUG: END Printing ALL objects information
-
-
- unsigned int numberOfCorners;
- std::list<laserscanutils::Corner> corner_list;
- //numberOfCorners = laserscanutils::findCornersInClusterLines(scan_params,algExtractCornerParams,line_list,corner_list);
-
-
-
-
-
-} // end main
diff --git a/src/deprecated/line_detector.cpp b/src/deprecated/line_detector.cpp
deleted file mode 100644
index 2e141f47613acbde2e93149803229cc7dc90ca62..0000000000000000000000000000000000000000
--- a/src/deprecated/line_detector.cpp
+++ /dev/null
@@ -1,374 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#include "line_detector.h"
-
-void laserscanutils::ExtractLineParams::print() const
-{
- std::cout << "Extract Line Algorithm Parameters : " << std::endl
- << " jump_dist_ut_: " << jump_dist_ut_ << std::endl
- << " jump_angle_ut_: " << jump_angle_ut_ << std::endl
- << " window_length: " << window_length_ << std::endl
- << " k_sigmas_ut_: " << k_sigmas_ut_ << std::endl
- << " concatenate_ii_ut_: " << concatenate_ii_ut_ << std::endl
- << " concatenate_angle_ut_: " << concatenate_angle_ut_ << std::endl;
-}
-
-void laserscanutils::fitLine(const Eigen::MatrixXs & _points, Line & _line)
-{
- //build the system
- Eigen::Matrix3s AA = _points * _points.transpose();
- AA.row(2) << 0,0,1;
-
- //solve for line
- _line.vector_ = AA.inverse().col(2);
-
- // normalize the line
- //std::cout << "line squaredNorm (before) " << _line.vector_.head(2).squaredNorm() << std::endl;
- _line.vector_ /= _line.vector_.head(2).norm();
- //std::cout << "line squaredNorm " << _line.vector_.head(2).squaredNorm() << std::endl;
-
- // compute fitting error
- _line.error_ = (_points.transpose() * _line.vector_).squaredNorm() / _points.cols(); //_line.error_ = (_points.transpose() * _line.vector_).array().abs().sum() / (_line.vector_.head(2).norm()*_points.cols());
-}
-
-unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _params,
- const ExtractLineParams & _alg_params,
- const std::vector<float> & _ranges,
- std::list<laserscanutils::Line> & _line_list)
-{
- //local variables
- Eigen::MatrixXs points;
- ScalarT cos_theta, theta;
- Line line;
- std::list<Line>::iterator line_it1, line_it2, last_it;
- std::list<unsigned int> jumps;
- std::vector<float> ranges(_ranges);
- std::list<unsigned int>::iterator jj_jump;
- bool update_iterators;
- unsigned int ii, window_points;
-
- //STEP 1: transform to euclidean coordinates and detect jumps
- ranges2xy(_params, ranges, _alg_params.jump_dist_ut_, _alg_params.jump_angle_ut_, points, jumps);
-// std::cout << "jumps: " << jumps.size() << std::endl;
-// std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
-
- // STEP 2: find line segments running over the scan
- ii = 0;
- //ii2=_alg_params.window_sz_;
- window_points = windowPoints(_params, _alg_params, _ranges, ii);
- jj_jump = jumps.begin();
-
-
- while ( ii+window_points < points.cols() )
- {
- //check if a jump exists between ii and ii+window_points
- if ( jj_jump != jumps.end() && ii < (*jj_jump) && (ii+window_points >= (*jj_jump)) )
- {
- ii = *jj_jump;
- jj_jump++;
- }
- else
- {
- //Found the best fitting line over points within the window [ii,ii+window_points]
- fitLine(points.block(0,ii,3,window_points), line);
-
- //if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
- if ( line.error_ < _params.range_std_dev_*_params.range_std_dev_*_alg_params.k_sigmas_ut_*_alg_params.k_sigmas_ut_ )
- {
- line.first_ = ii;
- line.last_ = ii + window_points;
- line.point_first_ = points.col(line.first_);
- line.point_last_ = points.col(line.last_);
- line.np_ = line.last_ - line.first_ + 1;
- _line_list.push_back(line);
- }
-
- //increment window iterators
- ii++;
- }
- window_points = windowPoints(_params, _alg_params, _ranges, ii);
- //std::cout << "min window points: " << _alg_params.min_window_points_ << std::endl;
- //std::cout << "range: " << ranges[ii] << std::endl;
- //std::cout << "l_step: " << _params.angle_step_ * ranges[ii] << std::endl;
- //std::cout << "window_sz: " << _alg_params.window_sz_ << std::endl;
- //std::cout << "window size / l_step: " << (_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
- //std::cout << "(unsigned int) window size / l_step: " << (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
- //std::cout << "window_points: " << window_points << std::endl << std::endl;
- }
- //std::cout << "Lines fitted: " << _line_list.size() << std::endl;
-
- //STEP 3: concatenate lines
- if ( _line_list.size() < 2 ) return _line_list.size(); //In case just less than two lines found, return
- line_it1 = _line_list.begin();
- line_it2 = line_it1;
- line_it2++;
- last_it = _line_list.end();
- last_it--;
- jj_jump = jumps.begin();
- while (line_it1 != last_it && line_it1 != _line_list.end())
- {
- while (jj_jump != jumps.end() && line_it1->first_ > (*jj_jump))
- jj_jump++;
-
- // check number of beams and jumps between lines
- if ( ( (int)line_it2->first_ - (int)line_it1->last_ ) <= (int)_alg_params.concatenate_ii_ut_ && //not too many points between lines
- !(jj_jump != jumps.end() && line_it1->first_ < (*jj_jump) && line_it2->last_ >= (*jj_jump)) ) //not jumps between lines
- {
- //compute angle between lines 1 and 2
- theta = angleBetweenLines(*line_it1, *line_it2);
-
- //Check angle threshold
- if ( theta < _alg_params.concatenate_angle_ut_ ) //fabs(theta) not required since theta>0
- {
- //compute a new line with all involved points
- Line new_line;
- fitLine(points.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line);
-
- //check if error below a threshold to effectively concatenate
- if ( new_line.error_ < _params.range_std_dev_*_alg_params.k_sigmas_ut_ )
- {
- //update line1 as the concatenation of l1 and l2
- new_line.first_ = line_it1->first_;
- new_line.last_ = line_it2->last_;
- new_line.point_first_ = line_it1->point_first_;
- new_line.point_last_ = line_it2->point_last_;
- new_line.np_ = new_line.last_ - new_line.first_ + 1;
-
- //assign new values to line 1. Remove line 2
- *line_it1 = new_line;
- line_it2 = _line_list.erase(line_it2);//iterator line_it2 points to the next line of the list
- //Update iterator to last element
- last_it = _line_list.end();
- last_it--;
- //std::cout << "lines concatenated " << _line_list.size() << std::endl;
- }
- else
- line_it2++;
- }
- else
- line_it2++;
-
- if (line_it2 == _line_list.end()) //no more lines to be checked with line 1, next line 1 (and 2 is the next one)
- {
- //std::cout << "no more lines to be checked with line 1, next line 1 (and 2 is the next one) " << _line_list.size() << std::endl;
- line_it1++;
- line_it2 = line_it1;
- line_it2++;
- }
- }
- else // lines not close enought or jumps between, next line 1 (and 2 is the next one)
- {
- //std::cout << "lines not close enought, next line 1 (and 2 is the next one) " << _line_list.size() << std::endl;
- line_it1++;
- line_it2 = line_it1;
- line_it2++;
- }
- }
- //std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
-
- //STEP 4: removing outliers
- for (line_it1 = _line_list.begin(); line_it1 != _line_list.end(); line_it1++)
- {
- // Remove front and back points with high error
- while (line_it1->last_ - line_it1->first_ > 1 && pow(points.col(line_it1->first_).transpose() * line_it1->vector_,2) > 2 * line_it1->error_)
- line_it1->first_++;
-
- while (line_it1->last_ - line_it1->first_ > 1 && pow(points.col(line_it1->last_).transpose() * line_it1->vector_,2) > 2 * line_it1->error_)
- line_it1->last_--;
-
- if (line_it1->last_ - line_it1->first_ +1 < line_it1->np_) // Only if any point removed
- {
- if (line_it1->last_ - line_it1->first_ < _alg_params.min_window_points_)
- {
- //std::cout << "line too short after removing outliers!" << std::endl;
- line_it1 = _line_list.erase(line_it1);//iterator line_it points to the next line of the list
- line_it1--;
- }
- else
- {
- //std::cout << "updating line after removing " << line_it1->np_ - (line_it1->last_ - line_it1->first_ +1) << " outliers!" << std::endl;
- //update line
- Line new_line;
- new_line.first_ = line_it1->first_;
- new_line.last_ = line_it1->last_;
- new_line.point_first_ = points.col(line_it1->first_);
- new_line.point_last_ = points.col(line_it1->last_);
- new_line.np_ = new_line.last_ - new_line.first_ + 1;
- fitLine(points.block(0,new_line.first_, 3, new_line.np_), new_line);
- *line_it1 = new_line;
- }
- }
- }
-
-
- return _line_list.size();
-}
-
-
-unsigned int laserscanutils::extractLinesHough( const Eigen::MatrixXd & _points,
- const ExtractLinesHoughParams & _alg_params,
- std::list<laserscanutils::Line> & _line_list )
-{
- double theta, range;
- int kr;
- Line line;
- double xmax, xmin, ymax, ymin;
-
- //A 2D array of lists. Each cell is a (r,theta) discretization in the line parameter space.
- //Each list keeps the laser point coordinates that support that cell
- std::vector<std::vector<std::list<std::pair<double,double> > > > hough_grid; //TODO: to be created/sized outside and required as a param
- std::list<std::pair<double,double> >::iterator pt_it; //iterator over the points of a given cell list
-
- //resize hough_grid according range and theta steps and bounds
- unsigned int hough_grid_rows = (unsigned int)ceil(M_PI/_alg_params.theta_step_);//[0,PI]
- unsigned int hough_grid_rows_half = (unsigned int)ceil(0.5*M_PI/_alg_params.theta_step_);//middle row index
- unsigned int hough_grid_cols = (unsigned int)ceil(2*_alg_params.range_max_/_alg_params.range_step_);//[-rmax,+rmax]
- unsigned int hough_grid_cols_half = (unsigned int)ceil(_alg_params.range_max_/_alg_params.range_step_);//middle col index
-
- //clear line list
- //_line_list.clear(); //TODO: TO BE DONE OUTSIDE!!
-
- //Set grid dimensions
- hough_grid.resize(hough_grid_rows);
- for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
- {
- hough_grid[ii].resize(hough_grid_cols);
- }
-
- //For each input point, register all the supports to the hough_grid
-// for (unsigned int laser_id = 0; laser_id < _points.size(); laser_id++) //loop on the laser devices
-// {
- //for (unsigned int ipt = 0; ipt < _points.at(laser_id).cols(); ipt++) //loop over all points of laser_id
- for (unsigned int ipt = 0; ipt < _points.cols(); ipt++) //loop over all points
- {
- for (unsigned int jth = 0; jth < hough_grid_rows; jth++) //loop over all theta values in the grid
- {
- //compute Real values of theta and range
- theta = jth*_alg_params.theta_step_;
- range = _points(0,ipt)*cos(theta) + _points(1,ipt)*sin(theta); //r=xcos(th)+ysin(th)
-
- //discretize range
- kr = (int)floor(range/_alg_params.range_step_) + (int)hough_grid_cols_half ;
-
- //check validity of the discretized values
- if( ( kr >= 0 ) && ( kr < hough_grid_cols ) )
- {
- //Add support to cell(jth,kr), by pushing back the point coordinates
- //hough_grid.at(jth).at(kr).push_back( std::pair<double,double>(_points.at(laser_id)(0,ipt),_points.at(laser_id)(1,ipt)) );
- hough_grid.at(jth).at(kr).push_back( std::pair<double,double>(_points(0,ipt),_points(1,ipt)) );
- }
- }
- }
-// }
-
- //Find cells having a peak of at least min_supports_ points supporting them
- for (unsigned int ii = 1; ii < hough_grid_rows-1; ii++)
- {
- for (unsigned int jj = 1; jj < hough_grid_cols-1; jj++)
- {
- //check min supports
- unsigned int cell_supports = hough_grid.at(ii).at(jj).size();
- if( cell_supports >= _alg_params.min_supports_ )
- {
- //check if cell ii,jj is a peak ( 8 neighboring cells should be below in number of supports)
- if ( ( cell_supports >= hough_grid.at(ii-1).at(jj-1).size() ) &&
- ( cell_supports >= hough_grid.at(ii-1).at(jj).size() ) &&
- ( cell_supports >= hough_grid.at(ii-1).at(jj+1).size() ) &&
- ( cell_supports >= hough_grid.at(ii).at(jj-1).size() ) &&
- ( cell_supports >= hough_grid.at(ii).at(jj+1).size() ) &&
- ( cell_supports >= hough_grid.at(ii+1).at(jj-1).size() ) &&
- ( cell_supports >= hough_grid.at(ii+1).at(jj).size() ) &&
- ( cell_supports >= hough_grid.at(ii+1).at(jj+1).size() ) )
- {
- //find best fitting line with the supporting points
- //TODO //run over supporters indexes of the ii,jj cell, and build a Eigen::Matrix3s
- //Eigen::Matrix3s supporters;
- //supporters.resize(3,...)
- //supporters << fill with point data
- //call fitLine(supporters, line);
-
- //set the line hough params TODO: Compute them from fitLine result, not from the Grid !!
- line.np_ = hough_grid.at(ii).at(jj).size(); //supporters
- line.theta_ = ii*_alg_params.theta_step_; //theta
- line.range_ = jj*_alg_params.range_step_; //range
-
- //set starting and ending points of the line TODO: Set the projection of min max points to the fitLine line
- xmax=-100; xmin=100; ymax=-100; ymin=100;
- for (pt_it = hough_grid.at(ii).at(jj).begin(); pt_it != hough_grid.at(ii).at(jj).end(); pt_it++)
- {
- //find xmax, xmin, ymax, ymin
- if (pt_it->first > xmax) xmax = pt_it->first;
- if (pt_it->second > ymax) ymax = pt_it->second;
- if (pt_it->first < xmin) xmin = pt_it->first;
- if (pt_it->second < ymin) ymin = pt_it->second;
- }
- if (ii < hough_grid_rows_half) //first and third quartile of r-theta plane
- {
- line.point_first_ << xmin,ymax,1;
- line.point_last_ << xmax,ymin,1;
-
- }
- else //second and fourth quartile of r-theta plane
- {
- line.point_first_ << xmin,ymin,1;
- line.point_last_ << xmax,ymax,1;
- }
-
- //push back the line to the list
- _line_list.push_back(line);
- }
- }
- }
- }
-
- //return the number of lines detected
- return _line_list.size();
-}
-
-
-laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2)
-{
- // lines should be normalized
- assert(abs(line1.vector_.head(2).squaredNorm() - 1) < 1e-9 && "line 1 is not normalized!");
- assert(abs(line2.vector_.head(2).squaredNorm() - 1) < 1e-9 && "line 2 is not normalized!");
-
- //compute angle, result in [0,PI]
- ScalarT theta = acos( (line1.vector_.head(2)).dot(line2.vector_.head(2)) );
-
- //returns the angle lower PI/2 of the crossing lines
- if (theta > M_PI/2) theta = M_PI - theta;
-
- //theta is in [0,PI/2]
- return theta;
-}
-
-unsigned int laserscanutils::windowPoints(const laserscanutils::ScanParams & _params,
- const ExtractLineParams & _alg_params,
- const std::vector<float> & _ranges,
- unsigned int& ii)
-{
- if (ii < _ranges.size() && _ranges[ii] !=0)
- return std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_params.angle_step_ * _ranges[ii])));
- else
- return _alg_params.min_window_points_;
-}
diff --git a/src/deprecated/line_detector.h b/src/deprecated/line_detector.h
deleted file mode 100644
index c81cade95d3ac73f8612bed699cf86633f5beccd..0000000000000000000000000000000000000000
--- a/src/deprecated/line_detector.h
+++ /dev/null
@@ -1,132 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#ifndef LINE_DETECTOR_H_
-#define LINE_DETECTOR_H_
-
-//laserscanutils
-#include "laser_scan_utils.h"
-#include "entities.h"
-#include "scan_basics.h"
-
-namespace laserscanutils
-{
- /** \brief Set of tunning parameters
- *
- * Set of tunning parameters for line extraction
- *
- */
- struct ExtractLineParams //TODO rename to ExtractLinesParams
- {
- //members
- ScalarT jump_dist_ut_; //Upper threshold in consecutive ranges to consider a jump
- ScalarT jump_angle_ut_; //Upper threshold in angle of two consecutive ranges to consider a jump
- ScalarT window_length_; // length (m) of the window of points to fit lines in the first pass
- unsigned int min_window_points_; // minimum number of points to fit lines in the first pass
- ScalarT k_sigmas_ut_;//Uppet threshold of how many std_dev are tolerated to count that a point is supporting a line
- unsigned int concatenate_ii_ut_;//Upper threshold for ray index difference between consecutive lines to consider concatenation
- ScalarT concatenate_angle_ut_; //Upper threshold for angle between consecutive lines to consider concatenation
-
- //just a print method
- void print() const;
- };
-
- /** \brief set of tunning parameters for the Hough transform line detection
- *
- * set of tunning parameters for the Hough transform line detection
- *
- **/
- struct ExtractLinesHoughParams
- {
- double range_max_; //maximum allowed range for lines
- double range_step_; //range step in the voting grid
- double theta_step_; //theta step in the voting grid
- unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line
- };
-
- /** \brief Find the best fittig line given a set of points
- *
- * Find the best fittig line given a set of points
- *
- * \Requires:
- * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
- *
- * \Provides:
- * \param _line: a laserscanutils::Line object of the best fitting line in the Least Squares sense
- *
- **/
- void fitLine(const Eigen::MatrixXs & _points, Line & _line);
-
- /** \brief Extract lines from a given scan. Result as a list of Line
- *
- * Extract lines from a given scan.
- * Returns Lines as a list of laserscanutils::Line
- * \param _params are the laser scan parameters
- *
- * \param _ranges raw ranges data
- * \param _line_list output lines extracted from the scan
- * \return Number of lines extracted.
- *
- */
- unsigned int extractLines(const laserscanutils::ScanParams & _params,
- const ExtractLineParams & _alg_params,
- const std::vector<float> & _ranges,
- std::list<laserscanutils::Line> & _line_list);
-
- /** \brief Extract lines using Hough transform. Result as a list of Line's
- *
- * Extract lines from a set of scans.
- * Returns Lines as a std::list<laserscanutils::Line>
- *
- * \Requires:
- * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
- * \param _alg_params Hough transform parameters
- *
- * \Provides:
- * \param _line_list set of lines extracted from _points
- * \return Number of lines extracted.
- *
- *
- */
- unsigned int extractLinesHough( const Eigen::MatrixXd & _points,
- const ExtractLinesHoughParams & _alg_params,
- std::list<laserscanutils::Line> & _line_list);
-
-
- /** \brief Returns the angle between two lines
- *
- * Returns the angle between two lines
- *
- */
- ScalarT angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2);
-
- /** \brief Returns the window points
- *
- * Returns the number of points in window depending on the ranges
- *
- */
- unsigned int windowPoints(const laserscanutils::ScanParams & _params,
- const ExtractLineParams & _alg_params,
- const std::vector<float> & _ranges,
- unsigned int& ii);
-}
-#endif
diff --git a/src/deprecated/object_detector.cpp b/src/deprecated/object_detector.cpp
deleted file mode 100644
index ba9ba79c79d2d88d91bc26e84ecc6c4d815e7eac..0000000000000000000000000000000000000000
--- a/src/deprecated/object_detector.cpp
+++ /dev/null
@@ -1,1054 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "object_detector.h"
-
-
-#include <fstream> // std::ofstream
-
-
-
-
-void laserscanutils::LaserObjectDetAlgParams::print() const
-{
- std::cout << "---------------------------------------------------" << std::endl
- << "-- Laser Object Detection Algorithm Parameters --" << std::endl
- << "---------------------------------------------------" << std::endl
- << " * Maximum Distance Between Points: " << this->cluster_jump_thr_ << std::endl
- << " * Minimum Number of Points for a Cluster: " << this->cluster_min_num_points_ << std::endl
- << " * Maximum Number of Points for a Cluster: " << this->cluster_max_num_points_ << std::endl
- << " * Segmnet Window Size: " << this->fit_line_window_size_ << std::endl
- << " * K_sigmas to tolerate std_dev in line: " << this->fit_line_k_sigmas_ << std::endl
- << " * theta max to concatenate segments: " << this->fit_line_theta_max_parallel_ << std::endl;
-
-}
-
-
-
-//****************************************************
-//********** PRINT TO MATLAB TESTS
-//****************************************************
-void laserscanutils::exportStaticScanProcessed2Matlab(const char * _file_name,
- const LaserObjectDetAlgParams &_alg_params,
- const LaserScanProcessed &_scan_processed)
-{
- std::ofstream fileMatlab;
- fileMatlab.open (_file_name, std::ofstream::out); // | std::ofstream::app);
-
- // FILE HEADER
- fileMatlab << "% ***** Autogenerated File - vvg ***** \n\n";
- fileMatlab << " clc,\n clear all,\n close all \n\n\n";
-
- // Algorithm Params
- fileMatlab << "% Algorithm Params \n";
- fileMatlab << "alg_detection_params = struct("
- " 'cluster_jump_thr_', " << _alg_params.cluster_jump_thr_ << ","
- " 'cluster_max_num_points_', " << _alg_params.cluster_max_num_points_ << ","
- " 'cluster_min_num_points_', " << _alg_params.cluster_min_num_points_ << ","
- " 'corner_theta_min_', " << _alg_params.corner_theta_min_ << ","
- "'fit_line_k_sigmas_', " << _alg_params.fit_line_k_sigmas_ << ","
- "'fit_line_theta_max_parallel_', " << _alg_params.fit_line_theta_max_parallel_ << ","
- "'fit_line_window_size_', " << _alg_params.fit_line_window_size_ << ","
- "'tf_scan_window_size_', " << _alg_params.tf_scan_window_size_ << "); \n\n";
-
- // Laser Scan Processed skeleton
- fileMatlab << "% Laser Scan Processed skeleton \n" ;
- fileMatlab << "scan_processed = cell([1]); \n\n";
-// fileMatlab << "scan_processed = struct(
-// "'name_', " << _scan_processed.cluster_indxs_); \n\n";
-
-//// // Global Odometry skeleton
-//// fileMatlab << "% Global Odometry skeleton\n" ;
-//// fileMatlab << "global_odom = struct([]); \n\n";
-
-
-// // Object List skeleton
-// fileMatlab << "% Object List skeleton \n" ;
-// fileMatlab << "object_list = struct('lineList',[]);\n";
-// fileMatlab << "object_list.lines = cell([1]);\n\n";
-// //fileMatlab << "object_list.lineList = struct([]);\n\n";
-
-
-// // Scan Matrix Homogeneous Coordinates
-// fileMatlab << "% Scan Matrix In Homogeneous Coordinates \n" ;
-// fileMatlab << "scanMatrixHomo = struct([]);\n\n";
-
-// // Rotation Matrix
-// fileMatlab << "% Rotation Matrix \n" ;
-// fileMatlab << "alfa = pi/2; \n";
-// fileMatlab << "rotateM = [cos(alfa), -sin(alfa); sin(alfa), -cos(alfa)]; \n";
-
-
- fileMatlab.close();
-
-}
-
-
-void laserscanutils::exportDataScanProcessed2Matlab(std::string &_file_name,
- LaserScanProcessed & _scan_processed)
-{
- std::ofstream fileMatlab;
-
- std::string new_name;
- int division = _scan_processed.id_/100;
- new_name = _file_name + std::to_string(division);
- new_name += ".m";
- fileMatlab.open (new_name, std::ofstream::out | std::ofstream::app);
-
-
- // FILE HEADER
- fileMatlab << "% ***** Autogenerated File 2 - vvg ***** \n\n";
- //fileMatlab << " clc,\n clear all,\n close all \n\n\n";
-
-
- // Laser Scan Processed
- fileMatlab << "% Laser Scan Processed Variables \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.id_ = " << _scan_processed.id_ << "; \n\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.scan = [";
- for (unsigned int row = 0; row < 3; row++)
- {
- for (unsigned int col = 0; col < _scan_processed.filtered_total_points_; col++)
- {
- fileMatlab << _scan_processed.scan_points_matrix_(row,col) << " ";
- }
- fileMatlab << ";\n";
- }
- fileMatlab << "]; \n\n";
-
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.cluster_indxs_ = [";
- for (unsigned int cluster = 0; cluster < _scan_processed.cluster_indxs_.size(); cluster++)
- {
- std::pair<int,int> tempCluster = _scan_processed.cluster_indxs_[cluster];
- fileMatlab << tempCluster.first << "; " << tempCluster.second << "; ";
- }
- fileMatlab << "]; \n\n";
-
- // Objects
- unsigned int object_count = 0;
- for (auto& temp_object : _scan_processed.object_list_)
- {
- object_count += 1;
- // Object
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.tracked = "
- << temp_object.id_ << ";\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.num_points = "
- << temp_object.num_points_ << ";\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.first_idx = "
- << temp_object.first_ << ";\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.last_idx = "
- << temp_object.last_ << ";\n";
-
- unsigned int line_counter = 0;
- for (auto& temp_line : temp_object.line_list_)
- {
- line_counter += 1;
- // lines of the object
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.line_list{";
- fileMatlab << line_counter << "}.vector = ["
- << temp_line.vector_(0) << "," << temp_line.vector_(1) << "," << temp_line.vector_(2) << "]; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.line_list{";
- fileMatlab << line_counter << "}.point_first = ["
- << temp_line.point_first_(0) << "," << temp_line.point_first_(1) << "," << temp_line.point_first_(2) << "]; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.line_list{";
- fileMatlab << line_counter << "}.point_last = ["
- << temp_line.point_last_(0) << "," << temp_line.point_last_(1) << "," << temp_line.point_last_(2) << "]; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.line_list{";
- fileMatlab << line_counter << "}.error = " << temp_line.error_ << ";\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.line_list{";
- fileMatlab << line_counter << "}.first_idx = " << temp_line.first_ << ";\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.line_list{";
- fileMatlab << line_counter << "}.last_idx = " << temp_line.last_ << ";\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.line_list{";
- fileMatlab << line_counter << "}.num_points = " << temp_line.np_ << ";\n";
-
- fileMatlab << "% Summary of Lines in Object. Each Row is a line containing: \n";
- fileMatlab << "% || object_count | pt_first_(0) | pt_first_(1) | pt_first_(2) | pt_last_(0) | pt_last_(1) | pt_last_(2) ...\n";
- fileMatlab << "% |vector(0) | vector(1) | vector(2) | error | init idx | last idx ||\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.lines_sum(" << object_count << ",:) = [" << object_count << ", "
- << temp_line.point_first_(0) << "," << temp_line.point_first_(1) << "," << temp_line.point_first_(2) << ", "
- << temp_line.point_last_(0) << "," << temp_line.point_last_(1) << "," << temp_line.point_last_(2) << ", "
- << temp_line.vector_(0) << "," << temp_line.vector_(1) << "," << temp_line.vector_(2) << ", "
- << temp_line.error_ << ", " << temp_line.first_ << ", " << temp_line.last_ << ", " << temp_line.np_ << "]; \n";
-
- }
-
- unsigned int corner_count = 0;
- for (auto& temp_corner : temp_object.corner_list_)
- {
- corner_count += 1;
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.corner_list{";
- fileMatlab << corner_count << "}.point = ["
- << temp_corner.pt_(0) << "," << temp_corner.pt_(1) << "," << temp_corner.pt_(2) << "]; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.corner_list{";
- fileMatlab << corner_count << "}.orientation = " << temp_corner.orientation_ << ";\n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.object_list{" << object_count << "}.corner_list{";
- fileMatlab << corner_count << "}.aperture = " << temp_corner.aperture_ << ";\n";
-
- fileMatlab << "% Summary of Corners in Object. Each Row is a corner containing: \n";
- fileMatlab << "% ||object_count | point_(0) | point_(1) | point_(2) | orientation | aperture || \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.corners_sum(" << object_count << ",:) = [" << object_count << ", "
- << temp_corner.pt_(0) << "," << temp_corner.pt_(1) << "," << temp_corner.pt_(2) << ","
- << temp_corner.orientation_ << ", " << temp_corner.aperture_ << "]; \n";
-
- }
- }
-
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.scan_total_points_ = " << _scan_processed.scan_total_points_ << "; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.filtered_total_points_ = " << _scan_processed.filtered_total_points_ << "; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.number_of_clusters_ = " << _scan_processed.number_of_clusters_ << "; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.num_points_in_clusters_ = "<< _scan_processed.num_points_in_clusters_ << "; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.number_of_valid_clusters_ = " << _scan_processed.number_of_valid_clusters_ << "; \n";
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.number_of_global_scan_ = " << _scan_processed.number_of_global_scan_ << "; \n";
-
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.odom_pos_ = ["
- << _scan_processed.odom_pos_[0] << ", " << _scan_processed.odom_pos_[1] << ", " << _scan_processed.odom_pos_[2] << "]; \n ";
-
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.odom_ori_ = ["
- << _scan_processed.odom_ori_[0] << ", " << _scan_processed.odom_ori_[1] << ", "
- << _scan_processed.odom_ori_[2] << ", " << _scan_processed.odom_ori_[3] << "]; \n ";
-
- fileMatlab << "scan_processed{" << _scan_processed.id_ << "}.odom_tf = [" << _scan_processed.odom_eigen_tf_ << "];\n\n";
-
-
-
-
- fileMatlab.close();
-}
-
-
-
-void laserscanutils::printAssociationResults(std::vector<std::vector<laserscanutils::AssociationResult> > & _asso_results)
-{
-
- std::cout << std::fixed;
- std::cout << std::setprecision(2);
- std::cout << std::endl << "Association Matrix Result "<< std::endl << "\t [corners | centroids | line_init | line_end ]= : " << std::endl;
-
- for (int i = 0; i<_asso_results.size(); i++)
- {
- for (int j = 0; j< _asso_results[i].size(); j++)
- {
- std::cout << "[ " << _asso_results[i][j].corner_ << "," << _asso_results[i][j].centroid_ << ","
- << _asso_results[i][j].line_init_ << "," << _asso_results[i][j].line_end_ << " ] \t";
- }
- std::cout << std::endl;
- }
- std::cout << std::endl;
-}
-
-
-
-
-//****************************************************
-//********** preFilterScan
-//****************************************************
-void laserscanutils::preFilterScan2homog(const laserscanutils::ScanParams & _params,
- const std::vector<float> & _ranges,
- Eigen::MatrixXs & _points,
- LaserScanProcessed &_scan_processed)
-{
- //std::cout << "[PreFiltering SCAN... ";
-
- unsigned int matPosition = 0;
- float act_range = -1;
- float act_theta = 0.0;
-
- for(unsigned int i=0; i<_ranges.size(); ++i)
- {
- act_range = _ranges[i];
-
- if ( (act_range >=_params.range_min_) && (act_range <= _params.range_max_) && !(std::isnan(act_range)) && !(std::isinf(act_range)) )
- {
- act_theta = _params.angle_min_+ _params.angle_step_*(i);
-
- _points.col(matPosition) << act_range*cos(act_theta), act_range*sin(act_theta), 1;
-
- matPosition++;
- }
- else
- {
- //std::cout << "Invalid Point: " << scan.ranges[i] << " - index: " << i << std::endl;
- }
-
- }
-
- // Filling stats
- _scan_processed.scan_total_points_ = _ranges.size();
- _scan_processed.filtered_total_points_ = matPosition;
-
-
- // Conservative resize of the eigen Matrix
- _points.conservativeResize(3, matPosition);
-
-
-
-// // DEBUG:
-// std::cout << std::endl << "[PRE-FILTERING]: TotalPoints: " << _scan_processed.scan_total_points_ << std::endl;
-// std::cout << "[PRE-FILTERING]: filtered_total_points_: " << _scan_processed.filtered_total_points_ << std::endl;
-// std::cout << "[PRE-FILTERING]: matrixResize: " << _points.cols() << std::endl;
-
- // std::cout << "..DONE] " << std::endl;
-}
-
-
-
-//****************************************************
-//********** extractClustersInScan
-//****************************************************
-void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetAlgParams & _alg_params,
- const Eigen::MatrixXs & _points,
- unsigned int _global_scan_number,
- laserscanutils::LaserScanProcessed & _scan_processed
- )
-{
-
- // std::cout << "Extracting Clusters... " ;
- _scan_processed.cluster_indxs_.clear();
- _scan_processed.object_list_.clear(); // Assure the object list for the scan is empty.
-
- std::pair<int,int> seg; // saves the first and last point for each segment
- laserscanutils::Object temp_object; // objects detected initialization.
-
- float points_distance = 0.0;
- int count_num_cluster = 0;
- int count_num_valid_cluster = 0;
-
- // Initial Segment
- seg.first = 0;
- float sq_threshold = pow(_alg_params.cluster_jump_thr_,2.0);
-
- for (unsigned int col = 1; col < _scan_processed.filtered_total_points_; col++)
- {
- points_distance = ( pow( ( _points(0,col) -_points(0,col-1) ), 2.0 ) + pow( ( _points(1,col) - _points(1,col-1) ), 2.0) );
-
- if ( points_distance <= sq_threshold) // same segment points
- {
- // Same segment
-
- // Calculating extrem Points -> Used for those unestructured objects (with no lines found) when finding the bounding Box
- if(_points(0,col-1) < temp_object.min_x_ )
- temp_object.min_x_ = _points(0,col-1);
- if(_points(1,col-1) < temp_object.min_y_ )
- temp_object.min_y_ = _points(1,col-1);
-
- if(_points(0,col-1) > temp_object.max_x_ )
- temp_object.max_x_ = _points(0,col-1);
- if(_points(1,col-1) > temp_object.max_y_ )
- temp_object.max_y_ = _points(1,col-1);
- }
-
- else
- {
- // new segment
-
- seg.second = col-1; // end of the previous segment
-
- // check length of the cluster
- if ( ( (seg.second - seg.first)+1 >= _alg_params.cluster_min_num_points_ ) &&
- ( (seg.second - seg.first)+1 <= _alg_params.cluster_max_num_points_) )
- {
- _scan_processed.cluster_indxs_.push_back(seg);
-
- // fulfill and add object
- temp_object.first_ = seg.first;
- temp_object.last_ = seg.second;
- temp_object.num_points_ = seg.second - seg.first +1;
-
- _scan_processed.object_list_.push_back(temp_object);
- _scan_processed.num_points_in_clusters_ += temp_object.num_points_;
-
- count_num_valid_cluster++; // count cluster as valid
-
- } // TODO OPT: else: save the invalid cluster in other vector of bad clusters to further analysis
-
-
- count_num_cluster++; // count any cluster
-
- // reset min max point values:
- temp_object.max_x_ = -1000.0;
- temp_object.max_y_ = -1000.0;
- temp_object.min_x_ = 1000.0;
- temp_object.min_y_ = 1000.0;
-
- // Initialize next segment (cluster)
- seg.first = col;
- }
-
- }
-
-
- // LAST POINT close last segment (REPEATS LAST STEP)
- seg.second = _scan_processed.filtered_total_points_-1;
- if ( ((seg.second - seg.first)+1 >= _alg_params.cluster_min_num_points_) && ((seg.second - seg.first)+1 <= _alg_params.cluster_max_num_points_) )
- {
- _scan_processed.cluster_indxs_.push_back(seg);
-
- // fulfill and add object
- temp_object.first_ = seg.first;
- temp_object.last_ = seg.second;
- temp_object.num_points_ = seg.second - seg.first +1;
- _scan_processed.object_list_.push_back(temp_object);
- _scan_processed.num_points_in_clusters_ += temp_object.num_points_;
-
- // reset min max point values:
- temp_object.max_x_ = -1000.0;
- temp_object.max_y_ = -1000.0;
- temp_object.min_x_ = 1000.0;
- temp_object.min_y_ = 1000.0;
-
- count_num_valid_cluster++; // count cluster as valid
-
- }
- count_num_cluster++; // count any cluster
-
-
- // Stats
- _scan_processed.number_of_clusters_ = count_num_cluster;
- _scan_processed.number_of_valid_clusters_ = count_num_valid_cluster;
- _scan_processed.number_of_global_scan_ = _global_scan_number;
-
-
-
-// *** [DEBUG] ****
-
-// // DEBUG printing clusters indexes
-// std::cout << " = " ;
-// for (auto it_clusters : _scan_processed.cluster_indxs_ )
-// {
-// std::cout << "[" << it_clusters.first << ", " << it_clusters.second << "] - ";
-// }
-
-// std::cout << "Done!" << std::endl;
-
-}
-
-
-
-
-
-//****************************************************
-//********** findLinesInCluster
-//****************************************************
-void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan_params,
- const laserscanutils::LaserObjectDetAlgParams & _alg_params,
- const Eigen::MatrixXs & _cluster,
- std::list<laserscanutils::Line>& _line_list)
-{
- laserscanutils::Line line;
- laserscanutils::Line new_line;
- std::list<laserscanutils::Line>::iterator line_it1, line_it2;
- ScalarT theta; // for angle between lines
-
- // cleaning line_list
- _line_list.clear();
-
- //init random generator -> TODO: Eliminate (this is from previous version of library)
- std::default_random_engine generator(1);
- std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.fit_line_window_size_);
-
- unsigned int ii = _alg_params.fit_line_window_size_-1;
- bool last = false;
-
- while (last != true)
- {
- if (ii >= _cluster.cols())
- {
- ii = _cluster.cols()-1;
- last = true;
- }
- unsigned int i_from = ii - _alg_params.fit_line_window_size_ + 1;
-
- //std::cout << "Fitting Line: from " << i_from << " to " << ii << "...";
- fitLine(_cluster.block(0,i_from, 3, _alg_params.fit_line_window_size_), line);
-
- //if error below stdev, add line to result set
- if ( line.error_ < std::pow(_scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_,2) )
- {
- //std::cout << "=> YES!" << std::endl;
- line.first_ = i_from;
- line.last_ = ii;
- line.point_first_ = _cluster.col(line.first_);
- line.point_last_ = _cluster.col(line.last_);
- line.np_ = line.last_ - line.first_ + 1;
-
- _line_list.push_back(line);
- }
- else
- {
- //std::cout << " NO! - Error = " << line.error_ << " > " << _scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_ << std::endl;
- }
- ii = ii+rand_window_overlapping(generator);
- }
-
- std::cout << "Lines fitted: " << _line_list.size() ;
-
-
- // concatenating lines
- line_it1 = _line_list.begin();
-
- while ( line_it1 != _line_list.end() )
- {
- line_it2 = line_it1;
- line_it2 ++;
- while (line_it2 != _line_list.end())
- {
- //compute angle between lines 1 and 2
- theta = laserscanutils::angleBetweenLines(*line_it1, *line_it2);
-
- //Check angle threshold
- if ( theta < _alg_params.fit_line_theta_max_parallel_) //fabs(theta) not required since theta>0
- {
- //compute a new line with all involved points
- fitLine(_cluster.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line);
-
- //check if error below a threshold to effectively concatenate
- if ( new_line.error_ < std::pow(_scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_,2) )
- {
- //std::cout << " => lines concatenated";
-
- //update line1 as the concatenation of l1 and l2
- new_line.first_ = line_it1->first_;
- new_line.last_ = line_it2->last_;
- new_line.point_first_ = line_it1->point_first_;
- new_line.point_last_ = line_it2->point_last_;
- new_line.np_ = new_line.last_ - new_line.first_ + 1;
- *line_it1 = new_line;
-
- //remove l2 from the list
- line_it2 = _line_list.erase(line_it2);
-
- //std::cout << " - line resultant: from "<< new_line.first_ << " to " << new_line.last_ << std::endl;
- //<< new_line.vector_.transpose() << " ERROR: " << new_line.error_ << std::endl << std::endl;
-
- }
- else
- {
- //std::cout << " => lines NOT concatenated";
-
- line_it2++;
- }
- }
- else
- {
- line_it2++;
- }
-
- } // end while it 2
-
- // Eliminating intermediate lines
- std::list<Line>::iterator temp_line_it = line_it1;
- line_it1++;
- while ( (temp_line_it->last_ >= line_it1->first_) && (line_it1 != _line_list.end()))
- {
- line_it1 = _line_list.erase(line_it1);
- }
-
-
- } // end while it 1
-
-
- // DEBUG TEST
-// // globalizing first and last point (in terms of the whole scan)
-// for (auto list_it1 = _line_list.begin(); list_it1 != _line_list.end(); list_it1++)
-// {
-// //std::cout << " INSIDE FIND LINES #." << std::distance(_line_list.begin(),list_it1) << std::endl;
-// list_it1->first_ += initIndex;
-// list_it1->last_ += initIndex;
-// //list_it1->print();
-// //std::cout << std::endl << std::endl;
-// }
-
-
- // DEBUG TEST
-// for (auto list_it1 = _line_list.begin(); list_it1 != _line_list.end(); list_it1++)
-// {
-// list_it1->print();
-// std::cout << std::endl;
-// }
-
-
-
- std::cout << " --> Final Lines In Object: " << _line_list.size() << std::endl << std::endl;
-
-}
-
-
-
-
-
-//****************************************************
-//********** findCornersInClusterLines
-//****************************************************
-unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::LaserObjectDetAlgParams & _obj_det_alg_params,
- const std::list<laserscanutils::Line> & _line_list,
- std::list<laserscanutils::Corner> & _corner_list)
-{
- //local variables
- ScalarT theta;
- Corner corner;
- std::list<Line>::const_iterator line_it1, line_it2;
-
- _corner_list.clear();
-
- line_it1 = _line_list.begin();
-
- if (_line_list.size() <2)
- {
- return 0; // Not enough lines for a normal corner:
- // TODO: Corner as init or final point of line. -> As done in association
- }
-
- auto last_it = _line_list.end();
- last_it --;
-
- while ( line_it1 != last_it)
- {
-
- line_it2 = line_it1;
- line_it2 ++;
-
- //compute angle between lines 1 and 2
- theta = angleBetweenLines(*line_it1, *line_it2);
-
- //Check angle threshold
- if ( theta > _obj_det_alg_params.corner_theta_min_)
- {
- // Find the corner as the cross product between lines (intersection of lines in homog)
- corner.pt_ = (line_it1->vector_).cross(line_it2->vector_);
-
- // normalize corner point to set last component to 1
- corner.pt_ = corner.pt_ / corner.pt_(2);
-
- //vector from corner to first point of l1
- Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
-
- //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
- corner.orientation_ = atan2(v1(1),v1(0));
-
- //Compute corner aperture with line_it1->first, corner and line_it2->last
- corner.aperture_ = cornerAperture(line_it1->point_first_, corner.pt_, line_it2->point_last_);
-
- //set other descriptors
- corner.line_1_ = *line_it1;
- corner.line_2_ = *line_it2;
-
- //add the corner to the list
- _corner_list.push_back(corner);
- }
- else
- //std::cout << " ... No Corner! : " << std::endl;
-
- line_it2 ++;
- line_it1 ++;
-
-
- }
- // // DEBUG
- //std::cout << " ****** CORNERS LIST SIZE: " << _corner_list.size() << std::endl;
-}
-
-
-
-
-
-//****************************************************
-//********** extractFeaturesInObject
-//****************************************************
-void laserscanutils::extractFeaturesInObject(const Eigen::MatrixXs &_points, laserscanutils::Object &_object)
-{
-
- float longest_line_size = -1;
- int longest_line_idx = -1;
-
- // Extract points corresponding to the object
- Eigen::MatrixXs points;
- points = _points.block(0,_object.first_,2,_object.num_points_);
-
- // Find longest line
- if (!_object.line_list_.empty())
- {
- longest_line_size = -1;
- longest_line_idx = -1;
- int counter = -1;
-
- for (auto line_it : _object.line_list_ )
- {
- counter++;
- if (line_it.length() > longest_line_size)
- {
- longest_line_size = line_it.length();
- longest_line_idx = counter;
- }
- }
- }
-
- if (longest_line_size > 0.7)
- {
- _object.structured_ = true;
-
- // Get the longest line
- auto longest_line_it = std::next(_object.line_list_.begin(), longest_line_idx);
- laserscanutils::Line longest_line = *longest_line_it;
-
- //Get orientation of the longest line to get object Orientation
- Eigen::Vector2s v1 = longest_line.point_first_.head(2) - longest_line.point_last_.head(2);
- _object.obj_orientation_ = std::atan2(v1(1),v1(0));
-
- // Defining Rectangle (Bounding Box) Size
- // - Rotation Matrix
- Eigen::Matrix2s tf;
- tf << std::cos(_object.obj_orientation_), -1*(std::sin(_object.obj_orientation_)),
- std::sin(_object.obj_orientation_), std::cos(_object.obj_orientation_);
-
- // - Points Transformed
- Eigen::MatrixXs pointsRot;
- pointsRot = tf.inverse()*points;
-
- // - Max and Min coordinates
- Eigen::MatrixXf::Index maxRowX, maxColX, minRowX, minColX, maxRowY, maxColY, minRowY, minColY;
- auto MaxX = pointsRot.row(0).maxCoeff(&maxRowX, &maxColX);
- auto MinX = pointsRot.row(0).minCoeff(&minRowX, &minColX);
- auto MaxY = pointsRot.row(1).maxCoeff(&maxRowY, &maxColY);
- auto MinY = pointsRot.row(1).minCoeff(&minRowY, &minColY);
-
- // - Object Rectangle definition
- _object.size_x_ = std::abs(MaxX - MinX);
- _object.size_y_ = std::abs(MaxY - MinY);
-
- _object.object_center_(0) = (MaxX - MinX)/2 + MinX;
- _object.object_center_(1) = (MaxY - MinY)/2 + MinY;
- _object.object_center_ = tf*_object.object_center_; // de-rotation the center
-
- // Defining Rectangle Polyline
- // - Rotate rectangle corners
- _object.polyline_points_.resize(2,5);
- _object.polyline_points_ << MaxX, MinX, MinX, MaxX, MaxX,
- MinY, MinY, MaxY, MaxY, MinY;
- _object.polyline_points_ = tf*_object.polyline_points_;
-
- }
-
-
- else // there are no lines OR line size is not enough -> give rectangle of unstructured Object
- {
- _object.structured_ = false;
- // Find x/y_min/max
- Eigen::MatrixXf::Index maxRowX, maxColX, minRowX, minColX, maxRowY, maxColY, minRowY, minColY;
- auto MaxX = points.row(0).maxCoeff(&maxRowX, &maxColX);
- auto MinX = points.row(0).minCoeff(&minRowX, &minColX);
- auto MaxY = points.row(1).maxCoeff(&maxRowY, &maxColY);
- auto MinY = points.row(1).minCoeff(&minRowY, &minColY);
-
- // Object Rectangle definition
- _object.size_x_ = std::abs(MaxX - MinX);
- _object.size_y_ = std::abs(MaxY - MinY);
-
- // - Center
- _object.object_center_(0) = (MaxX - MinX)/2 + MinX;
- _object.object_center_(1) = (MaxY - MinY)/2 + MinY;
-
- // - Polyline
- _object.polyline_points_.resize(2,5);
- _object.polyline_points_ << MaxX, MinX, MinX, MaxX, MaxX,
- MinY, MinY, MaxY, MaxY, MinY;
-
- }
-
- // Object Centroid
- _object.ref_centroid_point_(0) = (points.row(0).sum())/_object.num_points_;
- _object.ref_centroid_point_(1) = (points.row(1).sum())/_object.num_points_;
- _object.ref_centroid_point_(2) = 1;
-
-
-
- // Initial References
- if (!_object.corner_list_.empty())
- {
- // the front corner of the object will be the reference
- _object.ref_old_point_ = _object.corner_list_.front().pt_;
- _object.ref_act_point_ = _object.corner_list_.front().pt_;
- _object.ref_type_ = 8;
- _object.ref_position_ = 0; // referenced corner position in list = 0 = front();
- }
- else
- {
- // if no corners, ref will be the centroid
- _object.ref_old_point_ = _object.ref_centroid_point_;
- _object.ref_act_point_ = _object.ref_centroid_point_;
- _object.ref_type_ = 1;
- _object.ref_position_ = -1;
- }
-
-
-
-// // DEBUG
-// _object.print();
-
-}
-
-
-
-
-
-
-
-
-//****************************************************
-//********** associateCorners
-//****************************************************
-void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_objects_list,
- std::list<laserscanutils::Object> & _act_objects_list,
- float _corner_threshold, float _angle_thr, float _line_ext_threshold)
-
-{
- int num_of_col = _prev_objects_list.size();
- int num_of_row = _act_objects_list.size();
-
-// std::cout << std::endl << "act vs prev objects = " << num_of_row << " vs " << num_of_col << std::endl;
-
- // Create the matrix to store the association results [NxMx4]
- std::vector< std::vector< laserscanutils::AssociationResult > >asso_results;
- asso_results.resize( num_of_row, std::vector< laserscanutils::AssociationResult>(num_of_col,1000));
-
- std::vector< std::vector< laserscanutils::AssociationResult > >asso_results_count;
- asso_results_count.resize( num_of_row, std::vector< laserscanutils::AssociationResult>(num_of_col,0));
-
- float sq_corner_threshold = pow(_corner_threshold, 2);
- float sq_line_threshold = pow(_line_ext_threshold, 2);
-
- unsigned int prev_obj = 0;
- unsigned int act_obj = 0;
-
- bool obj_corner_asso = false;
- bool obj_init_line_asso = false;
- bool obj_end_line_asso = false;
- bool obj_centroid_asso = false;
-
-
- for (auto&& act_obj_it : _act_objects_list)
- {
- prev_obj = 0;
- for (auto&& prev_obj_it : _prev_objects_list)
- {
- // reset flags
- obj_corner_asso = false;
- obj_init_line_asso = false;
- obj_end_line_asso = false;
- obj_centroid_asso = false;
-
-// std::cout << std::endl << "Comparing Objects " << act_obj << " - " << prev_obj;
-
- // Compare CENTROIDS
- float temp_dist_centroid = std::pow( ( act_obj_it.ref_centroid_point_(0) - prev_obj_it.ref_centroid_point_(0) ), 2.0 ) +
- pow( ( act_obj_it.ref_centroid_point_(1) - prev_obj_it.ref_centroid_point_(1) ), 2.0 ) ;
-
-// // DEBUG
-// std::cout << "DISTANCE #Centroids: " << act_obj << " - " << prev_obj << ": " << std::endl;
-// std::cout << "\t\t Centroid Distance = " << temp_dist_centroid << std::endl;
-
- // Updating Matrices Results
- asso_results[act_obj][prev_obj].centroid_ = std::min(temp_dist_centroid, asso_results[act_obj][prev_obj].centroid_);
- asso_results_count[act_obj][prev_obj].centroid_ += 1;
-
- if (temp_dist_centroid < sq_corner_threshold)
- {
- obj_centroid_asso = true;
- }
-
-
- // Compare CORNERS LIST if exist
- if ( (act_obj_it.corner_list_.size() != 0) && (prev_obj_it.corner_list_.size() != 0) )
- {
-
- unsigned int prev_pos = 0;
- unsigned int act_pos = 0;
-
- for (auto&& act_corner_it : act_obj_it.corner_list_)
- {
- prev_pos = 0;
-
- for (auto&& prev_corner_it : prev_obj_it.corner_list_)
- {
-
- float temp_dist_corner = std::pow( ( act_corner_it.pt_(0) - prev_corner_it.pt_(0) ), 2.0 ) +
- pow( ( act_corner_it.pt_(1) - prev_corner_it.pt_(1) ), 2.0) ;
-
-// // DEBUG
-// std::cout << "DISTANCE #corners: " << act_obj << "."<< act_pos << " - " << prev_obj << "." << prev_pos << ": " << std::endl;
-// std::cout << "\t\t Corner Distance = " << temp_dist_corner << std::endl;
-
- // Updating Matrices Results
- asso_results[act_obj][prev_obj].corner_ = std::min(temp_dist_corner, asso_results[act_obj][prev_obj].corner_);
- asso_results_count[act_obj][prev_obj].corner_ += 1;
-
- if (temp_dist_corner < sq_corner_threshold)
- {
- obj_corner_asso =true;
-
- // If are associated, prev_scan reference is the actual one too. NO!
-// act_obj_it.ref_act_point_ = prev_obj_it.ref_act_point;
- }
-
- prev_pos++;
- }
-
- act_pos++;
- }
-
- }
- //else
- //std::cout << " => NO CORNERS LIST";
-
-
- // Compare LINES LIST if exist
- if ( (act_obj_it.line_list_.size() != 0) && (prev_obj_it.line_list_.size() != 0) )
- {
-
- unsigned int prev_pos = 0;
- unsigned int act_pos = 0;
-
- for (auto&& act_line_it : act_obj_it.line_list_)
- {
- prev_pos = 0;
-
- for (auto&& prev_line_it : prev_obj_it.line_list_)
- {
- // LINES ASSO: ANGLE DIFF
- float temp_dist_angle = laserscanutils::angleBetweenLines(act_line_it, prev_line_it);
-
- // LINES ASSO: INIT POINT DIFF
- float temp_dist_init_pt = std::pow( ( act_line_it.point_first_(0) - prev_line_it.point_first_(0) ), 2.0 ) +
- pow( ( act_line_it.point_first_(1) - prev_line_it.point_first_(1) ), 2.0) ;
-
- // LINES ASSO: FINAL POINT DIFF
- float temp_dist_final_pt = std::pow( ( act_line_it.point_last_(0) - prev_line_it.point_last_(0) ), 2.0 ) +
- pow( ( act_line_it.point_last_(1) - prev_line_it.point_last_(1) ), 2.0) ;
-
-// // DEBUG
-// std::cout << "DISTANCE #lines: " << act_obj << "."<< act_pos << " - " << prev_obj << "." << prev_pos << ": " << std::endl;
-// std::cout << "\t\t Init Points Distance = " << temp_dist_init_pt << std::endl;
-// std::cout << "\t\t Final Points Distance = " << temp_dist_final_pt << std::endl;
-// std::cout << "\t\t ANGLE = " << temp_dist_angle << std::endl;
-
- // Updating result Matrices.
- asso_results[act_obj][prev_obj].line_init_ = std::min(temp_dist_init_pt, asso_results[act_obj][prev_obj].line_init_);
- asso_results_count[act_obj][prev_obj].line_init_ += 1;
-
- asso_results[act_obj][prev_obj].line_end_ = std::min(temp_dist_final_pt, asso_results[act_obj][prev_obj].line_end_);
- asso_results_count[act_obj][prev_obj].line_end_ += 1;
-
- if ( (temp_dist_angle < _angle_thr) && (temp_dist_init_pt < sq_line_threshold) )
- {
- obj_init_line_asso = true;
- }
-
- if ( (temp_dist_angle < _angle_thr) && (temp_dist_final_pt < sq_line_threshold) )
- {
- obj_end_line_asso = true;
- }
-
- prev_pos++;
- }
-
- act_pos++;
- }
-
- }
-
-
- // ASSOCIATION RULES:
- // Check ref_points associated and define the tracking point.
-
- if (obj_corner_asso || obj_init_line_asso || obj_end_line_asso || obj_centroid_asso)
- {
- std::cout << std::endl << "***** Association SUMMARY: *****" << std::endl;
- std::cout << "\t OBJECTS: " << act_obj << " && " << prev_obj << std::endl;
- std::cout << "***** END SUMMARY ***** " << std::endl;
-
- act_obj_it.associated_ = true;
- act_obj_it.asso_to_prev_obj_in_pos_ = prev_obj;
-
- // Check ref_point of the association => Forward propagation (prev -> actual)
- if (act_obj_it.ref_type_ <= prev_obj_it.ref_type_)
- {
- act_obj_it.ref_act_point_ = prev_obj_it.ref_act_point_;
-// std::cout << std::endl << "Change Refs:\t OLD_ = " << act_obj_it.ref_old_point_.transpose() << "("<<act_obj_it.ref_type_<<") \n\t\t NEW = "
-// << act_obj_it.ref_act_point_.transpose() << "("<<prev_obj_it.ref_type_<<") \t\t" << std::endl;
-
- act_obj_it.ref_type_ = prev_obj_it.ref_type_;
-
-
- }
- else
- {
- // Added new version for integration => Not really used
- prev_obj_it.ref_act_point_ = act_obj_it.ref_act_point_;
-// std::cout << std::endl << "Change Refs Backward:\t OLD = " << prev_obj_it.ref_old_point_.transpose() << "("<<act_obj_it.ref_type_<<") \n\t\t NEW = "
-// << act_obj_it.ref_act_point_.transpose() << "("<<prev_obj_it.ref_type_<<") \t\t" << std::endl;
-
- prev_obj_it.ref_type_= act_obj_it.ref_type_;
- }
-
-// // Propagating object features:
-// if(act_obj_it.size_x_ < prev_obj_it.size_x_)
-// {
-// act_obj_it.size_x_ = prev_obj_it.size_x_;
-// }
-
-// if(act_obj_it.size_y_ < prev_obj_it.size_y_)
-// {
-// act_obj_it.size_y_ = prev_obj_it.size_y_;
-// }
-
- }
-
-
-
- prev_obj++;
- }
- act_obj++;
-
- }
-
- // DEBUG
- //laserscanutils::printAssociationResults(asso_results);
- //laserscanutils::printAssociationResults(asso_results_count);
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/deprecated/object_detector.h b/src/deprecated/object_detector.h
deleted file mode 100644
index 67d6f94c2dd62f11a4396d02a4dbf0b132fd5bdc..0000000000000000000000000000000000000000
--- a/src/deprecated/object_detector.h
+++ /dev/null
@@ -1,270 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef OBJECT_DETECTOR_H
-#define OBJECT_DETECTOR_H
-
-//std
-#include <iostream>
-#include <vector>
-#include <list>
-#include <iomanip>
-
-// Eigen
-#include <eigen3/Eigen/Geometry>
-
-//laserscanutils
-#include "laser_scan_utils.h"
-#include "entities.h"
-#include "scan_basics.h"
-#include "line_detector.h"
-#include "corner_detector.h"
-
-
-
-
-namespace laserscanutils
-{
- struct AssociationResult
- {
- // Constructor
- AssociationResult(float _value = 100.00)
- {
- this->corner_ = _value;
- this->angle_ = _value;
- this->line_init_ = _value;
- this->line_end_ = _value;
- this->centroid_ = _value;
- }
-
- float corner_;
- float angle_;
- float line_init_;
- float line_end_;
- float centroid_;
-
- };
-
- struct LaserScanProcessed // Stores all the needed information from a scan
- {
- // Constructor
- LaserScanProcessed(unsigned int _num_points=10)
- {
- std::cout << " DEBUG INIT. LASERSCANPROCESSED: NumPoints = " << _num_points << std::endl;
- assert(_num_points < 5000 && "Error: saving scan points in laser Scan Processed. Check initialization." );
- this->scan_points_matrix_.conservativeResize(3,_num_points);
-// this->scan_points_matrix_.resize(3,_num_points);
- this->odom_pos_ = {0.0, 0.0, 0.0};
- this->odom_ori_ = {0.0, 0.0, 0.0, 0.0};
- this->num_points_in_clusters_ = 0;
- }
-
- unsigned int id_; // Processed scan identifier.
-
- // Atributes
- //TBR?// std::vector<float> scan_raw_ranges_; // [TBC] Raw scan ranges information
- Eigen::MatrixXs scan_points_matrix_; // Homogeneous Coordinates for the valid scan points.
-
- std::vector< std::pair<int,int> > cluster_indxs_; // Indexs of the initial and final position (of the scan_points_matrix_) for the clusters in scan.
- std::list<laserscanutils::Object> object_list_; // Contains the different objects detected (clusters with associate information) in a scan.
- //std::list<laserscanutils::Line> line_list_; //[TBR] Contains all the lines found in the scan (GLOBAL). -> TBR: use object lines!
-
- unsigned int scan_total_points_; //Total number of points in the raw scan
- unsigned int filtered_total_points_; //Total number of valid points after pre filter
- unsigned int number_of_clusters_; //Total number of clusters found in the scan (valid and not valid)
- unsigned int num_points_in_clusters_; // Total number of points in the clusters
- unsigned int number_of_valid_clusters_; //Total number of valid clusters found in the scan
- unsigned int number_of_global_scan_; //Total number scan till now
-
-
- std::vector<float> odom_pos_; // [X,Y,Z] position of the global odometry "nav_msg: pose.pose.position"
- std::vector<float> odom_ori_; // [X,Y,Z,W] orientation of the global odometry. "nav_msg: pose.pose.orientation"
- Eigen::Matrix3s odom_eigen_tf_; // Eigen Matrix with the corresponding TF to base
-
-
- // std::vector<int> object_asso_values_; // Stores the strength of the associated objects
-
- };
-
-
-
- struct LaserObjectDetAlgParams
- {
- //members
- float cluster_jump_thr_; // max distance between points
- float cluster_min_num_points_; // minimum number of points for a cluster
- float cluster_max_num_points_; // maximum number of points for a cluster
-
- // Line extraction
- unsigned int fit_line_window_size_; // number of points to fit lines in the first pass
- ScalarT fit_line_theta_max_parallel_; // maximum theta between consecutive segments to fuse them in a single line.
- ScalarT fit_line_k_sigmas_; // How many std_dev are tolerated to count that a point is supporting a line
-
- // Corner extraction
- ScalarT corner_theta_min_; // minimum theta between consecutive segments to detect corner. PI/6=0.52
-
- // TF Scan association
- unsigned int tf_scan_window_size_; // Number of scanners on the buffer (comparison of movement btw thos step)
- float asso_corner_dist_; // max distance between corners of two scans to be associated
- float asso_angle_diff_; // max angle difference between lines of two scans to be associated
- float asso_lines_ext_dist_; // max distance between initial OR final line points of two scans to be associated
-
- //ExtractLineParams extract_line_params_; // parameters for extracting lines
-
-
- void print() const;
- };
-
-
-// template <class T, size_t ROW, size_t COL, size_t DEPTH>
-// using MultiDimArray = std::array<std::array<std::array<T, DEPTH>, COL>, ROW>;
-// MultiDimArray<float, > floats;
-
-
- /** \brief Test Porpuses to create a Matlab script with all the variables.
- **/
- void exportStaticScanProcessed2Matlab(const char * _file_name,
- const LaserObjectDetAlgParams & _alg_params,
- const LaserScanProcessed & _scan_processed);
-
-
- /** \brief Test Porpuses to create a Matlab script with all the variables.
- **/
- void exportDataScanProcessed2Matlab(std::string &_file_name,
- LaserScanProcessed & _scan_processed);
-
-
- /** \brief Test Porpuses print Matrix Association Results.
- **/
- void printAssociationResults(std::vector<std::vector<laserscanutils::AssociationResult> > & _asso_results);
-
-
-
-
- /** \brief Filters the valid ranges on a scan and converts them to homogeneous coordinates.
- *
- * Valid ranges are filtered according to scan parameters.
- * Converts ranges from a laserScan to homogeneous coordinates and save it to a matrix of same size than ranges size, returning index till valid values.
- *
- * \param _params is the laser_scan_utils structure of the laser parameters.
- * \param _ranges is the raw scan measures. Vector of float ranges.
- * \param _points is the returned points from the conversion of ranges to homogeneous coordinates
- * \param _stats is the statistics structure where number of valid points is saved under _stats.filteredTotalPoints
- *
- **/
- void preFilterScan2homog(const laserscanutils::ScanParams & _params,
- const std::vector<float> & _ranges,
- Eigen::MatrixXs & _points,
- laserscanutils::LaserScanProcessed &_scan_processed);
-
-
-
- /** \brief Find the clusters in the valid points from the scan.
- *
- * Check distance between valid points in homogeneous coordinates. If it is more than the threshold, considers it as a different cluster.
- *
- * \param _alg_params is the structure of algorithm params for the Laser Object Detection Algorithm.
- * \param _points is the returned points from the conversion of ranges to homogeneous coordinates
- * \param _global_scan_number is the global number of scan processed (to be used as header id)
- * \param _scan_processed is the structure where all the information of elements in a scan is stored
- *
- **/
- void extractClustersInScan(const laserscanutils::LaserObjectDetAlgParams & _alg_params,
- const Eigen::MatrixXs & _points,
- unsigned int _global_scan_number,
- laserscanutils::LaserScanProcessed & _scan_processed);
-
-
-
-
- /** \brief Extract lines in the given cluster.
- *
- * From a cluster of points (EigenMatrix) extract the existing lines according to the alg parameters
- *
- * \param _alg_params is the structure of algorithm params for the Laser Object Detection Algorithm.
- * \param _points is the returned points from the conversion of ranges to homogeneous coordinates
- * \param _global_scan_number is the global number of scan processed (to be used as header id)
- * \param _scan_processed is the structure where all the information of elements in a scan is stored
- *
- **/
- void findLinesInCluster(const laserscanutils::ScanParams & _scan_params,
- const laserscanutils::LaserObjectDetAlgParams &_alg_params,
- const Eigen::MatrixXs &_cluster,
- std::list<Line> &_line_list);
-
-
-
-
- /** \brief Extract corners in the lines of a given cluster.
- *
- * From a list of lines belonging to the same cluster, extracts the existing corners according to the alg parameters
- *
- * \param _alg_params is the structure of algorithm params for the Laser Object Detection Algorithm.
- * \param _line_list is the list of lines from which the corners will be extracted.
- * \param _corner_list is the list of found corners in the cluster.
- *
- **/
- unsigned int findCornersInClusterLines(const laserscanutils::LaserObjectDetAlgParams & _obj_det_alg_params,
- const std::list<laserscanutils::Line> & _line_list,
- std::list<laserscanutils::Corner> & _corner_list);
-
-
-
- /** \brief Calculate Features
- *
- * Calculate middle point, centroid, length, with, area...
- *
- * \param _alg_params is the structure of algorithm params for the Laser Object Detection Algorithm.
- * \param _points is the returned points from the conversion of ranges to homogeneous coordinates
- * \param _global_scan_number is the global number of scan processed (to be used as header id)
- * \param _scan_processed is the structure where all the information of elements in a scan is stored
- *
- **/
- void extractFeaturesInObject(const Eigen::MatrixXs & _points,
- Object &_object);
-
-
-
-
- /** \brief Performs the association from two objects list
- *
- * From a list of objects of both previous and actual scans, computes the distances and find the
- * closest (euclidean dist threshold) ones as associated.
- *
- *
- **/
- void associateObjects(std::list<laserscanutils::Object> & _prev_objects_list,
- std::list<laserscanutils::Object> & _act_objects_list,
- float _corner_threshold, float _angle_thr, float _line_ext_threshold);
-
-
-
-// /** \brief Fullfils the bounding boxes and rest of parameters on the detections.
-// *
-// **/
-// void objectsBB(laserscanutils::LaserScanProcessed & _act_scan);
-
-}
-
-
-
-
-#endif // OBJECT_DETECTOR_H
diff --git a/src/deprecated/scan_basics.cpp b/src/deprecated/scan_basics.cpp
deleted file mode 100644
index eb1be3c79184ffa7bf1854880748bf76087be9bd..0000000000000000000000000000000000000000
--- a/src/deprecated/scan_basics.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "scan_basics.h"
-
-void laserscanutils::ScanParams::print() const
-{
- std::cout << "Laser 2D Scan Parameters : " << std::endl
- << " Angle min: " << angle_min_ << std::endl
- << " Angle max: " << angle_max_ << std::endl
- << " Angle step: " << angle_step_ << std::endl
- << " Scan time: " << scan_time_ << std::endl
- << " Range min: " << range_min_ << std::endl
- << " Range max: " << range_max_ << std::endl
- << " Range std dev: " << range_std_dev_ << std::endl;
-}
-
-void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps)
-{
- ScalarT azimuth = _params.angle_min_;
- ScalarT prev_range;
- unsigned int ii = 0;
-
- //resize to all points case
- _points.resize(3,_ranges.size());
-
- _jumps.clear();
-
- //for each range, check correctness of value and translate from polar to xy coordinates
- while (ii <_ranges.size())
- {
- if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !std::isnan(_ranges[ii]) && !std::isinf(_ranges[ii]) )
- {
- //transform from polar to euclidean
- _points(0,ii) = _ranges[ii] * cos(azimuth);
- _points(1,ii) = _ranges[ii] * sin(azimuth);
- _points(2,ii) = 1;
-
- //check jump
- if ( (ii != 0) && (fabs(_ranges[ii]-prev_range) > _jump_threshold) )
- _jumps.push_back(ii);
-
- //keep current range and update counter
- prev_range = _ranges[ii];
- ii++;
- }
- else
- _ranges.erase(_ranges.begin()+ii);
-
- azimuth += _params.angle_step_;
- }
-
- //resize the output matrix to the number of correct points, while keeping values
- _points.conservativeResize(3, _ranges.size());
- //_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ?
-}
-
-void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, const ScalarT& _jump_angle_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps)
-{
- ScalarT azimuth = _params.angle_min_;
- ScalarT prev_range;
- unsigned int ii = 0;
-
- //resize to all points case
- _points.resize(3,_ranges.size());
-
- _jumps.clear();
-
- //for each range, check correctness of value and translate from polar to xy coordinates
- while (ii <_ranges.size())
- {
- if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !std::isnan(_ranges[ii]) && !std::isinf(_ranges[ii]) )
- {
- //transform from polar to euclidean
- _points(0,ii) = _ranges[ii] * cos(azimuth);
- _points(1,ii) = _ranges[ii] * sin(azimuth);
- _points(2,ii) = 1;
-
- //check jump
- if ( (ii != 0) && (fabs(_ranges[ii]-prev_range) > _jump_threshold) && (fabs(_ranges[ii]-prev_range) > tan(_jump_angle_threshold) * prev_range * _params.angle_step_) )
- _jumps.push_back(ii);
-
- //keep current range and update counter
- prev_range = _ranges[ii];
- ii++;
- }
- else
- _ranges.erase(_ranges.begin()+ii);
-
- azimuth += _params.angle_step_;
- }
-
- //resize the output matrix to the number of correct points, while keeping values
- _points.conservativeResize(3, _ranges.size());
- //_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ?
-}
-
-void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points)
-{
- ScalarT azimuth = _params.angle_min_;
- unsigned int ii = 0;
-
- //resize to all points case
- _points.resize(3,_ranges.size());
-
- //for each range, check correctness of value and translate from polar to xy coordinates
- while (ii <_ranges.size())
- {
- if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !std::isnan(_ranges[ii]) && !std::isinf(_ranges[ii]) )
- {
- //transform from polar to euclidean
- _points(0,ii) = _ranges[ii] * cos(azimuth);
- _points(1,ii) = _ranges[ii] * sin(azimuth);
- _points(2,ii) = 1;
-
- ii++;
- }
- else
- {
- //erase the current range
- _ranges.erase(_ranges.begin()+ii);
- }
-
- //increment azimuth
- azimuth += _params.angle_step_;
- }
-
- //resize the output matrix to the number of correct points, while keeping values
- _points.conservativeResize(3, _ranges.size());
-}
-
-laserscanutils::ScalarT laserscanutils::pi2pi(const ScalarT& angle)
-{
- return (angle > 0 ? fmod(angle + M_PI, 2 * M_PI) - M_PI : fmod(angle - M_PI, 2 * M_PI) + M_PI);
-}
diff --git a/src/deprecated/scan_basics.h b/src/deprecated/scan_basics.h
deleted file mode 100644
index 70592e90aeb70b5a9dcc3628520c744de525f800..0000000000000000000000000000000000000000
--- a/src/deprecated/scan_basics.h
+++ /dev/null
@@ -1,106 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of laser_scan_utils
-// laser_scan_utils is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#ifndef SCAN_BASICS_H_
-#define SCAN_BASICS_H_
-
-//std
-#include <iostream>
-#include <vector>
-#include <queue>
-#include <list>
-
-//laserscanutils
-#include "laser_scan_utils.h"
-
-namespace laserscanutils
-{
- struct ScanParams
- {
- //members
- double angle_min_; //radians
- double angle_max_; //radians
- double angle_step_; //radians
- double scan_time_; //time from the measuring the first ray up to measuring the last one, seconds
- double range_min_; //meters
- double range_max_; //meters
- double range_std_dev_; //standard deviation measurement noise in range, meters
-
- //just a print method
- void print() const;
- };
-
- /** \brief Transforms from ranges (polar) to euclidean (xy)
- *
- * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data.
- * Invalid values (inf's and nan's) and out of range measurements are not transformed.
- * Valid measurements are returned, by reference, in homogeneous coordinates.
- * Ranges vector is returned with all not correct elements removed.
- * A list of jumps indices is also returned by reference.
- * \param _params laser_scan_utils structure of the laser scan parameters.
- * \param _ranges raw range measurements from the laser scanner device. Size Nr. Type float to match ROS LaserScan message
- * \param _jump_theshold distance threshold to classify a jump in the range.
- * \param _points matrix of homogeneous coordinates of hit points. Size 3xNp, where Np can be <= than Nr (due to possible inf's and nan's from raw data)
- * \param jumps_id_ list of indexes of jumps in range measurements. Indexes correpond to _points vector.
- *
- **/
- void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps);
-
- /** \brief Transforms from ranges (polar) to euclidean (xy) detecting jumps not in absolute range threshold but in angle w.r.t the beams
- *
- * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data.
- * Invalid values (inf's and nan's) and out of range measurements are not transformed.
- * Valid measurements are returned, by reference, in homogeneous coordinates.
- * Ranges vector is returned with all not correct elements removed.
- * A list of jumps indices is also returned by reference.
- * \param _params laser_scan_utils structure of the laser scan parameters.
- * \param _ranges raw range measurements from the laser scanner device. Size Nr. Type float to match ROS LaserScan message
- * \param _jump_theshold distance threshold to classify a jump in the range.
- * \param _jump_angle_theshold angle threshold to classify a jump in the range.
- * \param _points matrix of homogeneous coordinates of hit points. Size 3xNp, where Np can be <= than Nr (due to possible inf's and nan's from raw data)
- * \param jumps_id_ list of indexes of jumps in range measurements. Indexes correpond to _points vector.
- *
- **/
- void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, const ScalarT& _jump_angle_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps);
-
- /** \brief Transforms from ranges (polar) to euclidean (xy)
- *
- * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data.
- * Invalid values (inf's and nan's) and out of range measurements are not transformed.
- * Valid measurements are returned, by reference, in homogeneous coordinates.
- * Ranges vector is returned with all not correct elements removed.
- * Requires:
- * \param _params laser_scan_utils structure of the laser scan parameters.
- * \param _ranges raw range measurements from the laser scanner device. Size Nr. Type float to match ROS LaserScan message
- * Outputs:
- * \param _points matrix of homogeneous coordinates of hit points. Size 3xNp, where Np can be <= than Nr (due to possible inf's and nan's from raw data)
- **/
- void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points);
-
-
- /** \brief Fits angle in (-pi, pi]
- *
- * Fits angle in (-pi, pi]
- **/
- ScalarT pi2pi(const ScalarT& angle);
-}
-#endif
diff --git a/src/grid_2d.cpp b/src/grid_2d.cpp
index 798097a417bb479ad646939ac0fd3324bd4bef54..b4dff7e56b48bdae0bb59542b841086cd93bdb57 100644
--- a/src/grid_2d.cpp
+++ b/src/grid_2d.cpp
@@ -20,7 +20,7 @@
//
//--------LICENSE_END--------
-#include "grid_2d.h"
+#include "laser_scan_utils/grid_2d.h"
namespace laserscanutils
{
diff --git a/src/grid_cluster.cpp b/src/grid_cluster.cpp
index c4b670b80ed2ba71a1ef584d6693ba82f14d446f..17168cf8136b71f88e0810592dc293989f0e4669 100644
--- a/src/grid_cluster.cpp
+++ b/src/grid_cluster.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "grid_cluster.h"
+#include "laser_scan_utils/grid_cluster.h"
namespace laserscanutils
{
diff --git a/src/icp.cpp b/src/icp.cpp
index f746dd620b42ef508c8b05beb8fa862680db55d7..1e60fbc7f77fdaff24b9c542feb3b8eefc35d1be 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "icp.h"
+#include "laser_scan_utils/icp.h"
#include <algorithm>
#include <unistd.h>
#include <fcntl.h>
diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 4fbb90cfb0cec69b406cc68f7b543facd7fa6b94..2827e90d8ef8e8a89b3cde6eccf9dfd03a418055 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "laser_scan.h"
+#include "laser_scan_utils/laser_scan.h"
namespace laserscanutils
{
diff --git a/src/laser_scan_with_params.cpp b/src/laser_scan_with_params.cpp
index ad6aa84e0d0893e1868de70a5ad06f6bc20180d0..8e5ce0d99f0bf8d0b15fe45071cb43a06c389acd 100644
--- a/src/laser_scan_with_params.cpp
+++ b/src/laser_scan_with_params.cpp
@@ -20,7 +20,7 @@
//
//--------LICENSE_END--------
-#include "laser_scan_with_params.h"
+#include "laser_scan_utils/laser_scan_with_params.h"
namespace laserscanutils
{
diff --git a/src/line_finder.cpp b/src/line_finder.cpp
index 3667697649702bf35a548e1a0bd311b03385a153..418a40499e7f8eadaba675624cc1c7d5b280e8c4 100644
--- a/src/line_finder.cpp
+++ b/src/line_finder.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "line_finder.h"
+#include "laser_scan_utils/line_finder.h"
namespace laserscanutils
{
diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp
index f04163aff1247051d750d812e3a2925f614f4b1f..930015cc99e1d3f29b891c07de728ebbb4894250 100644
--- a/src/line_finder_hough.cpp
+++ b/src/line_finder_hough.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "line_finder_hough.h"
+#include "laser_scan_utils/line_finder_hough.h"
namespace laserscanutils
{
diff --git a/src/line_finder_iterative.cpp b/src/line_finder_iterative.cpp
index cdd97d8344953b0efcf92cf8fae07e574fb42b69..d83e5ee2cbd64cde3570ca6a4b1503f8c925e9ff 100644
--- a/src/line_finder_iterative.cpp
+++ b/src/line_finder_iterative.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "line_finder_iterative.h"
+#include "laser_scan_utils/line_finder_iterative.h"
namespace laserscanutils
{
diff --git a/src/line_finder_jump_fit.cpp b/src/line_finder_jump_fit.cpp
index 8397a306cf72a9ee1248edce51726d7db01fa0ba..38a1ea71a8c42eabe1af47272af31f8a9138ea21 100644
--- a/src/line_finder_jump_fit.cpp
+++ b/src/line_finder_jump_fit.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "line_finder_jump_fit.h"
+#include "laser_scan_utils/line_finder_jump_fit.h"
namespace laserscanutils
{
diff --git a/src/line_segment.cpp b/src/line_segment.cpp
index a3b8ec185d0d0848ac65928e27e8aa88f9604ac5..a5c9c893fecf661d3c8b197664d614dee3d84012 100644
--- a/src/line_segment.cpp
+++ b/src/line_segment.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "line_segment.h"
+#include "laser_scan_utils/line_segment.h"
namespace laserscanutils
{
diff --git a/src/loop_closure_base.cpp b/src/loop_closure_base.cpp
index 03a436ee52a86dc8763fc6c33a2e3da7d5e7bb66..177a7b897555dbd22cc65cd6c2c140bfa19e2701 100644
--- a/src/loop_closure_base.cpp
+++ b/src/loop_closure_base.cpp
@@ -26,7 +26,7 @@
* \author: spujol
*/
-#include "loop_closure_base.h"
+#include "laser_scan_utils/loop_closure_base.h"
namespace laserscanutils {
diff --git a/src/point_set.cpp b/src/point_set.cpp
index 0970a415c794279a304449e6560e9a791dee56b5..a20fb16ce043add97d8a21f94834bfd69f854987 100644
--- a/src/point_set.cpp
+++ b/src/point_set.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "scan_segment.h"
+#include "laser_scan_utils/scan_segment.h"
namespace laserscanutils
{
diff --git a/src/polyline.cpp b/src/polyline.cpp
index d4658c57a628aa73084f07299281f90bc76c65d0..28bc1d617e89cd7588fab1a34ec926ccedce5521 100644
--- a/src/polyline.cpp
+++ b/src/polyline.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "polyline.h"
+#include "laser_scan_utils/polyline.h"
namespace laserscanutils
{
diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp
index 05af3e6971f0756506826d6932a81eafd5c2f016..94b81a462e23bae12dddd2873a9611a40e6763d8 100644
--- a/src/scan_segment.cpp
+++ b/src/scan_segment.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "scan_segment.h"
+#include "laser_scan_utils/scan_segment.h"
namespace laserscanutils
{
diff --git a/src/scilab/corner_aperture.sci b/src/scilab/corner_aperture.sci
deleted file mode 100644
index 6c6e3d708f1b0390925196a1c3f74aa7a71c5c06..0000000000000000000000000000000000000000
--- a/src/scilab/corner_aperture.sci
+++ /dev/null
@@ -1,49 +0,0 @@
-
-//Computes corner aperture in 2D, given two extreme points and the corner location
-// REQUIRES: three points in homogeneous coordinates (last component set to 1):
-// p1: first point of the first line supporting a corner
-// c: corner point
-// p2: last point of the second line supporting a corner
-// ENSURES:
-// A return angle in [0,2pi]
-// DEGENERATE CASE: if p1=c, and/or p2=c, the result is 0;
-
-function [ret_alpha] = corner_aperture(p1,c,p2);
-
- //check degenerate case (c=p1, or c=p2)
- deg_case = %F;//set degenerate case flag to false
- n_comp_equal = size(find(p1 == c),2); //number of components equal
- if n_comp_equal == 3
- deg_case = %T;
- end
- n_comp_equal = size(find(p2 == c),2); //number of components equal
- if n_comp_equal == 3
- deg_case = %T;
- end
-
- if deg_case == %F then
- //buid lines from c to p1 and p2
- l1 = cross(c,p1);
- l2 = cross(c,p2);
-
- //normalize lines
- l1 = l1 / ( sqrt( l1(1)*l1(1)+l1(2)*l1(2) ) );
- l2 = l2 / ( sqrt( l2(1)*l2(1)+l2(2)*l2(2) ) );
-
- //compute angle
- alpha = abs(acos( l1(1)*l2(1) + l1(2)*l2(2) ) ) ; // abs to avoid complex numbers due to numerical precision
-
- //compute det
- detM = det([p1';c';p2']);
-
- //if det<0, return the complementary angle
- if(detM<0) then
- ret_alpha = 2*%pi-alpha;
- else
- ret_alpha = alpha;
- end
- else
- ret_alpha = 0;
- end
-
-endfunction
diff --git a/src/scilab/corner_aperture_test.sce b/src/scilab/corner_aperture_test.sce
deleted file mode 100644
index 60b2cd151439470b335f27098a8af81c86819810..0000000000000000000000000000000000000000
--- a/src/scilab/corner_aperture_test.sce
+++ /dev/null
@@ -1,39 +0,0 @@
-
-//includes
-exec('/home/vvaquero/iri-lab/labrobotica/algorithms/laser_scan_utils/trunk/src/scilab/corner_aperture.sci');
-
-//points
-p1 = [2;5;1];
-p2 = [5;2;1];
-
-//plot steps
-dx = 0.1;
-dy = 0.1
-xmin = 1;
-xmax = 10;
-ymin = 1;
-ymax = 10;
-
-//move a corner between these two points to build the 3D plot
-ii = 0;
-ap_mat = [];
-for cx = xmin:dx:xmax
- ii = ii + 1;
- jj = 0;
- ap_mat_col = [];
- for cy = ymin:dy:ymax
- jj = jj + 1;
- aperture = corner_aperture(p1,[cx;cy;1],p2);
- ap_mat_col = [ap_mat_col; aperture];
- end
- ap_mat = [ap_mat ap_mat_col];
-end
-
-//plot results
-fh = scf();
-xset("colormap",jetcolormap(128));
-colorbar(min(ap_mat),max(ap_mat));
-surf(ap_mat,'thickness',0);
-//fh.color_map = jetcolormap(128);
-fh.children.rotation_angles = [0,-90];
-title('Corner aperture between two points');
diff --git a/src/scilab/corner_detector.sce b/src/scilab/corner_detector.sce
deleted file mode 100644
index ebb170499b57e2f9f0f20ad26c55b162daa0992b..0000000000000000000000000000000000000000
--- a/src/scilab/corner_detector.sce
+++ /dev/null
@@ -1,254 +0,0 @@
-//info about 2D homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
-
-// clear all
-xdel(winsid());
-clear;
-
-//scan params:
-Ns = 720; //scan rays
-aperture = %pi; //scan aperture [rad]
-azimuth_step = aperture/Ns;
-
-//User Tunning params
-Nw = 8; //window size
-theta_th = %pi/8;
-theta_max = 0.3;
-K = 3; //How many std_dev are tolerated to count that a point is supporting a line
-r_stdev = 0.01; //ranging std dev
-max_beam_dist = 5; //max distance in amount of beams to consider concatenation of two lines
-max_dist = 0.2; //max distance to a corner from the ends of both lines to take it
-range_jump_th = 0.5; //threshold of distance to detect jumps in ranges
-max_points_line = 1000; //max amount of beams of a line
-
-//init
-points = [];
-result_lines = [];
-line_indexes = [];
-corners = [];
-corners_jumps = [];
-jumps = [];
-apertures=[];//corner apertures
-
-//scan ranges
-ranges = read('/home/acoromin/dev/labrobotica/algorithms/laser_scan_utils/trunk/src/scilab/scan.txt',-1,Ns);
-//ranges = read(fullpath('scan.txt'),-1,Ns);
-//ranges = read(fullpath(TMPDIR + '/scan.txt'),-1,Ns);
-//ranges = [];
-
-//invent a set of points + noise
-//points = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43;
-// 7 6 5 4 3 2 1 2 3 4 5 6 7 8 9 10 9 8 7 6 5 4 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 7.4 7.3 7.2 7.1 7 6.9 6.8 6.7 6.6 6.5 6.4];
-for i=1:Ns
- points = [points [ranges(i)*cos(aperture/2 - azimuth_step*i); ranges(i)*sin(aperture/2 - azimuth_step*i)]];
- // Store range jumps
- if i>1 & abs(ranges(i)-ranges(i-1)) > range_jump_th then
- jumps = [jumps i];
- end
-end
-
-points = points + rand(points,"normal")*r_stdev;
-
-//Runs over a sliding window of Nw points looking for straight lines
-i_jump = 1;
-for i = Nw:size(points,2)
-
- //set the window indexes
- i_from = i-Nw+1;
- i_to = i;
- points_w = points(:,i_from:i_to);
-
- //update the jump to be checked
- while i_jump < size(jumps,2) & i_from > jumps(i_jump)
- i_jump = i_jump+1;
- end
-
- //check if there is a jump inside the window (if it is the case, do not fit a line)
- if jumps(i_jump) > i_from & jumps(i_jump) <= i_to then
- continue;
- end
-
- //Found the best fitting line over the window. Build the system: Ax=0. Matrix A = a_ij
-// a_00 = sum( points_w(1,:).^2 );
-// a_01 = sum( points_w(1,:).*points_w(2,:) );
-// a_02 = sum( points_w(1,:) );
-// a_10 = a_01;
-// a_11 = sum( points_w(2,:).^2 );
-// a_12 = sum( points_w(2,:) );
-// a_20 = a_02;
-// a_21 = a_12;
-// a_22 = Nw;
-// A1 = [a_00 a_01 a_02; a_10 a_11 a_12; a_20 a_21 a_22; 0 0 1];
-
- a_00 = sum( points_w(1,:).^2 );
- a_01 = sum( points_w(1,:).*points_w(2,:) );
- a_02 = sum( points_w(1,:) );
- a_10 = a_01;
- a_11 = sum( points_w(2,:).^2 );
- a_12 = sum( points_w(2,:) );
- A = [a_00 a_01 a_02; a_10 a_11 a_12; 0 0 1];
-
- //solve
-// line1 = pinv(A1)*[zeros(3,1);1];
- line = inv(A)*[0; 0; 1];
- //disp(line1-line);
-
- //compute error
- err = 0;
- for j=1:Nw
- err = err + abs(line'*[points_w(:,j);1])/sqrt(line(1)^2+line(2)^2);
- end
- err = err/Nw;
- //disp("error: "); disp(err);
-
- //if error below stdev, add line to result set
- if err < K*r_stdev then
- result_lines = [result_lines [line;points_w(:,1);points_w(:,$)]];
- line_indexes = [line_indexes [i_from; i_to]]; //ray index where the segment ends
- end
-end
-
-//line concatenation
-j = 1;
-while (j < size(result_lines,2))
-
- // num of rays between last of current line and first of next line
- if (line_indexes(1,j+1)-line_indexes(2,j)) > max_beam_dist then
- j=j+1;
- else
- //compute angle diff between consecutive segments
- cos_theta = result_lines(1:2,j)'*result_lines(1:2,j+1) / ( norm(result_lines(1:2,j)) * norm(result_lines(1:2,j+1)) );
- theta = abs(acos(cos_theta));
-
- //if angle diff lower than threshold, concatenate
- if theta < theta_max then
-
- //set the new window
- i_from = line_indexes(1,j);//first point index of the first line
- i_to = line_indexes(2,j+1);//last point index of the second line
- points_w = points(:,i_from:i_to); //that's the new window !
-
- //Found the best fitting line over the window. Build the system: Ax=0. Matrix A = a_ij
- a_00 = sum( points_w(1,:).^2 );
- a_01 = sum( points_w(1,:).*points_w(2,:) );
- a_02 = sum( points_w(1,:) );
- a_10 = a_01;
- a_11 = sum( points_w(2,:).^2 );
- a_12 = sum( points_w(2,:) );
- A = [a_00 a_01 a_02; a_10 a_11 a_12; 0 0 1];
-
- //solve
- line = inv(A)*[0; 0; 1];
-
- //compute error
- err = 0;
- for k=1:Nw
- err = err + abs(line'*[points_w(:,k);1])/sqrt(line(1)^2+line(2)^2);
- end
- err = err/Nw;
-
- //if error below stdev, change line to concatenation and erase the next line
- if err < K*r_stdev then
- result_lines(:,j) = [line;points_w(:,1);points_w(:,$)];
- line_indexes(:,j) = [i_from; i_to];
- result_lines = [result_lines(:,1:j) result_lines(:,j+2:$)];
- line_indexes = [line_indexes(:,1:j) line_indexes(:,j+2:$)];
- if (i_to-i_from)>max_points_line then
- j=j+1;
- end
- else
- j=j+1;
- end
- else
- j=j+1;
- end
- end
-end
-
-//corner detection
-for i = 1:(size(result_lines,2)-1)
- for j = i+1:size(result_lines,2)
-
- // num of rays between last of current line and first of next line
- if (line_indexes(1,j)-line_indexes(2,i)) > max_beam_dist then
- break;
- end
- //compute angle diff between consecutive segments
- cos_theta = result_lines(1:2,i)'*result_lines(1:2,j) / ( norm(result_lines(1:2,i)) * norm(result_lines(1:2,j)) );
- theta = abs(acos(cos_theta));
-
- //if angle diff greater than threshold && indexes are less than Nw, we decide corner
- if theta > theta_th then
-
- //Corner found! Compute "sub-pixel" corner location as the intersection of two lines
- corner = cross(result_lines(1:3,i),result_lines(1:3,j));
- corner = corner./corner(3);//norlamlize homogeneous point
-
- // Check if the corner is close to both lines ends
- if ( norm(corner(1:2)-points(:,line_indexes(2,i))) < max_dist & norm(corner(1:2)-points(:,line_indexes(1,j))) < max_dist ) then
-
- //add corner to the resulting list
- corners = [corners corner];
-
- //compute aperture
- p1 = [points(:,line_indexes(1,i));1]//first point of the i line (lower azimuth)
- p2 = [points(:,line_indexes(2,j));1]//last point of the j line (greater azimuth)
- det([p1';corner;p2])
- aperture =
- apertures = [apertures aperture];
- end
- //display
- //disp("theta: "); disp(theta);
- //disp("index:" ); disp(line_indexes(i)-Nw+1);//line_indexes(i) indicates the end point of the segment
- end
-
- end
-end
-
-// corner detection from jumps
-for i=1:size(jumps,2)
- if ranges(jumps(i)) < ranges(jumps(i)-1) then
- corners_jumps = [corners_jumps points(:,jumps(i))];
- else
- corners_jumps = [corners_jumps points(:,jumps(i)-1)];
- end
-end
-
-//Set figure
-fig1 = figure(0);
-fig1.background = 8;
-
-//plot points
-plot(points(1,:),points(2,:),"g.");
-
-//axis settings
-ah = gca();
-ah.isoview = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.grid = [0.1,0.1,0.1];
-ah.grid_position = "background";
-
-
-//plot lines
-for i=1:size(result_lines,2)
- m = -result_lines(1,i)/result_lines(2,i);
- xc = -result_lines(3,i)/result_lines(2,i);
- point1 = [result_lines(4,i) m*result_lines(4,i)+xc];
- point2 = [result_lines(6,i) m*result_lines(6,i)+xc];
- xpoly([point1(1) point2(1)],[point1(2) point2(2)]);
-end
-
-//plot corners
-plot(corners(1,:),corners(2,:),"ro");
-plot(corners_jumps(1,:),corners_jumps(2,:),"bo");
-
-disp(corners_jumps');
-disp(corners');
-
-//plot jumps
-//for i=1:size(jumps,2)
-// plot(ranges(jumps(i))*cos(aperture/2 - azimuth_step*jumps(i)), ranges(jumps(i))*sin(aperture/2 - azimuth_step*jumps(i)),"bx");
-//end
-
-
diff --git a/src/scilab/homogeneous_lines.sce b/src/scilab/homogeneous_lines.sce
deleted file mode 100644
index 8602a8b48fb612b898d0923599305f8cee313dfc..0000000000000000000000000000000000000000
--- a/src/scilab/homogeneous_lines.sce
+++ /dev/null
@@ -1,88 +0,0 @@
-
-//set points defining line 1 and 2
-p1o = [2;2;1];
-p1e = [-1;1;1];
-p2o = [-1;2;1];
-p2e = [2;-2;1];
-
-//compute lines
-l1 = cross(p1o,p1e);
-l2 = cross(p2o,p2e);
-
-//normalize (A) lines according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
-aux = sqrt( l1(1)*l1(1) + l1(2)*l1(2) );
-l1nA = l1./aux;
-aux = sqrt( l2(1)*l2(1) + l2(2)*l2(2) );
-l2nA = l2./aux;
-
-//normalize (B) lines according standard point normalization
-l1nB = l1./l1(3);
-l2nB = l2./l2(3);
-
-//move l1 to a new frame given by T2
-T2 = [cos(%pi/5) -sin(%pi/5) 2;sin(%pi/5) cos(%pi/5) 1;0 0 1]
-l1_f2 = det(T2)*(inv(T2))'*l1;
-l1_f2_nB = l1_f2./l1_f2(3);
-
-//plot lines
-fh=figure();
-fh.background = color("white");
-
-ah = gca();
-ah.isoview = "on";
-ah.auto_margins = "off";
-ah.data_bounds = [-4,4,-4,4];
-
-plot([p1o(1) p1e(1)], [p1o(2) p1e(2)], '+b');
-xpoly([p1o(1) p1e(1)], [p1o(2) p1e(2)]);
-e=gce(); // get the current entity (the last created: here the polyline)
-set(e,"foreground",9);
-set(e,"thickness",3);
-
-plot([p2o(1) p2e(1)], [p2o(2) p2e(2)], '+r');
-xpoly([p2o(1) p2e(1)], [p2o(2) p2e(2)]);
-e=gce(); // get the current entity (the last created: here the polyline)
-set(e,"foreground",5);
-set(e,"thickness",3);
-
-//plot normal vectors
-p1c = (p1o+p1e)./2; //center point of l1
-xpoly([p1c(1) p1c(1)+l1nB(1)], [p1c(2) p1c(2)+l1nB(2)]);
-e=gce(); // get the current entity (the last created: here the polyline)
-set(e,"foreground",9);
-set(e,"thickness",3);
-
-p2c = (p2o+p2e)./2; //center point of l2
-xpoly([p2c(1) p2c(1)+l2nB(1)], [p2c(2) p2c(2)+l2nB(2)]);
-e=gce(); // get the current entity (the last created: here the polyline)
-set(e,"foreground",5);
-set(e,"thickness",3);
-
-//plot lines
-fh=figure();
-fh.background = color("white");
-
-ah = gca();
-ah.isoview = "on";
-ah.auto_margins = "off";
-ah.data_bounds = [-4,4,-4,4];
-
-//plot transformed line
-p1o_f2 = T2*p1o;
-p1e_f2 = T2*p1e;
-plot([p1o_f2(1) p1e_f2(1)], [p1o_f2(2) p1e_f2(2)], '+g');
-xpoly([p1o_f2(1) p1e_f2(1)], [p1o_f2(2) p1e_f2(2)]);
-e=gce(); // get the current entity (the last created: here the polyline)
-set(e,"foreground",14);
-set(e,"thickness",3);
-
-p1c = (p1o_f2+p1e_f2)./2; //center point of l1_f2
-xpoly([p1c(1) p1c(1)+l1_f2_nB(1)], [p1c(2) p1c(2)+l1_f2_nB(2)]);
-e=gce(); // get the current entity (the last created: here the polyline)
-set(e,"foreground",14);
-set(e,"thickness",3);
-
-
-
-
-
diff --git a/src/scilab/scan.txt b/src/scilab/scan.txt
deleted file mode 100644
index d19cf457c35f5fa45abe6d1a9c63883d5ffd891d..0000000000000000000000000000000000000000
--- a/src/scilab/scan.txt
+++ /dev/null
@@ -1 +0,0 @@
-2.78886,2.78289,2.7832,2.78367,2.7843,2.78508,2.78603,2.78713,2.78839,2.78981,2.79139,2.78669,2.78855,2.79057,2.79274,2.79507,2.79098,2.79359,2.79635,2.79927,2.79566,2.79886,2.80221,2.79895,2.80258,2.80638,2.80345,2.80753,2.81176,2.80917,2.81368,2.81132,2.81611,2.81396,2.81903,2.73664,2.72743,2.71782,2.70883,2.69945,2.69067,2.6815,2.67294,2.66398,2.65562,2.64736,2.6387,2.63064,2.62218,2.61431,2.60605,2.59837,2.59078,2.5828,2.57539,2.56759,2.56036,2.55321,2.54568,2.53871,2.53182,2.52455,2.51782,2.51118,2.50415,2.49767,2.49127,2.48495,2.47824,2.47207,2.46597,2.4595,2.45355,2.44768,2.44188,2.4357,2.43005,2.42446,2.41894,2.4135,2.40768,2.40236,2.39712,2.39195,2.3864,2.38135,2.37637,2.37146,2.36661,2.36182,2.35666,2.352,2.3474,2.34286,2.34186,2.36289,2.41051,2.43311,2.45628,2.48003,2.50439,2.52937,2.555,2.61224,2.63993,2.66837,2.69757,2.72758,2.75841,2.79011,2.82269,2.85621,2.8907,2.96659,3.0042,3.04295,3.08289,3.12406,3.16652,3.21033,3.22693,3.23444,3.24207,3.24982,3.2577,3.26571,3.2855,3.29383,3.30229,3.31088,3.3196,3.32846,3.33745,3.34658,3.35584,3.36524,3.37478,3.38445,3.39427,3.40423,3.41434,3.42459,3.43499,3.44553,3.45623,3.46707,3.47807,3.48923,3.50053,3.512,3.52362,3.53541,3.54736,3.55947,3.57175,3.5842,3.59681,3.6096,3.62256,3.63569,3.64901,3.6625,3.67617,3.69003,3.70407,3.7183,3.73272,3.74733,3.76214,3.7931,3.80843,3.82397,3.83972,3.85567,3.87183,3.88822,3.90481,3.92163,3.93867,3.95593,3.97343,3.97347,3.99127,4.00931,4.02758,4.0461,4.06486,4.08387,4.10314,4.12266,4.14244,4.16248,4.20237,4.22313,4.24418,4.26551,4.28712,4.30903,4.33122,4.35372,4.37652,4.39963,4.42304,4.44678,4.47084,4.49523,4.51994,4.545,4.57041,4.59615,4.62225,4.64871,4.67554,4.70274,4.73032,4.75828,4.78663,4.81538,4.84452,4.8741,4.90409,4.9345,4.96534,4.99662,5.02836,5.06053,5.09317,5.12628,5.15987,5.19395,5.22853,5.2636,5.2992,5.33532,5.37197,5.44106,5.47923,5.51796,5.55729,5.59721,5.63773,5.67888,5.72066,5.76307,5.80615,5.8499,5.89432,5.93945,6.02374,6.07085,6.11872,6.16734,6.21676,6.26698,6.318,6.36987,6.42258,6.47618,6.5758,6.6319,6.68894,6.74694,6.80593,6.86593,6.92698,7.04022,7.10425,7.1694,7.23572,7.30322,7.37192,7.49928,7.57149,7.64505,7.58372,7.51951,7.45681,7.32129,7.32938,7.34276,7.35632,7.36877,7.38272,7.39687,7.41124,7.4258,7.43923,7.4542,7.46937,7.48477,7.49904,7.51485,7.53087,7.54579,7.56225,7.57894,7.59587,7.61164,7.62902,8.37389,8.39194,8.41173,8.43177,8.45059,8.47118,8.49202,8.51164,8.53305,8.55319,8.57516,8.59739,8.61839,8.64122,8.6628,8.6862,8.70837,8.73237,8.7567,8.77979,8.80472,8.82843,8.85402,8.87835,8.9046,8.9296,8.9565,8.98218,9.0082,9.03616,9.06287,9.09152,9.11895,9.14834,9.17652,9.20503,9.23557,9.26487,9.29454,9.32626,9.35674,9.38762,9.42055,9.45226,9.4844,9.51695,9.55162,9.58503,9.61893,9.65324,4.38929,4.38536,4.36058,4.3365,4.3131,4.29036,4.26827,4.24682,4.22598,4.20576,4.18612,4.1944,4.17582,4.15708,4.13859,4.12032,4.10229,4.08449,4.06691,4.04955,4.03241,4.01549,3.99916,3.98265,3.96634,3.95024,3.93434,3.91901,3.90349,3.88817,3.87304,3.85845,3.84368,3.82909,3.81504,3.80081,3.78674,3.7732,3.75948,3.74591,3.73287,3.71963,3.7069,3.69398,3.68156,3.66894,3.65647,3.6445,3.63233,3.62065,3.60876,3.59736,3.58576,3.58265,3.61553,3.62696,3.63867,3.67347,3.68596,3.72229,3.7356,3.77355,3.78772,3.80219,3.84244,3.85785,3.89993,3.9163,3.93303,3.97774,3.99551,4.01367,4.06121,4.0805,4.10019,4.15081,4.17174,4.19309,13.8062,13.7714,13.7384,13.7075,13.8936,13.9735,14.0549,14.1382,14.3407,15.8017,15.9009,16.002,16.1054,16.3519,16.462,16.5744,16.6893,16.9594,17.0819,17.2072,17.3352,17.4661,17.6,8.14878,8.1334,8.11823,8.10324,8.08848,8.07391,8.0588,8.04465,8.03069,8.01693,8.00338,7.99,7.97684,7.96312,7.95032,7.93773,7.92533,7.91309,7.90106,7.88922,7.87755,7.86606,7.85476,7.84289,7.83195,7.82116,7.81058,7.80017,7.78993,7.77984,7.76995,7.76021,7.75066,7.74128,7.73204,7.76034,7.99805,8.11853,8.24311,8.37202,12.3718,12.3587,12.346,12.3336,12.3213,12.3094,12.2976,12.2862,12.275,12.264,12.2534,12.2429,12.2327,12.2228,12.213,12.2036,12.1944,12.1854,12.1766,12.3577,12.667,16.7608,16.7501,16.7398,16.7297,16.7201,16.7106,16.7015,16.6929,16.6844,16.9488,20.069,20.0619,20.0552,20.0489,20.043,20.0374,20.0323,20.0275,20.0231,20.0191,20.0155,20.0122,20.0094,20.0069,20.0048,20.0031,20.0018,20.0008,20.0002,20.0001,20.0002,20.0008,20.0018,20.0031,20.0048,20.0069,20.0094,20.0122,20.0155,20.0191,20.0231,20.0275,20.0323,20.0374,20.043,20.0489,20.0552,20.0619,20.069,20.0764,20.0843,20.0926,20.1012,20.1102,20.1196,20.1294,20.1397,20.1502,20.1612,20.1726,20.1844,20.1966,20.2092,20.2222,20.2356,20.2494,20.2636,20.2782,20.2932,20.3086,20.3244,20.3407,20.3573,20.3744,20.3919,20.4098,20.4281,20.4469,20.466,20.4856,20.5057,20.5261,20.547,20.5684,20.5901,20.6123,20.635,20.6581,20.6816,20.7056,20.73,20.7549,20.7802,20.806,20.8323,20.859,20.8862,20.9139,20.942,20.9706,20.9997,21.0293,21.0594,21.0899,21.1209,21.1525,21.1845,21.217,21.2501,21.2836,21.3177,21.3522,21.3873,21.423,21.4591,21.4958,21.533,21.5707,21.6091,21.6479,21.6873,21.7273,21.7678,21.8089,21.8505,21.8928,21.9356,21.979,22.023,22.0676,22.1128,22.1586,22.2051,22.2521,22.2998,22.3481,22.397,22.4466,22.4968,22.5477,22.5992,22.6515,22.7043,22.7579,22.8122,22.8671,22.9228,22.9792,23.0363,23.0941,23.1526,23.2119,23.2719,23.3327,23.3943,23.4566,23.5197,23.5836,23.6483,23.7138,23.7802,23.8473,23.9153,23.9842,24.0539,24.1244,24.1959,24.2682,24.3414,24.4156,24.4906,24.5666,24.6435,24.7214,24.8003,24.8801,24.9609,25.0428,25.1256,25.2095,25.2944,25.3804,25.4675,25.5556,25.6449,25.7353,25.8268,25.9194,26.0132,26.1082,26.2044,26.3018,26.4004,26.5003,26.6015,26.7039,26.8077,26.9127,27.0191,27.1269,27.2361,27.3466,27.4586,27.572,27.6869,27.8033,27.9213,28.0407,28.1617
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 0f66e7183ad47f8fe9401f04d9941f6f12a1b3f3..3234bd9b88144230baa4ac384d7bb97408b8c192 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,21 +1,10 @@
# Retrieve googletest from github & compile
add_subdirectory(gtest)
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
-#Include directories
-INCLUDE_DIRECTORIES(../src)
-INCLUDE_DIRECTORIES(/data)
-FIND_PACKAGE(Eigen3 3.3 REQUIRED)
-FIND_PACKAGE(PythonLibs QUIET)
-INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
-
############# USE THIS TEST AS AN EXAMPLE ####################
# #
# Create a specific test executable for gtest_example #
# laser_scan_utils_add_gtest(gtest_example gtest_example.cpp)#
-# target_link_libraries(gtest_example ${PROJECT_NAME}) #
# #
##############################################################
@@ -25,14 +14,8 @@ laser_scan_utils_add_gtest(gtest_example gtest_example.cpp)
# gtest icp
IF(csm_FOUND)
laser_scan_utils_add_gtest(gtest_icp gtest_icp.cpp)
- IF(python_FOUND)
- target_link_libraries(gtest_icp ${PROJECT_NAME} ${PYTHON_LIBRARIES})
- ELSE(python_FOUND)
- target_link_libraries(gtest_icp ${PROJECT_NAME})
- ENDIF(python_FOUND)
ENDIF(csm_FOUND)
IF(falkolib_FOUND)
- laser_scan_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/data)
- target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME})
+ laser_scan_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp)
ENDIF(falkolib_FOUND)
\ No newline at end of file
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 1bfcc9c1a097aa0b8d5d685c2e292e1680b544e5..be183ee5d085cf8d0378a303906edc2268b281a3 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,78 @@
-cmake_minimum_required(VERSION 2.8.8)
-project(gtest_builder C CXX)
+if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+ message("CMake version less than 3.11.0")
-# We need thread support
-#find_package(Threads REQUIRED)
+ # Enable ExternalProject CMake module
+ include(ExternalProject)
-# Enable ExternalProject CMake module
-include(ExternalProject)
+ set(GTEST_FORCE_SHARED_CRT ON)
+ set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+ # Download GoogleTest
+ ExternalProject_Add(googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG v1.8.x
+ # TIMEOUT 1 # We'll try this
+ CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+ -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+ -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+ -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+ -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+ -DBUILD_GTEST=ON
+ PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+ # Disable install step
+ INSTALL_COMMAND ""
+ UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+ )
-# For some reason I need to disable PTHREADS
-# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
-# This is a known issue for MinGW :
-# https://github.com/google/shaderc/pull/174
-#if(MINGW)
- set(GTEST_DISABLE_PTHREADS ON)
-#endif()
+ # Get GTest source and binary directories from CMake project
-# Download GoogleTest
-ExternalProject_Add(googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG v1.8.x
- # TIMEOUT 1 # We'll try this
- CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
- -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
- -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
- -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
- -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
- -DBUILD_GTEST=ON
- PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
- # Disable install step
- INSTALL_COMMAND ""
- UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-)
+ # Specify include dir
+ ExternalProject_Get_Property(googletest source_dir)
+ set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-# Get GTest source and binary directories from CMake project
+ # Specify MainTest's link libraries
+ ExternalProject_Get_Property(googletest binary_dir)
+ set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+ # Create a libgtest target to be used as a dependency by test programs
+ add_library(libgtest IMPORTED STATIC GLOBAL)
+ add_dependencies(libgtest googletest)
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+ # Set libgtest properties
+ set_target_properties(libgtest PROPERTIES
+ "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+ "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+ )
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+else()
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
- "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
- "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+ message("CMake version equal or greater than 3.11.0")
+ include(FetchContent)
+
+ FetchContent_Declare(
+ googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG main)
+
+ #FetchContent_MakeAvailable(googletest)
+ # Disable installation of googletest: https://stackoverflow.com/questions/65527126/disable-install-for-fetchcontent
+ FetchContent_GetProperties(googletest)
+ if(NOT googletest_POPULATED)
+ FetchContent_Populate(googletest)
+ add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR} EXCLUDE_FROM_ALL)
+ endif()
+
+endif()
+
function(laser_scan_utils_add_gtest target)
add_executable(${target} ${ARGN})
- add_dependencies(${target} libgtest)
- target_link_libraries(${target} libgtest)
-
- #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+ if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+ add_dependencies(${target} libgtest)
+ target_link_libraries(${target} PUBLIC libgtest ${PROJECT_NAME})
+ target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
+ else()
+ target_link_libraries(${target} PUBLIC gtest_main ${PROJECT_NAME})
+ endif()
add_test(NAME ${target} COMMAND ${target})
endfunction()
diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index f4a2ad22f1dbdef70761beda8c5ddf899d14f76f..481f367b23eb6ed6fc91d90f0887e6ea7158b3c4 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -20,8 +20,8 @@
//
//--------LICENSE_END--------
#include "gtest/utils_gtest.h"
-#include "laser_scan.h"
-#include "icp.h"
+#include "laser_scan_utils/laser_scan.h"
+#include "laser_scan_utils/icp.h"
#define TEST_PLOTS false
#if TEST_PLOTS
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index 35f5cbaceba65f90140d5022f643fbc43195b0a9..56be534370a343e148965ca8d3eb3923645a94e2 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -19,10 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "loop_closure_base.h"
-#include "loop_closure_falko.h"
-#include "data/scan_data.h"
#include "gtest/utils_gtest.h"
+#include "data/scan_data.h"
+#include "laser_scan_utils/loop_closure_base.h"
+#include "laser_scan_utils/loop_closure_falko.h"
using namespace laserscanutils;