├── .clang-format ├── .editorconfig ├── .pre-commit-config.yaml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake_modules ├── FindG2O.cmake └── FindSUITESPARSE.cmake ├── config └── mapper_params.yaml ├── include └── line_segment_mapping │ ├── g2o_solver.h │ ├── line_segment_extractor.h │ ├── line_segment_feature.h │ ├── line_segment_map_manager.h │ ├── line_segment_mapper.h │ └── util.h ├── launch ├── demo.rviz └── line_segment_mapping.launch ├── package.xml └── src ├── g2o_solver.cpp ├── line_segment_extractor.cpp ├── line_segment_map_manager.cpp ├── line_segment_mapper.cpp └── slam_karto_g2o.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | --- 4 | Language: Cpp 5 | DerivePointerAlignment: false 6 | PointerAlignment: Left 7 | Cpp11BracedListStyle: true 8 | Standard: Cpp11 9 | CommentPragmas: '^ NOLINT' 10 | # Mimic cpplint style 11 | IncludeCategories: 12 | # Note that the "main" header is priority 0 13 | # The priority is assigned to first match in the ordered list 14 | # Miscelaneous system libraries 15 | - Regex: '<(immintrin.h|malloc.h|wait.h|x86intrin.h|cuda.*)>' 16 | Priority: 3 17 | # C standard libraries 18 | - Regex: '<(arpa/|netinet/|net/if|sys/)?[^\./]*\.h>' 19 | Priority: 1 20 | # C++ standard libraries 21 | - Regex: '<[^/\./]*>' 22 | Priority: 2 23 | # Experimental or other system libraries 24 | - Regex: '<' 25 | Priority: 3 26 | # Test libs 27 | - Regex: '"(gtest|gmock)/' 28 | Priority: 4 29 | # Protobuf Files 30 | - Regex: '\.pb\.h' 31 | Priority: 6 32 | # Line segment mapping libs 33 | - Regex: '^"(line_segment_mapping)' 34 | Priority: 7 35 | # The rest 36 | - Regex: '.*' 37 | Priority: 5 38 | --- 39 | -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | # http://editorconfig.org 2 | root = true 3 | 4 | [*] 5 | charset = utf-8 6 | # like -i=2 7 | indent_style = space 8 | indent_size = 2 9 | insert_final_newline = true 10 | trim_trailing_whitespace = true 11 | 12 | [*.{sh,bash,bashrc}] 13 | shell_variant = bash # like -ln=bash 14 | binary_next_line = true # like -bn 15 | switch_case_indent = true # like -ci 16 | space_redirects = true # like -sr 17 | 18 | # in case third party libraries need a Makefile. 19 | [Makefile] 20 | indent_style = tab 21 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # See https://pre-commit.com for more information 2 | # See https://pre-commit.com/hooks.html for more hooks 3 | repos: 4 | - repo: https://github.com/pre-commit/pre-commit-hooks 5 | rev: v3.2.0 6 | hooks: 7 | - id: trailing-whitespace 8 | - id: end-of-file-fixer 9 | - id: check-added-large-files 10 | - id: check-merge-conflict 11 | - repo: https://github.com/pre-commit/mirrors-clang-format 12 | rev: "v9.0.0" 13 | hooks: 14 | - id: clang-format 15 | types_or: [c++] 16 | - repo: https://github.com/cpplint/cpplint 17 | rev: 1.6.1 18 | hooks: 19 | - id: cpplint 20 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(line_segment_mapping) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | set(CMAKE_BUILD_TYPE Release) 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra") 9 | 10 | # Add new CMake Modules 11 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | cmake_modules 15 | geometry_msgs 16 | message_filters 17 | nav_msgs 18 | open_karto 19 | roscpp 20 | sensor_msgs 21 | tf 22 | visualization_msgs 23 | pcl_ros 24 | ) 25 | 26 | # Eigen 27 | find_package(Eigen3 REQUIRED) 28 | 29 | # g2o 30 | find_package(G2O REQUIRED) 31 | 32 | # suitesparse 33 | find_package(SUITESPARSE REQUIRED) 34 | 35 | # glog 36 | find_package(glog REQUIRED) 37 | 38 | catkin_package( 39 | INCLUDE_DIRS include 40 | LIBRARIES line_segment_mapping 41 | CATKIN_DEPENDS 42 | cmake_modules 43 | geometry_msgs 44 | message_filters 45 | nav_msgs 46 | open_karto 47 | roscpp 48 | sensor_msgs 49 | tf 50 | visualization_msgs 51 | pcl_ros 52 | ) 53 | 54 | include_directories( 55 | include 56 | ${catkin_INCLUDE_DIRS} 57 | ${G2O_INCLUDE_DIR} 58 | ${SUITESPARSE_INCLUDE_DIRS} 59 | ) 60 | 61 | add_library(${PROJECT_NAME} 62 | src/g2o_solver.cpp 63 | src/line_segment_extractor.cpp 64 | src/line_segment_map_manager.cpp 65 | src/line_segment_mapper.cpp 66 | ) 67 | target_link_libraries(${PROJECT_NAME} 68 | ${catkin_LIBRARIES} 69 | ${G2O_LIBRARIES} 70 | ${SUITESPARSE_LIBRARIES} 71 | glog::glog 72 | Eigen3::Eigen 73 | ) 74 | 75 | add_executable(${PROJECT_NAME}_node src/slam_karto_g2o.cpp) 76 | target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES}) 77 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, NKU Mobile & Flying Robotics Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 2-D Line Segment Mapping 2 | 3 | Source code of an incremental and consistent 2-D line segment-based mapping approach proposed in 4 | 5 | > J. Wen, X. Zhang, H. Gao, J. Yuan, and Y. Fang, "CAE-RLSM: Consistent and efficient redundant line segment merging for online feature map building,", ***IEEE Transactions on Instrumentation and Measurement***, 2020, 69(7): 4222-4237. [[paper]](https://ieeexplore.ieee.org/document/8882497) [[video]](https://youtu.be/025_dzmVGWY) 6 | 7 | The 2-D line segment feature is extarcted by a seeded region growing-based line segment extarction algorithm proposed in 8 | 9 | > H. Gao, X. Zhang, Y. Fang, and J. Yuan, "A line segment extraction algorithm using laser data based on seeded region growing," ***International Journal of Advanced Robotic Systems***, 2018, 15(1): 1-10. [[paper]](https://journals.sagepub.com/doi/full/10.1177/1729881418755245)[[code]](https://github.com/NKU-MobFly-Robotics/laser-line-segment) 10 | 11 | Please cite the above papers if you use this repo in your research. 12 | 13 | ## How to use 14 | 15 | This repo has been tested on Ubuntu 16.04 and 18.04. The line segment mapping module is integrated into a 2-D pose graph SLAM system, which uses open karto package as the front-end and g2o solver as the back-end. Please check out [this repo](https://github.com/nkuwenjian/slam_karto_g2o) to install g2o solver. 16 | 17 | After installing g2o solver, please create and initialize a ROS workspace. We assume that your workspace is named catkin_ws. Then, run the following commands to clone and build open karto package: 18 | ``` 19 | $ cd ~/catkin_ws/src/ 20 | $ git clone https://github.com/ros-perception/open_karto.git 21 | $ cd .. 22 | $ catkin_make -DCMAKE_BUILD_TYPE=Release 23 | ``` 24 | 25 | Please also install glog 26 | ``` 27 | sudo apt install libgoogle-glog-dev 28 | ``` 29 | 30 | After the above preparation, clone and build this package: 31 | ``` 32 | $ cd ~/catkin_ws/src/ 33 | $ git clone https://github.com/NKU-MobFly-Robotics/line_segment_mapping.git 34 | $ cd .. 35 | $ catkin_make -DCMAKE_BUILD_TYPE=Release 36 | ``` 37 | 38 | Finally, run the following commands to launch Karto SLAM with line segment mapping: 39 | ``` 40 | $ source ~/catkin_ws/devel/setup.bash 41 | $ roslaunch line_segment_mapping line_segment_mapping.launch 42 | ``` 43 | 44 | Open a new terminal and play your rosbag: 45 | ``` 46 | $ rosbag play --clock 47 | ``` 48 | -------------------------------------------------------------------------------- /cmake_modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Locate the g2o libraries 2 | # A general framework for graph optimization. 3 | # 4 | # This module defines 5 | # G2O_FOUND, if false, do not try to link against g2o 6 | # G2O_LIBRARIES, path to the libg2o 7 | # G2O_INCLUDE_DIR, where to find the g2o header files 8 | # 9 | # Niko Suenderhauf 10 | # Adapted by Felix Endres 11 | 12 | IF(UNIX) 13 | 14 | #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 15 | # in cache already 16 | # SET(G2O_FIND_QUIETLY TRUE) 17 | #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 18 | 19 | MESSAGE(STATUS "Searching for g2o ...") 20 | FIND_PATH(G2O_INCLUDE_DIR 21 | NAMES core math_groups types 22 | PATHS /usr/local /usr 23 | PATH_SUFFIXES include/g2o include) 24 | 25 | IF (G2O_INCLUDE_DIR) 26 | MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}") 27 | ENDIF (G2O_INCLUDE_DIR) 28 | 29 | FIND_LIBRARY(G2O_CORE_LIB 30 | NAMES g2o_core g2o_core_rd 31 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 32 | PATH_SUFFIXES lib) 33 | FIND_LIBRARY(G2O_STUFF_LIB 34 | NAMES g2o_stuff g2o_stuff_rd 35 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 36 | PATH_SUFFIXES lib) 37 | FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB 38 | NAMES g2o_types_slam2d g2o_types_slam2d_rd 39 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 40 | PATH_SUFFIXES lib) 41 | FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB 42 | NAMES g2o_types_slam3d g2o_types_slam3d_rd 43 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 44 | PATH_SUFFIXES lib) 45 | FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB 46 | NAMES g2o_solver_cholmod g2o_solver_cholmod_rd 47 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 48 | PATH_SUFFIXES lib) 49 | FIND_LIBRARY(G2O_SOLVER_PCG_LIB 50 | NAMES g2o_solver_pcg g2o_solver_pcg_rd 51 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 52 | PATH_SUFFIXES lib) 53 | FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB 54 | NAMES g2o_solver_csparse g2o_solver_csparse_rd 55 | PATHS /usr/local /usr 56 | PATH_SUFFIXES lib) 57 | FIND_LIBRARY(G2O_INCREMENTAL_LIB 58 | NAMES g2o_incremental g2o_incremental_rd 59 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 60 | PATH_SUFFIXES lib) 61 | FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB 62 | NAMES g2o_csparse_extension g2o_csparse_extension_rd 63 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 64 | PATH_SUFFIXES lib) 65 | 66 | SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB} 67 | ${G2O_CORE_LIB} 68 | ${G2O_STUFF_LIB} 69 | ${G2O_TYPES_SLAM2D_LIB} 70 | ${G2O_TYPES_SLAM3D_LIB} 71 | ${G2O_SOLVER_CHOLMOD_LIB} 72 | ${G2O_SOLVER_PCG_LIB} 73 | ${G2O_SOLVER_CSPARSE_LIB} 74 | ${G2O_INCREMENTAL_LIB} 75 | ) 76 | 77 | IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 78 | SET(G2O_FOUND "YES") 79 | IF(NOT G2O_FIND_QUIETLY) 80 | MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}") 81 | ENDIF(NOT G2O_FIND_QUIETLY) 82 | ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 83 | IF(NOT G2O_LIBRARIES) 84 | IF(G2O_FIND_REQUIRED) 85 | message(FATAL_ERROR "Could not find libg2o!") 86 | ENDIF(G2O_FIND_REQUIRED) 87 | ENDIF(NOT G2O_LIBRARIES) 88 | 89 | IF(NOT G2O_INCLUDE_DIR) 90 | IF(G2O_FIND_REQUIRED) 91 | message(FATAL_ERROR "Could not find g2o include directory!") 92 | ENDIF(G2O_FIND_REQUIRED) 93 | ENDIF(NOT G2O_INCLUDE_DIR) 94 | ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 95 | 96 | ENDIF(UNIX) 97 | -------------------------------------------------------------------------------- /cmake_modules/FindSUITESPARSE.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find SUITESPARSE 2 | # Once done this will define 3 | # 4 | # SUITESPARSE_FOUND - system has SUITESPARSE 5 | # SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory 6 | # SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE 7 | # SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package) 8 | # SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package) 9 | # SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs 10 | # SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs 11 | # SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly 12 | 13 | IF (SUITESPARSE_INCLUDE_DIRS) 14 | # Already in cache, be silent 15 | SET(SUITESPARSE_FIND_QUIETLY TRUE) 16 | ENDIF (SUITESPARSE_INCLUDE_DIRS) 17 | 18 | if( WIN32 ) 19 | # Find cholmod part of the suitesparse library collection 20 | 21 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 22 | PATHS "C:\\libs\\win32\\SuiteSparse\\Include" ) 23 | 24 | # Add cholmod include directory to collection include directories 25 | IF ( CHOLMOD_INCLUDE_DIR ) 26 | list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) 27 | ENDIF( CHOLMOD_INCLUDE_DIR ) 28 | 29 | 30 | # find path suitesparse library 31 | FIND_PATH( SUITESPARSE_LIBRARY_DIRS 32 | amd.lib 33 | PATHS "C:\\libs\\win32\\SuiteSparse\\libs" ) 34 | 35 | # if we found the library, add it to the defined libraries 36 | IF ( SUITESPARSE_LIBRARY_DIRS ) 37 | list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd ) 38 | ENDIF( SUITESPARSE_LIBRARY_DIRS ) 39 | 40 | else( WIN32 ) 41 | IF(APPLE) 42 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 43 | PATHS /opt/local/include/ufsparse 44 | /usr/local/include ) 45 | 46 | FIND_PATH( SUITESPARSE_LIBRARY_DIR 47 | NAMES libcholmod.a 48 | PATHS /opt/local/lib 49 | /usr/local/lib ) 50 | ELSE(APPLE) 51 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 52 | PATHS /usr/local/include 53 | /usr/include 54 | /usr/include/suitesparse/ 55 | ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod 56 | PATH_SUFFIXES cholmod/ CHOLMOD/ ) 57 | 58 | FIND_PATH( SUITESPARSE_LIBRARY_DIR 59 | NAMES libcholmod.so libcholmod.a 60 | PATHS /usr/lib 61 | /usr/lib64 62 | /usr/lib/x86_64-linux-gnu 63 | /usr/lib/i386-linux-gnu 64 | /usr/local/lib 65 | /usr/lib/arm-linux-gnueabihf/ 66 | /usr/lib/aarch64-linux-gnu/ 67 | /usr/lib/arm-linux-gnueabi/ 68 | /usr/lib/arm-linux-gnu) 69 | ENDIF(APPLE) 70 | 71 | # Add cholmod include directory to collection include directories 72 | IF ( CHOLMOD_INCLUDE_DIR ) 73 | list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) 74 | ENDIF( CHOLMOD_INCLUDE_DIR ) 75 | 76 | # if we found the library, add it to the defined libraries 77 | IF ( SUITESPARSE_LIBRARY_DIR ) 78 | 79 | list ( APPEND SUITESPARSE_LIBRARIES amd) 80 | list ( APPEND SUITESPARSE_LIBRARIES btf) 81 | list ( APPEND SUITESPARSE_LIBRARIES camd) 82 | list ( APPEND SUITESPARSE_LIBRARIES ccolamd) 83 | list ( APPEND SUITESPARSE_LIBRARIES cholmod) 84 | list ( APPEND SUITESPARSE_LIBRARIES colamd) 85 | # list ( APPEND SUITESPARSE_LIBRARIES csparse) 86 | list ( APPEND SUITESPARSE_LIBRARIES cxsparse) 87 | list ( APPEND SUITESPARSE_LIBRARIES klu) 88 | # list ( APPEND SUITESPARSE_LIBRARIES spqr) 89 | list ( APPEND SUITESPARSE_LIBRARIES umfpack) 90 | 91 | IF (APPLE) 92 | list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig) 93 | ENDIF (APPLE) 94 | 95 | # Metis and spqr are optional 96 | FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY 97 | NAMES metis 98 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 99 | IF (SUITESPARSE_METIS_LIBRARY) 100 | list ( APPEND SUITESPARSE_LIBRARIES metis) 101 | ENDIF(SUITESPARSE_METIS_LIBRARY) 102 | 103 | if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp") 104 | SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid") 105 | else() 106 | SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid") 107 | endif() 108 | 109 | if(SUITESPARSE_SPQR_VALID) 110 | FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY 111 | NAMES spqr 112 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 113 | IF (SUITESPARSE_SPQR_LIBRARY) 114 | list ( APPEND SUITESPARSE_LIBRARIES spqr) 115 | ENDIF (SUITESPARSE_SPQR_LIBRARY) 116 | endif() 117 | 118 | ENDIF( SUITESPARSE_LIBRARY_DIR ) 119 | 120 | endif( WIN32 ) 121 | 122 | 123 | IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 124 | IF(WIN32) 125 | list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig ) 126 | ENDIF(WIN32) 127 | SET(SUITESPARSE_FOUND TRUE) 128 | MESSAGE(STATUS "Found SuiteSparse") 129 | ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 130 | SET( SUITESPARSE_FOUND FALSE ) 131 | MESSAGE(FATAL_ERROR "Unable to find SuiteSparse") 132 | ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 133 | -------------------------------------------------------------------------------- /config/mapper_params.yaml: -------------------------------------------------------------------------------- 1 | # General Parameters 2 | use_scan_matching: True 3 | use_scan_barycenter: True 4 | minimum_travel_distance: 0.2 5 | minimum_travel_heading: 0.174 #in radians 6 | scan_buffer_size: 70 7 | scan_buffer_maximum_scan_distance: 20.0 8 | link_match_minimum_response_fine: 0.8 9 | link_scan_maximum_distance: 10.0 10 | loop_search_maximum_distance: 4.0 11 | do_loop_closing: True 12 | loop_match_minimum_chain_size: 10 13 | loop_match_maximum_variance_coarse: 0.4 # gets squared later 14 | loop_match_minimum_response_coarse: 0.8 15 | loop_match_minimum_response_fine: 0.8 16 | 17 | # Correlation Parameters - Correlation Parameters 18 | correlation_search_space_dimension: 0.3 19 | correlation_search_space_resolution: 0.01 20 | correlation_search_space_smear_deviation: 0.03 21 | 22 | # Correlation Parameters - Loop Closure Parameters 23 | loop_search_space_dimension: 8.0 24 | loop_search_space_resolution: 0.05 25 | loop_search_space_smear_deviation: 0.03 26 | 27 | # Scan Matcher Parameters 28 | distance_variance_penalty: 0.3 # gets squared later 29 | angle_variance_penalty: 0.349 # in degrees (gets converted to radians then squared) 30 | fine_search_angle_offset: 0.00349 # in degrees (gets converted to radians) 31 | coarse_search_angle_offset: 0.349 # in degrees (gets converted to radians) 32 | coarse_angle_resolution: 0.0349 # in degrees (gets converted to radians) 33 | minimum_angle_penalty: 0.9 34 | minimum_distance_penalty: 0.5 35 | use_response_expansion: False 36 | -------------------------------------------------------------------------------- /include/line_segment_mapping/g2o_solver.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include "g2o/core/sparse_optimizer.h" 35 | #include "open_karto/Mapper.h" 36 | #include "visualization_msgs/MarkerArray.h" 37 | 38 | namespace line_segment_mapping { 39 | 40 | class G2oSolver : public karto::ScanSolver { 41 | public: 42 | G2oSolver(); 43 | ~G2oSolver() override; 44 | 45 | void Clear() override; 46 | void Compute() override; 47 | const karto::ScanSolver::IdPoseVector& GetCorrections() const override; 48 | 49 | void AddNode(karto::Vertex* vertex) override; 50 | void AddConstraint(karto::Edge* edge) override; 51 | 52 | void PublishGraphVisualization(visualization_msgs::MarkerArray* marker_array); 53 | 54 | private: 55 | karto::ScanSolver::IdPoseVector corrections_; 56 | g2o::SparseOptimizer optimizer_; 57 | }; 58 | 59 | } // namespace line_segment_mapping 60 | -------------------------------------------------------------------------------- /include/line_segment_mapping/line_segment_extractor.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "open_karto/Karto.h" 40 | #include "open_karto/Math.h" 41 | 42 | #include "line_segment_mapping/line_segment_feature.h" 43 | 44 | namespace line_segment_mapping { 45 | 46 | typedef struct PointReadings { 47 | karto::PointVectorDouble points; 48 | std::vector ranges; 49 | std::vector angles; 50 | int num; 51 | } PointReadings; 52 | 53 | // 参数，从launch文件中读入 54 | typedef struct Params { 55 | double least_thresh; // 正交拟合阈值 56 | double min_line_length; // 拟合线段最短距离 57 | double predict_distance; // 真实点与与预测点之间的距离阈值 58 | int min_line_points; // 一条线段包含的激光点个数 59 | int seed_line_points; // 种子线段包含的激光点个数 60 | double max_line_gap; // 一条线段连续两点之间的最大距离 61 | } Params; 62 | 63 | // 直线段信息结构体 64 | typedef struct line { 65 | double a; // 直线参数 66 | double b; 67 | double c; 68 | int left; // 直线范围 69 | int right; 70 | } line; 71 | 72 | // 直线方程式结构体 73 | typedef struct least { 74 | double a; 75 | double b; 76 | double c; 77 | } least; 78 | 79 | class LineSegmentExtractor { 80 | public: 81 | // Constructor 82 | LineSegmentExtractor(); 83 | // Destructor 84 | ~LineSegmentExtractor() = default; 85 | 86 | public: 87 | void extract_lines(const karto::RangeReadingsVector& point_readings, 88 | const karto::LaserRangeFinder* laser_range_finder, 89 | LineSegmentPtrVector* line_segments); 90 | // 设置参数 91 | void set_least_threshold(double least_thresh); 92 | void set_min_line_length(double min_line_length); 93 | void set_predict_distance(double predict_distance); 94 | void set_min_line_points(int min_line_points); 95 | void set_seed_line_points(int seed_line_points); 96 | void set_max_line_gap(double max_line_gap); 97 | 98 | private: 99 | // 初始化参数 100 | void load_parameters(); 101 | // 通过激光数据的首末索引值进行直线方程的求解 102 | least least_square(int start, int end, int firstfit, 103 | const PointReadings& data); 104 | // 检测种子直线 105 | bool detect_line(int start, int num, const PointReadings& data); 106 | // 通过种子直线，复原出整条直线，并进行最后的求定 107 | int detect_full_line(int start, const PointReadings& data); 108 | // 整理整条直线 109 | void clean_line(const PointReadings& data); 110 | // 删除小于长度阈值的线段 111 | bool delete_short_line(int n1, int n2, const PointReadings& data); 112 | // 113 | void generate(const PointReadings& data, LineSegmentPtrVector* line_segments); 114 | 115 | // 判断线段端点是否是有序的 116 | void is_sequential(); 117 | 118 | void range_filtering(const karto::RangeReadingsVector& point_readings, 119 | const karto::LaserRangeFinder* laser_range_finder, 120 | PointReadings* data); 121 | void process(const PointReadings& data, LineSegmentPtrVector* line_segments); 122 | 123 | private: 124 | Params params_; 125 | // 线段结构体信息 126 | std::vector m_line; 127 | // 直线拟合中间传递变量，已设为全局变量 128 | least m_least; 129 | // 拟合中间变量 130 | double mid1; 131 | double mid2; 132 | double mid3; 133 | double mid4; 134 | double mid5; 135 | }; 136 | 137 | } // namespace line_segment_mapping 138 | -------------------------------------------------------------------------------- /include/line_segment_mapping/line_segment_feature.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "glog/logging.h" 39 | #include "open_karto/Karto.h" 40 | 41 | #include "line_segment_mapping/util.h" 42 | 43 | namespace line_segment_mapping { 44 | 45 | class LineSegment { 46 | public: 47 | LineSegment() {} 48 | 49 | LineSegment(const karto::Vector2& startPoint, 50 | const karto::Vector2& endPoint) { 51 | m_StartPoint = startPoint; 52 | m_EndPoint = endPoint; 53 | m_Barycenter = (m_StartPoint + m_EndPoint) / 2; 54 | m_DirectionVector = m_EndPoint - m_StartPoint; 55 | CHECK(!std::isnan(m_DirectionVector.GetX()) && 56 | !std::isnan(m_DirectionVector.GetY())); // 排除NaN值的情况 57 | 58 | m_Length = m_DirectionVector.Length(); 59 | CHECK_GE(m_Length, kDoubleEpsilon); 60 | 61 | m_Heading = atan2(m_DirectionVector.GetY(), m_DirectionVector.GetX()); 62 | CHECK(karto::math::InRange(m_Heading, -M_PI, M_PI)); 63 | m_UpdateTimes = 1; 64 | 65 | double A = m_EndPoint.GetY() - m_StartPoint.GetY(); 66 | double B = m_StartPoint.GetX() - m_EndPoint.GetX(); 67 | double C = m_EndPoint.GetX() * m_StartPoint.GetY() - 68 | m_StartPoint.GetX() * m_EndPoint.GetY(); 69 | m_Role = std::abs(C) / std::sqrt(A * A + B * B); 70 | 71 | double x = -A * C / (A * A + B * B); 72 | double y = -B * C / (A * A + B * B); 73 | m_Phi = atan2(y, x); 74 | 75 | m_pScan = nullptr; 76 | m_ClusterIndex = -1; 77 | } 78 | 79 | LineSegment(const LineSegment& rOther) { 80 | m_StartPoint = rOther.m_StartPoint; 81 | m_EndPoint = rOther.m_EndPoint; 82 | m_Barycenter = rOther.m_Barycenter; 83 | m_DirectionVector = rOther.m_DirectionVector; 84 | m_Length = rOther.m_Length; 85 | m_Heading = rOther.m_Heading; 86 | m_UpdateTimes = rOther.m_UpdateTimes; 87 | m_Role = rOther.m_Role; 88 | m_Phi = rOther.m_Phi; 89 | 90 | m_pScan = nullptr; 91 | m_ClusterIndex = -1; 92 | } 93 | 94 | virtual ~LineSegment() { m_pScan = nullptr; } 95 | 96 | public: 97 | const karto::Vector2& GetStartPoint() const { return m_StartPoint; } 98 | 99 | const karto::Vector2& GetEndPoint() const { return m_EndPoint; } 100 | 101 | const karto::Vector2& GetBarycenter() const { return m_Barycenter; } 102 | 103 | const karto::Vector2& GetDirectionVector() const { 104 | return m_DirectionVector; 105 | } 106 | 107 | double GetHeading() const { return m_Heading; } 108 | 109 | double GetLength() const { return m_Length; } 110 | 111 | int GetUpdateTimes() const { return m_UpdateTimes; } 112 | 113 | void SetUpdateTimes(int updateTimes) { m_UpdateTimes = updateTimes; } 114 | 115 | double GetRole() const { return m_Role; } 116 | 117 | double GetPhi() const { return m_Phi; } 118 | 119 | void SetScan(karto::LocalizedRangeScan* pScan) { m_pScan = pScan; } 120 | 121 | const karto::LocalizedRangeScan* GetScan() const { return m_pScan; } 122 | 123 | void SetLineSegmentClusterIndex(int lineSegmentClusterIndex) { 124 | m_ClusterIndex = lineSegmentClusterIndex; 125 | } 126 | 127 | const int& GetLineSegmentClusterIndex() const { return m_ClusterIndex; } 128 | 129 | LineSegment GetGlobalLineSegments() const { 130 | karto::Pose2 scanPose = m_pScan->GetSensorPose(); 131 | double cosine = std::cos(scanPose.GetHeading()); 132 | double sine = std::sin(scanPose.GetHeading()); 133 | 134 | karto::Vector2 startPoint, endPoint; 135 | 136 | startPoint.SetX(m_StartPoint.GetX() * cosine - m_StartPoint.GetY() * sine + 137 | scanPose.GetX()); 138 | startPoint.SetY(m_StartPoint.GetX() * sine + m_StartPoint.GetY() * cosine + 139 | scanPose.GetY()); 140 | endPoint.SetX(m_EndPoint.GetX() * cosine - m_EndPoint.GetY() * sine + 141 | scanPose.GetX()); 142 | endPoint.SetY(m_EndPoint.GetX() * sine + m_EndPoint.GetY() * cosine + 143 | scanPose.GetY()); 144 | 145 | LineSegment lineSegment(startPoint, endPoint); 146 | return lineSegment; 147 | } 148 | 149 | private: 150 | karto::Vector2 m_StartPoint; 151 | karto::Vector2 m_EndPoint; 152 | 153 | double m_Heading; // 线段方向 154 | double m_Length; 155 | karto::Vector2 m_Barycenter; // 线段的中心点 156 | karto::Vector2 m_DirectionVector; // 单位方向向量 157 | 158 | const karto::LocalizedRangeScan* m_pScan; // 该线段关联的激光扫描 159 | int m_ClusterIndex; 160 | 161 | int m_UpdateTimes; // 线段被更新的次数，初始为1 162 | double m_Role; 163 | double m_Phi; 164 | }; 165 | 166 | typedef std::vector LineSegmentVector; 167 | typedef std::shared_ptr LineSegmentPtr; 168 | typedef std::vector LineSegmentPtrVector; 169 | typedef std::unordered_map LineSegmentPtrHashTable; 170 | typedef std::unordered_map 171 | LineSegmentPtrVectorHashTable; 172 | 173 | } // namespace line_segment_mapping 174 | -------------------------------------------------------------------------------- /include/line_segment_mapping/line_segment_map_manager.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | 36 | #include "open_karto/Karto.h" 37 | 38 | #include "line_segment_mapping/line_segment_feature.h" 39 | 40 | namespace line_segment_mapping { 41 | /** 42 | * An incremental and consistent line segment mapping module, please refer the 43 | * following paper for more detail J. Wen, X. Zhang, H. Gao, J. Yuan, Y. Fang, 44 | * "CAE_RLSM: Consistent and efficient redundant line segment merging for online 45 | * feature map building", IEEE Transactions on Instrumentation and Measurement, 46 | * 2020, 69(7): 4222-4237. 47 | */ 48 | class LineSegmentMapManager { 49 | public: 50 | LineSegmentMapManager() = default; 51 | virtual ~LineSegmentMapManager() = default; 52 | 53 | /** 54 | * Incremental line segment mapping based on one-to-one redundant line segment 55 | * merging strategy 56 | */ 57 | bool NaiveMerge(const LineSegmentPtrVector& rLineSegments); 58 | 59 | /** 60 | * Incremental line segment mapping based on one-to-many redundant line 61 | * segment merging strategy 62 | */ 63 | bool IncrementalMerge(const LineSegmentPtrVector& rLineSegments); 64 | 65 | /** 66 | * Global map adjustment based on the optimized robot poses 67 | */ 68 | void GlobalMapAdjustment(); 69 | 70 | const LineSegmentPtrHashTable& GetLineSegmentMap() const { 71 | return m_LineSegmentMap; 72 | } 73 | 74 | const LineSegmentPtrVectorHashTable& GetLineSegmentClusters() const { 75 | return m_LineSegmentClusters; 76 | } 77 | 78 | const std::vector& GetClustersIndexArray() const { 79 | return m_ClustersIndexArray; 80 | } 81 | 82 | private: 83 | /** 84 | * Transform the current line segments to the world coordinates, and calculate 85 | * the overlap with the global line segment map 86 | */ 87 | bool CalculateOverlap(const LineSegment& prevLineSegment, 88 | const LineSegment& currLineSegment); 89 | 90 | void MapAdjustment(int index); 91 | 92 | /** 93 | * Merge linesegments 94 | */ 95 | LineSegment* MergeLineSegments(const std::vector& rLineSegments); 96 | 97 | bool updateCheck(); 98 | 99 | private: 100 | LineSegmentPtrHashTable 101 | m_LineSegmentMap; // Global line segment map, wherein each line segment 102 | // is represented in the world coordinates 103 | LineSegmentPtrVectorHashTable 104 | m_LineSegmentClusters; // A hashtable used to record the line segment 105 | // cluster and the corresponding original line 106 | // segments 107 | std::vector m_ClustersIndexArray; // An array used to store the indices 108 | // of the line segment cluster 109 | int m_LineSegmentClustersIndex = 0; // Index of the line segment cluster 110 | }; 111 | 112 | } // namespace line_segment_mapping 113 | -------------------------------------------------------------------------------- /include/line_segment_mapping/line_segment_mapper.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | 37 | #include "open_karto/Mapper.h" 38 | 39 | #include "line_segment_mapping/line_segment_map_manager.h" 40 | 41 | namespace line_segment_mapping { 42 | 43 | class LineSegmentMapper : public karto::Mapper { 44 | public: 45 | /** 46 | * Default constructor 47 | */ 48 | LineSegmentMapper(); 49 | 50 | /** 51 | * Constructor a mapper with a name 52 | * @param rName mapper name 53 | */ 54 | explicit LineSegmentMapper(const std::string& rName); 55 | 56 | /** 57 | * Destructor 58 | */ 59 | virtual ~LineSegmentMapper() = default; 60 | 61 | public: 62 | /** 63 | * Process a localized range scan for incorporation into the map. The scan 64 | * must be identified with a range finder device. Once added to a map, the 65 | * corrected pose information in the localized scan will be updated to the 66 | * correct pose as determined by the mapper. 67 | * 68 | * @param pScan A localized range scan that has pose information associated 69 | * directly with the scan data. The pose is that of the range device 70 | * originating the scan. Note that the mapper will set corrected pose 71 | * information in the scan object. 72 | * 73 | * @return true if the scan was added successfully, false otherwise 74 | */ 75 | virtual bool Process(karto::LocalizedRangeScan* pScan, 76 | const LineSegmentPtrVector& rLineSegments); 77 | 78 | virtual LineSegmentMapManager* GetLineSegmentMapManager() const { 79 | return m_pLineSegmentMapManager.get(); 80 | } 81 | 82 | private: 83 | /** 84 | * Test if the scan is "sufficiently far" from the last scan added. 85 | * @param pScan scan to be checked 86 | * @param pLastScan last scan added to mapper 87 | * @return true if the scan is "sufficiently far" from the last scan added or 88 | * the scan is the first scan to be added 89 | */ 90 | bool HasMovedEnough(karto::LocalizedRangeScan* pScan, 91 | karto::LocalizedRangeScan* pLastScan); 92 | 93 | private: 94 | /** 95 | * Restrict the copy constructor 96 | */ 97 | LineSegmentMapper(const LineSegmentMapper&); 98 | 99 | /** 100 | * Restrict the assignment operator 101 | */ 102 | const LineSegmentMapper& operator=(const LineSegmentMapper&); 103 | 104 | private: 105 | std::unique_ptr m_pLineSegmentMapManager = 106 | nullptr; // 新增加的模块 107 | bool m_Initialized = false; 108 | }; 109 | 110 | } // namespace line_segment_mapping 111 | -------------------------------------------------------------------------------- /include/line_segment_mapping/util.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | * 31 | * Author: Jian Wen (nkuwenjian@gmail.com) 32 | *****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | #define UNUSED(var) (void)(var) 39 | 40 | namespace line_segment_mapping { 41 | 42 | static constexpr double kDoubleEpsilon = 1e-6; 43 | 44 | inline double point_to_line_distance(double A, double B, double C, double x, 45 | double y) { 46 | return std::abs(A * x + B * y + C) / std::sqrt(A * A + B * B); 47 | } 48 | 49 | } // namespace line_segment_mapping 50 | -------------------------------------------------------------------------------- /launch/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 775 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 0.699999988 54 | Class: rviz/Map 55 | Color Scheme: map 56 | Draw Behind: false 57 | Enabled: true 58 | Name: Map 59 | Topic: /map 60 | Unreliable: false 61 | Use Timestamp: false 62 | Value: true 63 | - Class: rviz/Marker 64 | Enabled: true 65 | Marker Topic: /line_markers 66 | Name: Marker 67 | Namespaces: 68 | karto: true 69 | Queue Size: 100 70 | Value: true 71 | - Class: rviz/TF 72 | Enabled: true 73 | Frame Timeout: 15 74 | Frames: 75 | All Enabled: true 76 | base_link: 77 | Value: true 78 | laser: 79 | Value: true 80 | map: 81 | Value: true 82 | odom: 83 | Value: true 84 | Marker Scale: 1 85 | Name: TF 86 | Show Arrows: true 87 | Show Axes: true 88 | Show Names: true 89 | Tree: 90 | map: 91 | odom: 92 | base_link: 93 | laser: 94 | {} 95 | Update Interval: 0 96 | Value: true 97 | Enabled: true 98 | Global Options: 99 | Background Color: 48; 48; 48 100 | Default Light: true 101 | Fixed Frame: map 102 | Frame Rate: 30 103 | Name: root 104 | Tools: 105 | - Class: rviz/Interact 106 | Hide Inactive Objects: true 107 | - Class: rviz/MoveCamera 108 | - Class: rviz/Select 109 | - Class: rviz/FocusCamera 110 | - Class: rviz/Measure 111 | - Class: rviz/SetInitialPose 112 | Topic: /initialpose 113 | - Class: rviz/SetGoal 114 | Topic: /move_base_simple/goal 115 | - Class: rviz/PublishPoint 116 | Single click: true 117 | Topic: /clicked_point 118 | Value: true 119 | Views: 120 | Current: 121 | Class: rviz/Orbit 122 | Distance: 45.4522896 123 | Enable Stereo Rendering: 124 | Stereo Eye Separation: 0.0599999987 125 | Stereo Focal Distance: 1 126 | Swap Stereo Eyes: false 127 | Value: false 128 | Focal Point: 129 | X: -6.94073915 130 | Y: 1.23576283 131 | Z: 0.00123576762 132 | Focal Shape Fixed Size: true 133 | Focal Shape Size: 0.0500000007 134 | Invert Z Axis: false 135 | Name: Current View 136 | Near Clip Distance: 0.00999999978 137 | Pitch: 1.56979632 138 | Target Frame: 139 | Value: Orbit (rviz) 140 | Yaw: 4.71238804 141 | Saved: ~ 142 | Window Geometry: 143 | Displays: 144 | collapsed: false 145 | Height: 1056 146 | Hide Left Dock: false 147 | Hide Right Dock: false 148 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007430000003efc0100000002fb0000000800540069006d00650100000000000007430000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004be0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 149 | Selection: 150 | collapsed: false 151 | Time: 152 | collapsed: false 153 | Tool Properties: 154 | collapsed: false 155 | Views: 156 | collapsed: false 157 | Width: 1859 158 | X: 61 159 | Y: 24 160 | -------------------------------------------------------------------------------- /launch/line_segment_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | line_segment_mapping 4 | 0.0.0 5 | The line_segment_mapping package 6 | 7 | 8 | 9 | 10 | user 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | cmake_modules 53 | geometry_msgs 54 | message_filters 55 | nav_msgs 56 | open_karto 57 | roscpp 58 | sensor_msgs 59 | tf 60 | visualization_msgs 61 | pcl_ros 62 | cmake_modules 63 | geometry_msgs 64 | message_filters 65 | nav_msgs 66 | open_karto 67 | roscpp 68 | sensor_msgs 69 | tf 70 | visualization_msgs 71 | pcl_ros 72 | cmake_modules 73 | geometry_msgs 74 | message_filters 75 | nav_msgs 76 | open_karto 77 | roscpp 78 | sensor_msgs 79 | tf 80 | visualization_msgs 81 | pcl_ros 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /src/g2o_solver.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "line_segment_mapping/g2o_solver.h" 33 | 34 | #include 35 | 36 | #include "g2o/core/block_solver.h" 37 | #include "g2o/core/factory.h" 38 | #include "g2o/core/optimization_algorithm_factory.h" 39 | #include "g2o/core/optimization_algorithm_gauss_newton.h" 40 | #include "g2o/core/optimization_algorithm_levenberg.h" 41 | #include "g2o/solvers/csparse/linear_solver_csparse.h" 42 | #include "g2o/types/slam2d/types_slam2d.h" 43 | #include "ros/console.h" 44 | #include "tf/tf.h" 45 | 46 | namespace line_segment_mapping { 47 | 48 | // Typedef for the block solver utilized for optimization 49 | using BlockSolver = g2o::BlockSolver>; 50 | 51 | // Typedef for the linear solver utilized for optimization 52 | using LinearSolver = g2o::LinearSolverCSparse; 53 | 54 | G2oSolver::G2oSolver() { 55 | // Initialize the SparseOptimizer 56 | auto linear_solver = std::make_unique(); 57 | linear_solver->setBlockOrdering(false); 58 | auto block_solver = std::make_unique(std::move(linear_solver)); 59 | auto* solver = 60 | new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver)); 61 | 62 | optimizer_.setAlgorithm(solver); 63 | } 64 | 65 | G2oSolver::~G2oSolver() { 66 | // freeing the graph memory 67 | optimizer_.clear(); 68 | } 69 | 70 | void G2oSolver::Clear() { 71 | // freeing the graph memory 72 | LOG(INFO) << "Clearing Optimizer..."; 73 | corrections_.clear(); 74 | } 75 | 76 | void G2oSolver::Compute() { 77 | corrections_.clear(); 78 | 79 | // Fix the first node in the graph to hold the map in place 80 | g2o::OptimizableGraph::Vertex* first = optimizer_.vertex(0); 81 | if (first == nullptr) { 82 | LOG(ERROR) << "No Node with ID 0 found!"; 83 | return; 84 | } 85 | first->setFixed(true); 86 | 87 | // Do the graph optimization 88 | optimizer_.initializeOptimization(); 89 | int iter = optimizer_.optimize(40); 90 | if (iter > 0) { 91 | LOG(INFO) << "Optimization finished after " << iter << " iterations."; 92 | } else { 93 | LOG(ERROR) << "Optimization failed, result might be invalid!"; 94 | return; 95 | } 96 | 97 | // Write the result so it can be used by the mapper 98 | const g2o::SparseOptimizer::VertexContainer& nodes = 99 | optimizer_.activeVertices(); 100 | for (const auto* node : nodes) { 101 | std::array estimate; 102 | if (node->getEstimateData(estimate.data())) { 103 | karto::Pose2 pose(estimate[0], estimate[1], estimate[2]); 104 | corrections_.emplace_back(node->id(), pose); 105 | } else { 106 | LOG(ERROR) << "Could not get estimated pose from Optimizer!"; 107 | } 108 | } 109 | } 110 | 111 | void G2oSolver::AddNode(karto::Vertex* vertex) { 112 | const karto::Pose2& odom = vertex->GetObject()->GetCorrectedPose(); 113 | auto* pose_vertex = new g2o::VertexSE2(); 114 | pose_vertex->setEstimate( 115 | g2o::SE2(odom.GetX(), odom.GetY(), odom.GetHeading())); 116 | pose_vertex->setId(vertex->GetObject()->GetUniqueId()); 117 | optimizer_.addVertex(pose_vertex); 118 | VLOG(4) << "Adding node " << vertex->GetObject()->GetUniqueId() << "."; 119 | } 120 | 121 | void G2oSolver::AddConstraint(karto::Edge* edge) { 122 | // Create a new edge 123 | auto* odometry = new g2o::EdgeSE2(); 124 | 125 | // Set source and target 126 | int source_id = edge->GetSource()->GetObject()->GetUniqueId(); 127 | int target_id = edge->GetTarget()->GetObject()->GetUniqueId(); 128 | odometry->vertices()[0] = optimizer_.vertex(source_id); 129 | odometry->vertices()[1] = optimizer_.vertex(target_id); 130 | if (odometry->vertices()[0] == nullptr) { 131 | LOG(ERROR) << "Source vertex with id " << source_id << " does not exist!"; 132 | free(odometry); 133 | return; 134 | } 135 | if (odometry->vertices()[1] == nullptr) { 136 | LOG(ERROR) << "Target vertex with id " << target_id << " does not exist!"; 137 | free(odometry); 138 | return; 139 | } 140 | 141 | // Set the measurement (odometry distance between vertices) 142 | auto* link_info = dynamic_cast(edge->GetLabel()); 143 | karto::Pose2 diff = link_info->GetPoseDifference(); 144 | g2o::SE2 measurement(diff.GetX(), diff.GetY(), diff.GetHeading()); 145 | odometry->setMeasurement(measurement); 146 | 147 | // Set the covariance of the measurement 148 | karto::Matrix3 precision_matrix = link_info->GetCovariance().Inverse(); 149 | Eigen::Matrix info; 150 | info(0, 0) = precision_matrix(0, 0); 151 | info(0, 1) = info(1, 0) = precision_matrix(0, 1); 152 | info(0, 2) = info(2, 0) = precision_matrix(0, 2); 153 | info(1, 1) = precision_matrix(1, 1); 154 | info(1, 2) = info(2, 1) = precision_matrix(1, 2); 155 | info(2, 2) = precision_matrix(2, 2); 156 | odometry->setInformation(info); 157 | 158 | // Add the constraint to the optimizer 159 | VLOG(4) << "Adding edge from node " << source_id << " to node " << target_id 160 | << "."; 161 | optimizer_.addEdge(odometry); 162 | } 163 | 164 | const karto::ScanSolver::IdPoseVector& G2oSolver::GetCorrections() const { 165 | return corrections_; 166 | } 167 | 168 | /** 169 | * Fill out a visualization_msg to display the odometry graph 170 | * This function also identifies the loop closure edges and adds them to the 171 | * loop_closure_edges_ data for further processing 172 | */ 173 | void G2oSolver::PublishGraphVisualization( 174 | visualization_msgs::MarkerArray* marker_array) { 175 | // Vertices are round, red spheres 176 | visualization_msgs::Marker m; 177 | m.header.frame_id = "map"; 178 | m.header.stamp = ros::Time::now(); 179 | m.id = 0; 180 | m.ns = "karto"; 181 | m.type = visualization_msgs::Marker::SPHERE; 182 | m.pose.position.x = 0.0; 183 | m.pose.position.y = 0.0; 184 | m.pose.position.z = 0.0; 185 | m.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 186 | m.scale.x = 0.1; 187 | m.scale.y = 0.1; 188 | m.scale.z = 0.1; 189 | m.color.r = 1.0; 190 | m.color.g = 0; 191 | m.color.b = 0.0; 192 | m.color.a = 1.0; 193 | m.lifetime = ros::Duration(0); 194 | 195 | // Odometry edges are opaque blue line strips 196 | visualization_msgs::Marker edge; 197 | edge.header.frame_id = "map"; 198 | edge.header.stamp = ros::Time::now(); 199 | edge.action = visualization_msgs::Marker::ADD; 200 | edge.ns = "karto"; 201 | edge.id = 0; 202 | edge.type = visualization_msgs::Marker::LINE_STRIP; 203 | edge.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 204 | edge.scale.x = 0.1; 205 | edge.scale.y = 0.1; 206 | edge.scale.z = 0.1; 207 | edge.color.a = 1.0; 208 | edge.color.r = 0.0; 209 | edge.color.g = 0.0; 210 | edge.color.b = 1.0; 211 | 212 | // Loop edges are purple, opacity depends on backend state 213 | visualization_msgs::Marker loop_edge; 214 | loop_edge.header.frame_id = "map"; 215 | loop_edge.header.stamp = ros::Time::now(); 216 | loop_edge.action = visualization_msgs::Marker::ADD; 217 | loop_edge.ns = "karto"; 218 | loop_edge.id = 0; 219 | loop_edge.type = visualization_msgs::Marker::LINE_STRIP; 220 | loop_edge.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 221 | loop_edge.scale.x = 0.1; 222 | loop_edge.scale.y = 0.1; 223 | loop_edge.scale.z = 0.1; 224 | loop_edge.color.a = 1.0; 225 | loop_edge.color.r = 1.0; 226 | loop_edge.color.g = 0.0; 227 | loop_edge.color.b = 1.0; 228 | 229 | int id = static_cast(marker_array->markers.size()); 230 | m.action = visualization_msgs::Marker::ADD; 231 | 232 | std::unordered_set vertex_ids; 233 | // HyperGraph Edges 234 | const auto& edges = optimizer_.edges(); 235 | for (const auto* edge_it : edges) { 236 | // Is it a unary edge? Need to skip or else die a bad death 237 | if (edge_it->vertices().size() < 2) { 238 | continue; 239 | } 240 | 241 | const auto* v1 = 242 | dynamic_cast(edge_it->vertices()[0]); 243 | const auto* v2 = 244 | dynamic_cast(edge_it->vertices()[1]); 245 | 246 | geometry_msgs::Point p1; 247 | geometry_msgs::Point p2; 248 | p1.x = v1->estimate()[0]; 249 | p1.y = v1->estimate()[1]; 250 | p2.x = v2->estimate()[0]; 251 | p2.y = v2->estimate()[1]; 252 | 253 | if ((v2->id() - v1->id()) < 70) { 254 | // not a loop closure 255 | edge.points.clear(); 256 | edge.points.push_back(p1); 257 | edge.points.push_back(p2); 258 | edge.id = id; 259 | marker_array->markers.push_back(visualization_msgs::Marker(edge)); 260 | id++; 261 | } else { 262 | // it's a loop closure 263 | loop_edge.points.clear(); 264 | loop_edge.points.push_back(p1); 265 | loop_edge.points.push_back(p2); 266 | loop_edge.id = id++; 267 | 268 | loop_edge.color.a = 1.0; 269 | 270 | marker_array->markers.push_back(visualization_msgs::Marker(loop_edge)); 271 | } 272 | 273 | // Check the vertices exist, if not add 274 | if (vertex_ids.find(v2->id()) == vertex_ids.end()) { 275 | // Add the vertex to the marker array 276 | m.id = id; 277 | m.pose.position.x = p2.x; 278 | m.pose.position.y = p2.y; 279 | vertex_ids.insert(v2->id()); 280 | marker_array->markers.push_back(visualization_msgs::Marker(m)); 281 | id++; 282 | } 283 | } 284 | } 285 | 286 | } // namespace line_segment_mapping 287 | -------------------------------------------------------------------------------- /src/line_segment_extractor.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "line_segment_mapping/line_segment_extractor.h" 33 | 34 | #include "glog/logging.h" 35 | 36 | namespace line_segment_mapping { 37 | // 构造函数 38 | LineSegmentExtractor::LineSegmentExtractor() { load_parameters(); } 39 | 40 | void LineSegmentExtractor::load_parameters() { 41 | params_.least_thresh = 0.03; 42 | params_.min_line_length = 0.6; 43 | params_.min_line_points = 10; 44 | params_.predict_distance = 0.1; 45 | params_.seed_line_points = 6; 46 | params_.max_line_gap = 0.3; 47 | } 48 | 49 | // set paramters 50 | void LineSegmentExtractor::set_least_threshold(double least_thresh) { 51 | params_.least_thresh = least_thresh; 52 | } 53 | 54 | void LineSegmentExtractor::set_min_line_length(double min_line_length) { 55 | params_.min_line_length = min_line_length; 56 | } 57 | 58 | void LineSegmentExtractor::set_predict_distance(double predict_distance) { 59 | params_.predict_distance = predict_distance; 60 | } 61 | 62 | void LineSegmentExtractor::set_min_line_points(int min_line_points) { 63 | params_.min_line_points = min_line_points; 64 | } 65 | 66 | void LineSegmentExtractor::set_seed_line_points(int seed_line_points) { 67 | params_.seed_line_points = seed_line_points; 68 | } 69 | 70 | void LineSegmentExtractor::set_max_line_gap(double max_line_gap) { 71 | params_.max_line_gap = max_line_gap; 72 | } 73 | 74 | void LineSegmentExtractor::range_filtering( 75 | const karto::RangeReadingsVector& point_readings, 76 | const karto::LaserRangeFinder* laser_range_finder, PointReadings* data) { 77 | double range_thr = laser_range_finder->GetRangeThreshold(); 78 | double angle_min = laser_range_finder->GetMinimumAngle(); 79 | double range_min = laser_range_finder->GetMinimumRange(); 80 | double angle_increment = laser_range_finder->GetAngularResolution(); 81 | 82 | // compute point readings 83 | data->points.clear(); 84 | data->ranges.clear(); 85 | data->angles.clear(); 86 | data->num = 0; 87 | 88 | for (int i = 0; 89 | i < static_cast(laser_range_finder->GetNumberOfRangeReadings()); 90 | ++i) { 91 | double range = point_readings[i]; 92 | double angle = angle_min + i * angle_increment; 93 | 94 | if (!karto::math::InRange(range, range_min, range_thr)) { 95 | continue; 96 | } 97 | 98 | data->points.emplace_back(range * std::cos(angle), range * std::sin(angle)); 99 | data->ranges.push_back(range); 100 | data->angles.push_back(angle); 101 | data->num++; 102 | } 103 | } 104 | 105 | void LineSegmentExtractor::extract_lines( 106 | const karto::RangeReadingsVector& point_readings, 107 | const karto::LaserRangeFinder* laser_range_finder, 108 | LineSegmentPtrVector* line_segments) { 109 | CHECK_NOTNULL(line_segments); 110 | PointReadings data; 111 | range_filtering(point_readings, laser_range_finder, &data); 112 | 113 | process(data, line_segments); 114 | } 115 | 116 | // 一次最小二乘求解直线参数 117 | least LineSegmentExtractor::least_square(int start, int end, int firstfit, 118 | const PointReadings& data) { 119 | double w1 = 0, w2 = 0, w3 = 0; 120 | least temp; 121 | double n = end - start + 1; 122 | 123 | if (firstfit == 1) { 124 | mid1 = 0; 125 | mid2 = 0; 126 | mid3 = 0; 127 | mid4 = 0; 128 | mid5 = 0; 129 | int k = 0; 130 | for (k = start; k <= end; k++) { 131 | mid1 += data.points[k].GetX(); 132 | mid2 += data.points[k].GetY(); 133 | mid3 += data.points[k].GetX() * data.points[k].GetX(); 134 | mid4 += data.points[k].GetY() * data.points[k].GetY(); 135 | mid5 += data.points[k].GetX() * data.points[k].GetY(); 136 | } 137 | } else { 138 | if (firstfit == 2) { 139 | mid1 += data.points[end].GetX(); 140 | mid2 += data.points[end].GetY(); 141 | mid3 += data.points[end].GetX() * data.points[end].GetX(); 142 | mid4 += data.points[end].GetY() * data.points[end].GetY(); 143 | mid5 += data.points[end].GetX() * data.points[end].GetY(); 144 | } else { 145 | mid1 += data.points[start].GetX(); 146 | mid2 += data.points[start].GetY(); 147 | mid3 += data.points[start].GetX() * data.points[start].GetX(); 148 | mid4 += data.points[start].GetY() * data.points[start].GetY(); 149 | mid5 += data.points[start].GetX() * data.points[start].GetY(); 150 | } 151 | } 152 | w1 = n * mid5 - mid1 * mid2; 153 | w2 = mid2 * mid2 - n * mid4 - mid1 * mid1 + n * mid3; 154 | w3 = mid1 * mid2 - n * mid5; 155 | // ax+by+c = 0 等价于 y = kx + b;kx-y + b = 0 //a = k,c = b,b=-1 156 | if (w1 == 0) { 157 | temp.a = -1; 158 | temp.b = 0; 159 | temp.c = mid1 / n; 160 | } else { 161 | temp.a = (-w2 + std::sqrt(w2 * w2 - 4 * w1 * w3)) / 2.0 / w1; 162 | temp.b = -1; 163 | temp.c = (mid2 - temp.a * mid1) / n; 164 | } 165 | // temp.a = (n*mid5-mid1*mid2)/(n*mid3-mid1*mid1); 166 | // temp.b = -1; 167 | // temp.c = (mid2-temp.a*mid1)/n; 168 | 169 | return temp; 170 | } 171 | 172 | // 判断下一个点是否在直线上，是，返回true；否则，返回false。 173 | bool LineSegmentExtractor::detect_line(int start, int num, 174 | const PointReadings& data) { 175 | // 定义点到直线的垂直误差 176 | double error1 = 0.0; 177 | // 定义下一点到预测位置的误差 178 | double error2 = 0.0; 179 | // 定义连续两点之间的距离 180 | double dist = 0.0; 181 | int k = 0; 182 | // 预测下一点位置 183 | karto::Vector2 m_pn; 184 | 185 | // 下一点，y = kp*x; 186 | double kp = 0; 187 | double theta = 0; 188 | for (k = start; k < start + num; k++) { 189 | // 到直线的垂直距离 190 | error1 = 191 | point_to_line_distance(m_least.a, m_least.b, m_least.c, 192 | data.points[k].GetX(), data.points[k].GetY()); 193 | if (error1 > params_.least_thresh) { 194 | return false; 195 | } 196 | 197 | theta = data.angles[k]; 198 | if (std::abs(std::abs(theta) - M_PI_2) < kDoubleEpsilon) { 199 | m_pn.SetX(0); 200 | m_pn.SetY(m_least.c); 201 | } else { 202 | kp = tan(theta); 203 | m_pn.SetX(m_least.c / (kp - m_least.a)); 204 | m_pn.SetY(kp * m_pn.GetX()); 205 | } 206 | 207 | // 计算到预测点之间的误差 208 | error2 = (m_pn - data.points[k]).Length(); 209 | // 与预测点之间的距离 210 | if (error2 > params_.predict_distance) { 211 | return false; 212 | } 213 | 214 | // 计算连续两点之间的距离 215 | if (k != start + num - 1) { 216 | dist = (data.points[k + 1] - data.points[k]).Length(); 217 | if (dist > params_.max_line_gap) { 218 | return false; 219 | } 220 | } 221 | } 222 | 223 | return true; 224 | } 225 | 226 | // 检测完整的直线段 227 | int LineSegmentExtractor::detect_full_line(int start, 228 | const PointReadings& data) { 229 | line m_temp; 230 | 231 | bool flag1 = true; 232 | bool flag2 = true; 233 | int n1 = 0; 234 | int n2 = 0; 235 | double a = 0; 236 | double b = 0; 237 | double c = 0; 238 | 239 | a = m_least.a; 240 | b = m_least.b; 241 | c = m_least.c; 242 | 243 | n2 = start + params_.seed_line_points; 244 | least m_result; 245 | m_result.a = 0; 246 | m_result.b = 0; 247 | m_result.c = 0; 248 | double dist; 249 | 250 | // 向前生长 251 | while (flag2) { 252 | if (n2 <= (data.num - 1)) { 253 | dist = (data.points[n2] - data.points[n2 - 1]).Length(); 254 | if (dist < params_.max_line_gap) { 255 | if (point_to_line_distance(a, b, c, data.points[n2].GetX(), 256 | data.points[n2].GetY()) < 257 | params_.least_thresh) { 258 | m_least = least_square(start, n2, 2, data); 259 | 260 | n2 = n2 + 1; 261 | a = m_least.a; 262 | b = m_least.b; 263 | c = m_least.c; 264 | } else { 265 | flag2 = false; 266 | } 267 | } else { 268 | flag2 = false; 269 | } 270 | } else { 271 | flag2 = false; 272 | } 273 | } 274 | n2 = n2 - 1; 275 | 276 | // 向后回溯 277 | n1 = start - 1; 278 | while (flag1) { 279 | if (n1 >= 0) { 280 | dist = (data.points[n1 + 1] - data.points[n1]).Length(); 281 | if (dist < params_.max_line_gap) { 282 | if (point_to_line_distance(a, b, c, data.points[n1].GetX(), 283 | data.points[n1].GetY()) < 284 | params_.least_thresh) { 285 | m_least = least_square(n1, n2, 3, data); 286 | 287 | n1 = n1 - 1; 288 | a = m_least.a; 289 | b = m_least.b; 290 | c = m_least.c; 291 | } else { 292 | flag1 = false; 293 | } 294 | } else { 295 | flag1 = false; 296 | } 297 | } else { 298 | flag1 = false; 299 | } 300 | } 301 | n1 = n1 + 1; 302 | 303 | m_temp.left = n1; 304 | m_temp.right = n2; 305 | // 此处是统一再做一次拟合，可能以一定步长进行拟合搜索的时候，需要这样完整的拟合过程，此时不需要 306 | m_result = least_square(n1, n2, 1, data); 307 | m_temp.a = m_result.a; 308 | m_temp.b = m_result.b; 309 | m_temp.c = m_result.c; 310 | 311 | if ((n2 - n1 + 1) >= params_.min_line_points) { 312 | if (delete_short_line(n1, n2, data)) { 313 | m_line.push_back(m_temp); 314 | } 315 | return n2; 316 | } else { 317 | return start; 318 | } 319 | } 320 | 321 | void LineSegmentExtractor::clean_line(const PointReadings& data) { 322 | if (m_line.size() < 2) { 323 | return; 324 | } 325 | 326 | int m = 0; 327 | int n = 0; 328 | int m_iter = 0; 329 | double error1 = 0; 330 | double error2 = 0; 331 | least temp_least; 332 | temp_least.a = 0; 333 | temp_least.b = 0; 334 | temp_least.c = 0; 335 | 336 | double theta_one_ = 0; 337 | double theta_two_ = 0; 338 | double theta_d_ = 0; 339 | size_t q = 0, p = 0; 340 | 341 | for (p = 0; p < m_line.size() - 1; p++) { 342 | m = m_line[p].right; 343 | for (q = p + 1; q < m_line.size(); q++) { 344 | n = m_line[q].left; 345 | if (m >= n) { 346 | theta_one_ = atan(m_line[p].a); 347 | theta_two_ = atan(m_line[q].a); 348 | 349 | theta_d_ = std::abs(theta_one_ - theta_two_); 350 | 351 | if ((theta_d_ < 0.1) || (theta_d_ > (M_PI - 0.1))) { 352 | int _left = std::min(m_line[p].left, m_line[q].left); 353 | 354 | least m_temp = least_square(_left, m_line[q].right, 1, data); 355 | 356 | m_line[p].a = m_temp.a; 357 | m_line[p].b = m_temp.b; 358 | m_line[p].c = m_temp.c; 359 | 360 | m_line[p].left = _left; 361 | m_line[p].right = m_line[q].right; 362 | 363 | m_line.erase(m_line.begin() + q); 364 | 365 | m = m_line[p].right; 366 | q = q - 1; 367 | } 368 | } 369 | } 370 | } 371 | 372 | // 处理有相互链接关系的线段 373 | for (p = 0; p < (m_line.size() - 1); p++) { 374 | q = p + 1; 375 | m = m_line[p].right; 376 | n = m_line[q].left; 377 | if (m >= n) { 378 | for (m_iter = n; m_iter <= m; m_iter++) { 379 | error1 = point_to_line_distance(m_line[p].a, m_line[p].b, m_line[p].c, 380 | data.points[m_iter].GetX(), 381 | data.points[m_iter].GetY()); 382 | error2 = point_to_line_distance(m_line[q].a, m_line[q].b, m_line[q].c, 383 | data.points[m_iter].GetX(), 384 | data.points[m_iter].GetY()); 385 | if (error1 > error2) { 386 | break; 387 | } 388 | } 389 | m_line[p].right = m_iter - 1; 390 | temp_least = least_square(m_line[p].left, m_line[p].right, 1, data); 391 | m_line[p].a = temp_least.a; 392 | m_line[p].b = temp_least.b; 393 | m_line[p].c = temp_least.c; 394 | 395 | m_line[q].left = m_iter; 396 | temp_least = least_square(m_line[q].left, m_line[q].right, 1, data); 397 | m_line[q].a = temp_least.a; 398 | m_line[q].b = temp_least.b; 399 | m_line[q].c = temp_least.c; 400 | } 401 | } 402 | } 403 | 404 | bool LineSegmentExtractor::delete_short_line(int n1, int n2, 405 | const PointReadings& data) { 406 | // 删除一些长度小于0.6m的线段，这个可以视具体情况而定 407 | double dist = (data.points[n2] - data.points[n1]).Length(); 408 | if (dist < params_.min_line_length) { 409 | return false; 410 | } else { 411 | return true; 412 | } 413 | } 414 | 415 | void LineSegmentExtractor::is_sequential() { 416 | for (auto iter = m_line.begin(); iter != m_line.end() - 1; ++iter) { 417 | int m1 = iter->left; 418 | int n1 = iter->right; 419 | CHECK_LT(m1, n1); 420 | for (auto inner_iter = iter + 1; inner_iter != m_line.end(); ++inner_iter) { 421 | int m2 = inner_iter->left; 422 | int n2 = inner_iter->right; 423 | CHECK_LT(m2, n2); 424 | CHECK_LT(n1, m2); 425 | } 426 | } 427 | } 428 | 429 | void LineSegmentExtractor::generate(const PointReadings& data, 430 | LineSegmentPtrVector* line_segments) { 431 | karto::Vector2 endpoint1; 432 | karto::Vector2 endpoint2; 433 | line_segments->clear(); 434 | 435 | int m = 0, n = 0; 436 | for (size_t i = 0; i < m_line.size(); ++i) { 437 | m = m_line[i].left; 438 | n = m_line[i].right; 439 | CHECK_LT(m, n); 440 | 441 | if (m_line[i].b != 0) { 442 | endpoint1.SetX((data.points[m].GetX() / m_line[i].a + 443 | data.points[m].GetY() - m_line[i].c) / 444 | (m_line[i].a + 1.0 / m_line[i].a)); 445 | endpoint1.SetY(m_line[i].a * endpoint1.GetX() + m_line[i].c); 446 | } else { 447 | endpoint1.SetX(data.points[m].GetY()); 448 | endpoint1.SetY(m_line[i].c / m_line[i].a); 449 | } 450 | 451 | if (m_line[i].b != 0) { 452 | endpoint2.SetX((data.points[n].GetX() / m_line[i].a + 453 | data.points[n].GetY() - m_line[i].c) / 454 | (m_line[i].a + 1.0 / m_line[i].a)); 455 | endpoint2.SetY(m_line[i].a * endpoint2.GetX() + m_line[i].c); 456 | } else { 457 | endpoint2.SetX(data.points[n].GetY()); 458 | endpoint2.SetY(m_line[i].c / m_line[i].a); 459 | } 460 | 461 | LineSegmentPtr lineSegment = 462 | std::make_shared(endpoint1, endpoint2); 463 | line_segments->push_back(lineSegment); 464 | } 465 | } 466 | 467 | // 识别主函数 468 | void LineSegmentExtractor::process(const PointReadings& data, 469 | LineSegmentPtrVector* line_segments) { 470 | int line_include = 0; 471 | m_line.clear(); 472 | 473 | if (data.num < params_.min_line_points) { 474 | return; 475 | } 476 | 477 | // 附近特征点数目 478 | for (int i = 0; i < (data.num - params_.min_line_points); i++) { 479 | m_least = least_square(i, i + params_.seed_line_points - 1, 1, data); 480 | // std::cout << m_least.a << " " << m_least.b << " " << m_least.c << 481 | // std::endl; 482 | if (detect_line(i, params_.seed_line_points, data)) { 483 | line_include = detect_full_line(i, data); 484 | i = line_include; 485 | } 486 | } 487 | clean_line(data); 488 | 489 | for (auto iter = m_line.begin(); iter != m_line.end();) { 490 | if (!delete_short_line(iter->left, iter->right, data)) { 491 | iter = m_line.erase(iter); 492 | } else { 493 | ++iter; 494 | } 495 | } 496 | 497 | // std::cout << m_line.size() << std::endl; 498 | // is_sequential(); 499 | generate(data, line_segments); 500 | } 501 | 502 | } // namespace line_segment_mapping 503 | -------------------------------------------------------------------------------- /src/line_segment_map_manager.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "line_segment_mapping/line_segment_map_manager.h" 33 | 34 | #include "glog/logging.h" 35 | 36 | namespace line_segment_mapping { 37 | 38 | bool LineSegmentMapManager::NaiveMerge( 39 | const LineSegmentPtrVector& rLineSegments) { 40 | bool doUpdate = false; 41 | 42 | // The global line segment map is empty, insert the current line segments 43 | // directly 44 | if (m_LineSegmentMap.empty()) { 45 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); 46 | ++iter) { 47 | LineSegment globalLineSegment = (*iter)->GetGlobalLineSegments(); 48 | LineSegmentPtr newLineSegment = 49 | std::make_shared(globalLineSegment); 50 | m_LineSegmentMap.insert( 51 | std::make_pair(m_LineSegmentClustersIndex, newLineSegment)); 52 | 53 | LineSegmentPtrVector lineSegmentCluster; 54 | lineSegmentCluster.push_back(*iter); 55 | m_LineSegmentClusters.insert( 56 | std::make_pair(m_LineSegmentClustersIndex, lineSegmentCluster)); 57 | 58 | m_ClustersIndexArray.push_back(m_LineSegmentClustersIndex); 59 | m_LineSegmentClustersIndex++; 60 | } 61 | doUpdate = true; 62 | } else { 63 | std::vector remain; 64 | remain.resize(rLineSegments.size(), true); 65 | int remainId = 0; 66 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); 67 | ++iter) { 68 | for (auto inner_iter = m_LineSegmentMap.begin(); 69 | inner_iter != m_LineSegmentMap.end(); ++inner_iter) { 70 | // Obtain the current line segments in world coordinates 71 | LineSegment globalLineSegment = (*iter)->GetGlobalLineSegments(); 72 | 73 | if (CalculateOverlap(*(inner_iter->second), globalLineSegment)) { 74 | LineSegmentVector candidates; 75 | candidates.push_back(globalLineSegment); 76 | 77 | LineSegmentPtrVector* originalLineSegments = 78 | &m_LineSegmentClusters[inner_iter->first]; 79 | for (auto iterator = originalLineSegments->begin(); 80 | iterator != originalLineSegments->end(); ++iterator) { 81 | LineSegment tmp = (*iterator)->GetGlobalLineSegments(); 82 | candidates.push_back(tmp); 83 | } 84 | 85 | // Merge the redundant line segments and update the global line 86 | // segment map 87 | LineSegment* updatedLine = MergeLineSegments(candidates); 88 | inner_iter->second.reset(updatedLine); 89 | 90 | originalLineSegments->push_back(*iter); 91 | 92 | remain[remainId] = false; 93 | 94 | doUpdate = true; 95 | break; // inner loop end 96 | } 97 | } 98 | 99 | remainId++; 100 | } 101 | 102 | // Insert the line segments extarcted from the current scan into the global 103 | // line segment map These line segments are new and there are no line 104 | // segments in the global map that match them 105 | remainId = 0; 106 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); 107 | ++iter) { 108 | if (remain[remainId]) { 109 | LineSegment globalLineSegment = (*iter)->GetGlobalLineSegments(); 110 | LineSegmentPtr newLineSegment = 111 | std::make_shared(globalLineSegment); 112 | m_LineSegmentMap.insert( 113 | std::make_pair(m_LineSegmentClustersIndex, newLineSegment)); 114 | 115 | LineSegmentPtrVector lineSegmentCluster; 116 | lineSegmentCluster.push_back(*iter); 117 | m_LineSegmentClusters.insert( 118 | std::make_pair(m_LineSegmentClustersIndex, lineSegmentCluster)); 119 | 120 | m_ClustersIndexArray.push_back(m_LineSegmentClustersIndex); 121 | m_LineSegmentClustersIndex++; 122 | } 123 | 124 | remainId++; 125 | } 126 | } 127 | 128 | // assert(updateCheck()); 129 | 130 | return doUpdate; 131 | } 132 | 133 | bool LineSegmentMapManager::IncrementalMerge( 134 | const LineSegmentPtrVector& rLineSegments) { 135 | bool doUpdate = false; 136 | bool doMerge = false; 137 | int index = -1; 138 | LineSegmentVector candidates; 139 | std::vector candidateIndex; 140 | 141 | // The global line segment map is empty, insert the current line segments 142 | // directly 143 | if (m_LineSegmentMap.empty()) { 144 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); 145 | ++iter) { 146 | LineSegment globalLineSegment = (*iter)->GetGlobalLineSegments(); 147 | LineSegmentPtr newLineSegment = 148 | std::make_shared(globalLineSegment); 149 | m_LineSegmentMap.insert( 150 | std::make_pair(m_LineSegmentClustersIndex, newLineSegment)); 151 | (*iter)->SetLineSegmentClusterIndex(m_LineSegmentClustersIndex); 152 | 153 | LineSegmentPtrVector lineSegmentCluster; 154 | lineSegmentCluster.push_back(*iter); 155 | m_LineSegmentClusters.insert( 156 | std::make_pair(m_LineSegmentClustersIndex, lineSegmentCluster)); 157 | 158 | m_ClustersIndexArray.push_back(m_LineSegmentClustersIndex); 159 | m_LineSegmentClustersIndex++; 160 | } 161 | 162 | doUpdate = true; 163 | } else { 164 | std::vector remain; 165 | remain.resize(rLineSegments.size(), true); 166 | int remainId = 0; 167 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); 168 | ++iter) { 169 | for (auto inner_iter = m_LineSegmentMap.begin(); 170 | inner_iter != m_LineSegmentMap.end();) { 171 | LineSegment globalLineSegment = (*iter)->GetGlobalLineSegments(); 172 | if (CalculateOverlap(*(inner_iter->second), globalLineSegment)) { 173 | doUpdate = true; 174 | doMerge = true; 175 | remain[remainId] = false; 176 | 177 | LineSegmentPtrVector originalLineSegments = 178 | m_LineSegmentClusters[inner_iter->first]; 179 | for (auto iterator = originalLineSegments.begin(); 180 | iterator != originalLineSegments.end(); ++iterator) { 181 | LineSegment tmp = (*iterator)->GetGlobalLineSegments(); 182 | candidates.push_back(tmp); 183 | } 184 | 185 | // It is possible to have more than one line segment in the global map 186 | // matching this line segment So we record the minimum index of the 187 | // matched line segments in the global map 188 | if (index == -1) { 189 | candidates.push_back(globalLineSegment); 190 | index = inner_iter->first; 191 | ++inner_iter; 192 | } else { 193 | candidateIndex.push_back(inner_iter->first); 194 | inner_iter = m_LineSegmentMap.erase(inner_iter); 195 | } 196 | } else { 197 | ++inner_iter; 198 | } 199 | } // inner loop end 200 | 201 | if (doMerge) { 202 | LineSegment* updatedLine = MergeLineSegments(candidates); 203 | m_LineSegmentMap[index].reset(updatedLine); 204 | 205 | LineSegmentPtrVector* objectCluster = &m_LineSegmentClusters[index]; 206 | 207 | // Move the corresponding original line segments of the merged line 208 | // segments to the new cluster 209 | for (auto index_iter = candidateIndex.begin(); 210 | index_iter != candidateIndex.end(); ++index_iter) { 211 | LineSegmentPtrVector* candidateCluster = 212 | &m_LineSegmentClusters[*index_iter]; 213 | for (auto cluster_iter = candidateCluster->begin(); 214 | cluster_iter != candidateCluster->end(); ++cluster_iter) { 215 | (*cluster_iter)->SetLineSegmentClusterIndex(index); 216 | } 217 | 218 | objectCluster->insert(objectCluster->end(), candidateCluster->begin(), 219 | candidateCluster->end()); 220 | candidateCluster->clear(); 221 | } 222 | (*iter)->SetLineSegmentClusterIndex(index); 223 | objectCluster->push_back(*iter); 224 | 225 | bool flag = false; 226 | for (auto index_iter = m_ClustersIndexArray.begin(); 227 | index_iter != m_ClustersIndexArray.end();) { 228 | for (auto candidate_iter = candidateIndex.begin(); 229 | candidate_iter != candidateIndex.end();) { 230 | if (*index_iter == *candidate_iter) { 231 | flag = true; 232 | index_iter = m_ClustersIndexArray.erase(index_iter); 233 | candidate_iter = candidateIndex.erase(candidate_iter); 234 | break; 235 | } else { 236 | ++candidate_iter; 237 | } 238 | } 239 | 240 | if (flag) { 241 | flag = false; 242 | } else { 243 | ++index_iter; 244 | } 245 | } 246 | } 247 | 248 | // refresh 249 | doMerge = false; 250 | index = -1; 251 | candidates.clear(); 252 | CHECK(candidateIndex.empty()); 253 | 254 | remainId++; 255 | } 256 | 257 | remainId = 0; 258 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); 259 | ++iter) { 260 | if (remain[remainId]) { 261 | LineSegment globalLineSegment = (*iter)->GetGlobalLineSegments(); 262 | LineSegmentPtr newLineSegment = 263 | std::make_shared(globalLineSegment); 264 | m_LineSegmentMap.insert( 265 | std::make_pair(m_LineSegmentClustersIndex, newLineSegment)); 266 | (*iter)->SetLineSegmentClusterIndex(m_LineSegmentClustersIndex); 267 | 268 | LineSegmentPtrVector lineSegmentCluster; 269 | lineSegmentCluster.push_back(*iter); 270 | m_LineSegmentClusters.insert( 271 | std::make_pair(m_LineSegmentClustersIndex, lineSegmentCluster)); 272 | 273 | m_ClustersIndexArray.push_back(m_LineSegmentClustersIndex); 274 | m_LineSegmentClustersIndex++; 275 | } 276 | 277 | remainId++; 278 | } 279 | } 280 | 281 | // assert(updateCheck()); 282 | 283 | return doUpdate; 284 | } 285 | 286 | void LineSegmentMapManager::GlobalMapAdjustment() { 287 | std::cout << "Global map adjustment called." << std::endl; 288 | 289 | // #pragma omp parallel for 290 | for (int i = 0; i < static_cast(m_ClustersIndexArray.size()); i++) { 291 | MapAdjustment(m_ClustersIndexArray[i]); 292 | } 293 | 294 | std::cout << "Global map adjustment done." << std::endl; 295 | // assert(updateCheck()); 296 | } 297 | 298 | bool LineSegmentMapManager::CalculateOverlap( 299 | const LineSegment& prevLineSegment, const LineSegment& currLineSegment) { 300 | double headingDeviation = karto::math::NormalizeAngle( 301 | currLineSegment.GetHeading() - prevLineSegment.GetHeading()); 302 | // The two line segments are not parallel 303 | if (std::abs(headingDeviation) > karto::math::DegreesToRadians(4.0)) { 304 | return false; 305 | } 306 | 307 | double A, B, C; 308 | 309 | // Considering whether the slope of a straight line exists or not, it is 310 | // discussed in two cases and finally transformed into a general formula of a 311 | // straight line 312 | if (std::abs(std::abs(prevLineSegment.GetHeading()) - M_PI_2) < 313 | kDoubleEpsilon) { 314 | A = 1; 315 | B = 0; 316 | C = -(prevLineSegment.GetStartPoint().GetX() + 317 | prevLineSegment.GetEndPoint().GetX()) / 318 | 2.0; 319 | } else { 320 | A = tan(prevLineSegment.GetHeading()); 321 | B = -1; 322 | C = prevLineSegment.GetStartPoint().GetY() - 323 | A * prevLineSegment.GetStartPoint().GetX(); 324 | } 325 | 326 | double dist1 = 327 | point_to_line_distance(A, B, C, currLineSegment.GetStartPoint().GetX(), 328 | currLineSegment.GetStartPoint().GetY()); 329 | double dist2 = 330 | point_to_line_distance(A, B, C, currLineSegment.GetEndPoint().GetX(), 331 | currLineSegment.GetEndPoint().GetY()); 332 | double dist = std::max(dist1, dist2); 333 | 334 | if (dist > 0.10) { 335 | return false; 336 | } 337 | 338 | // 将线段端点投影到直线上 339 | // 已知直线M的一般式方程为Ax+By+C=0, 则过点(a, 340 | // b)且与L垂直的直线N的一般式方程为Bx-Ay+Ab-Ba=0 341 | // 直线M与直线N的交点坐标：x=(B*B*a-A*B*b-A*C)/(A*A+B*B), 342 | // y=(A*A*b-A*B*a-B*C)/(A*A+B*B) 343 | double x, y; 344 | karto::Vector2 prevStartPoint, prevEndPoint, currStartPoint, 345 | currEndPoint; 346 | 347 | prevStartPoint.SetX(prevLineSegment.GetStartPoint().GetX()); 348 | prevStartPoint.SetY(prevLineSegment.GetStartPoint().GetY()); 349 | prevEndPoint.SetX(prevLineSegment.GetEndPoint().GetX()); 350 | prevEndPoint.SetY(prevLineSegment.GetEndPoint().GetY()); 351 | 352 | x = (B * B * currLineSegment.GetStartPoint().GetX() - 353 | A * B * currLineSegment.GetStartPoint().GetY() - A * C) / 354 | (A * A + B * B); 355 | y = (A * A * currLineSegment.GetStartPoint().GetY() - 356 | A * B * currLineSegment.GetStartPoint().GetX() - B * C) / 357 | (A * A + B * B); 358 | currStartPoint.SetX(x); 359 | currStartPoint.SetY(y); 360 | 361 | x = (B * B * currLineSegment.GetEndPoint().GetX() - 362 | A * B * currLineSegment.GetEndPoint().GetY() - A * C) / 363 | (A * A + B * B); 364 | y = (A * A * currLineSegment.GetEndPoint().GetY() - 365 | A * B * currLineSegment.GetEndPoint().GetX() - B * C) / 366 | (A * A + B * B); 367 | currEndPoint.SetX(x); 368 | currEndPoint.SetY(y); 369 | 370 | // 当前线段起点指向参考线段起点的方向向量 371 | karto::Vector2 vector1 = currStartPoint - prevStartPoint; 372 | // 当前线段起点指向参考线段终点的方向向量 373 | karto::Vector2 vector2 = currStartPoint - prevEndPoint; 374 | // 当前线段终点指向参考线段起点的方向向量 375 | karto::Vector2 vector3 = currEndPoint - prevStartPoint; 376 | // 当前线段终点指向参考线段终点的方向向量 377 | karto::Vector2 vector4 = currEndPoint - prevEndPoint; 378 | 379 | // 说明当前线段的起点落在参考线段的两侧 380 | if (vector1.GetX() * vector2.GetX() + vector1.GetY() * vector2.GetY() > 0) { 381 | // 说明当前线段的起点落在参考线段起点的外侧 382 | if (vector1.Length() < vector2.Length()) { 383 | // 说明当前线段的终点落在参考线段的两侧 384 | if (vector3.GetX() * vector4.GetX() + vector3.GetY() * vector4.GetY() > 385 | 0) { 386 | // 说明当前线段的终点落在参考线段起点的外侧 387 | if (vector3.Length() < vector4.Length()) { 388 | if (vector3.Length() <= 0.10) { 389 | return true; 390 | } else { 391 | return false; 392 | } 393 | } else { 394 | // 说明当前线段的终点落在参考线段终点的外侧 395 | return true; 396 | } 397 | } else { 398 | // 说明当前线段的终点落在参考线段的内部(包含端点) 399 | return true; 400 | } 401 | } else { 402 | // 说明当前线段的起点落在参考线段终点的外侧 403 | if (vector2.Length() <= 0.10) { 404 | return true; 405 | } else { 406 | return false; 407 | } 408 | } 409 | } else { 410 | // 说明当前线段的起点落在参考线段的内部(包含端点) 411 | return true; 412 | } 413 | } 414 | 415 | void LineSegmentMapManager::MapAdjustment(int index) { 416 | LineSegmentPtrVector lineSegmentCluster = m_LineSegmentClusters[index]; 417 | LineSegmentVector globalLineSegments; 418 | 419 | for (auto iter = lineSegmentCluster.begin(); iter != lineSegmentCluster.end(); 420 | ++iter) { 421 | LineSegment globalLineSegment = (*iter)->GetGlobalLineSegments(); 422 | globalLineSegments.push_back(globalLineSegment); 423 | } 424 | 425 | LineSegment* newLineSegment = MergeLineSegments(globalLineSegments); 426 | m_LineSegmentMap[index].reset(newLineSegment); 427 | 428 | for (auto iter = lineSegmentCluster.begin(); iter != lineSegmentCluster.end(); 429 | ++iter) { 430 | (*iter)->SetLineSegmentClusterIndex(index); 431 | } 432 | } 433 | 434 | /** 435 | * Merge linesegments 436 | */ 437 | LineSegment* LineSegmentMapManager::MergeLineSegments( 438 | const LineSegmentVector& rLineSegments) { 439 | CHECK(!rLineSegments.empty()); 440 | karto::Vector2 directionVector; 441 | karto::Vector2 centralPoint; 442 | int updateRate = 0; 443 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); ++iter) { 444 | directionVector += iter->GetDirectionVector(); 445 | centralPoint += iter->GetBarycenter(); 446 | updateRate += iter->GetUpdateTimes(); 447 | } 448 | // 计算线段的单位方向向量 449 | double vectorLength = directionVector.Length(); // 归一化因子 450 | CHECK_GE(vectorLength, kDoubleEpsilon); 451 | 452 | double heading = atan2(directionVector.GetY(), directionVector.GetX()); 453 | CHECK(karto::math::InRange(heading, -M_PI, M_PI)); 454 | 455 | karto::Vector2 barycenter = 456 | centralPoint / static_cast(rLineSegments.size()); 457 | 458 | double A, B, C; 459 | // 考虑直线斜率存在与否，分两种情况讨论，最终化为直线的一般式 460 | if (std::abs(std::abs(heading) - M_PI_2) < kDoubleEpsilon) { 461 | A = 1; 462 | B = 0; 463 | C = -barycenter.GetX(); 464 | } else { 465 | A = tan(heading); 466 | B = -1; 467 | C = barycenter.GetY() - A * barycenter.GetX(); 468 | } 469 | 470 | karto::PointVectorDouble startPointVector, endPointVector; 471 | karto::Vector2 startPoint, endPoint; 472 | for (auto iter = rLineSegments.begin(); iter != rLineSegments.end(); ++iter) { 473 | startPoint.SetX((B * B * iter->GetStartPoint().GetX() - 474 | A * B * iter->GetStartPoint().GetY() - A * C) / 475 | (A * A + B * B)); 476 | startPoint.SetY((A * A * iter->GetStartPoint().GetY() - 477 | A * B * iter->GetStartPoint().GetX() - B * C) / 478 | (A * A + B * B)); 479 | startPointVector.push_back(startPoint); 480 | endPoint.SetX((B * B * iter->GetEndPoint().GetX() - 481 | A * B * iter->GetEndPoint().GetY() - A * C) / 482 | (A * A + B * B)); 483 | endPoint.SetY((A * A * iter->GetEndPoint().GetY() - 484 | A * B * iter->GetEndPoint().GetX() - B * C) / 485 | (A * A + B * B)); 486 | endPointVector.push_back(endPoint); 487 | } 488 | 489 | double maximumDistance = 0; 490 | karto::Vector2 updatedStartPoint, updatedEndPoint; 491 | for (auto iter = startPointVector.begin(); iter != startPointVector.end(); 492 | ++iter) { 493 | for (auto inner_iter = endPointVector.begin(); 494 | inner_iter != endPointVector.end(); ++inner_iter) { 495 | double distance = (*iter - *inner_iter).Length(); 496 | if (distance > maximumDistance) { 497 | maximumDistance = distance; 498 | updatedStartPoint = *iter; 499 | updatedEndPoint = *inner_iter; 500 | } 501 | } 502 | } 503 | 504 | LineSegment* lineSegment = 505 | new LineSegment(updatedStartPoint, updatedEndPoint); 506 | lineSegment->SetUpdateTimes(updateRate); 507 | return lineSegment; 508 | } 509 | 510 | bool LineSegmentMapManager::updateCheck() { 511 | for (auto iter = m_LineSegmentMap.begin(); iter != m_LineSegmentMap.end(); 512 | ++iter) { 513 | int updateNum = static_cast(m_LineSegmentClusters[iter->first].size()); 514 | if (updateNum != iter->second->GetUpdateTimes()) { 515 | return false; 516 | } 517 | } 518 | return true; 519 | } 520 | 521 | } // namespace line_segment_mapping 522 | -------------------------------------------------------------------------------- /src/line_segment_mapper.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include "line_segment_mapping/line_segment_mapper.h" 33 | 34 | #include // NOLINT 35 | 36 | namespace line_segment_mapping { 37 | /** 38 | * Default constructor 39 | */ 40 | LineSegmentMapper::LineSegmentMapper() : Mapper("Mapper") {} 41 | 42 | /** 43 | * Default constructor 44 | */ 45 | LineSegmentMapper::LineSegmentMapper(const std::string& rName) 46 | : Mapper(rName) {} 47 | 48 | bool LineSegmentMapper::Process(karto::LocalizedRangeScan* pScan, 49 | const LineSegmentPtrVector& rLineSegments) { 50 | if (pScan != nullptr) { 51 | karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); 52 | 53 | // validate scan 54 | if (pLaserRangeFinder == nullptr || pScan == nullptr || 55 | pLaserRangeFinder->Validate(pScan) == false) { 56 | return false; 57 | } 58 | 59 | if (m_Initialized == false) { 60 | // initialize mapper with range threshold from device 61 | Initialize(pLaserRangeFinder->GetRangeThreshold()); 62 | 63 | m_pLineSegmentMapManager = std::make_unique(); 64 | 65 | m_Initialized = true; 66 | } 67 | 68 | // get last scan 69 | // 如果这一帧是第一帧，则pLastScan返回nullptr 70 | karto::LocalizedRangeScan* pLastScan = 71 | GetMapperSensorManager()->GetLastScan(pScan->GetSensorName()); 72 | 73 | // update scans corrected pose based on last correction 74 | if (pLastScan != nullptr) { 75 | karto::Transform lastTransform(pLastScan->GetOdometricPose(), 76 | pLastScan->GetCorrectedPose()); 77 | // 根据码盘数据定位 78 | pScan->SetCorrectedPose( 79 | lastTransform.TransformPose(pScan->GetOdometricPose())); 80 | } 81 | 82 | // test if scan is outside minimum boundary or if heading is larger then 83 | // minimum heading 84 | if (!HasMovedEnough(pScan, pLastScan)) { 85 | return false; 86 | } 87 | 88 | karto::Matrix3 covariance; 89 | covariance.SetToIdentity(); 90 | 91 | // correct scan (if not first scan) 92 | if (getParamUseScanMatching() && pLastScan != nullptr) { 93 | karto::Pose2 bestPose; 94 | // 核心一：扫描匹配 95 | GetSequentialScanMatcher()->MatchScan( 96 | pScan, 97 | GetMapperSensorManager()->GetRunningScans(pScan->GetSensorName()), 98 | bestPose, covariance); 99 | pScan->SetSensorPose(bestPose); // 位姿更新 100 | } 101 | 102 | // add scan to buffer and assign id 103 | GetMapperSensorManager()->AddScan(pScan); 104 | 105 | if (getParamUseScanMatching()) { 106 | // add to graph 107 | GetGraph()->AddVertex(pScan); 108 | GetGraph()->AddEdges(pScan, covariance); 109 | 110 | GetMapperSensorManager()->AddRunningScan(pScan); 111 | 112 | if (getParamDoLoopClosing()) { 113 | std::vector deviceNames = 114 | GetMapperSensorManager()->GetSensorNames(); 115 | const_forEach(std::vector, &deviceNames) { 116 | // 核心二：回环检测 117 | bool update = GetGraph()->TryCloseLoop(pScan, *iter); 118 | if (update) { 119 | const auto start_timestamp = std::chrono::system_clock::now(); 120 | m_pLineSegmentMapManager->GlobalMapAdjustment(); 121 | m_pLineSegmentMapManager->IncrementalMerge(rLineSegments); 122 | const auto end_timestamp = std::chrono::system_clock::now(); 123 | 124 | const auto time_diff = end_timestamp - start_timestamp; 125 | std::cout << "Global map adjustment costs " 126 | << time_diff.count() * 1e3 << "ms\n"; 127 | } else { 128 | // const auto start_timestamp = std::chrono::system_clock::now(); 129 | if (m_pLineSegmentMapManager->IncrementalMerge(rLineSegments)) { 130 | // const auto end_timestamp = std::chrono::system_clock::now(); 131 | // const auto time_diff = end_timestamp - start_timestamp; 132 | // std::cout << "One-to-many incremental line segment merging 133 | // costs" 134 | // << time_diff.count() * 1e3 << "ms\n"; 135 | } 136 | } 137 | } 138 | } 139 | } else { 140 | // const auto start_timestamp = std::chrono::system_clock::now(); 141 | if (m_pLineSegmentMapManager->IncrementalMerge(rLineSegments)) { 142 | // const auto end_timestamp = std::chrono::system_clock::now(); 143 | // const auto time_diff = end_timestamp - start_timestamp; 144 | // std::cout << "One-to-many incremental line segment merging costs" 145 | // << time_diff.count() * 1e3 << "ms\n"; 146 | } 147 | } 148 | 149 | GetMapperSensorManager()->SetLastScan(pScan); 150 | 151 | return true; 152 | } 153 | 154 | return false; 155 | } 156 | 157 | /** 158 | * Is the scan sufficiently far from the last scan? 159 | * @param pScan 160 | * @param pLastScan 161 | * @return true if the scans are sufficiently far 162 | */ 163 | bool LineSegmentMapper::HasMovedEnough(karto::LocalizedRangeScan* pScan, 164 | karto::LocalizedRangeScan* pLastScan) { 165 | // test if first scan 166 | if (pLastScan == nullptr) { 167 | return true; 168 | } 169 | 170 | // test if enough time has passed 171 | double timeInterval = pScan->GetTime() - pLastScan->GetTime(); 172 | if (timeInterval >= getParamMinimumTimeInterval()) { 173 | return true; 174 | } 175 | 176 | karto::Pose2 lastScannerPose = 177 | pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); 178 | karto::Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); 179 | 180 | // test if we have turned enough 181 | double deltaHeading = karto::math::NormalizeAngle( 182 | scannerPose.GetHeading() - lastScannerPose.GetHeading()); 183 | if (std::abs(deltaHeading) >= getParamMinimumTravelHeading()) { 184 | return true; 185 | } 186 | 187 | // test if we have moved enough 188 | double squaredTravelDistance = 189 | lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition()); 190 | if (squaredTravelDistance >= 191 | karto::math::Square(getParamMinimumTravelDistance()) - kDoubleEpsilon) { 192 | return true; 193 | } 194 | 195 | return false; 196 | } 197 | 198 | } // namespace line_segment_mapping 199 | -------------------------------------------------------------------------------- /src/slam_karto_g2o.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (c) 2022, NKU Mobile & Flying Robotics Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | *****************************************************************************/ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include "message_filters/subscriber.h" 40 | #include "nav_msgs/GetMap.h" 41 | #include "nav_msgs/MapMetaData.h" 42 | #include "ros/console.h" 43 | #include "ros/ros.h" 44 | #include "sensor_msgs/LaserScan.h" 45 | #include "tf/message_filter.h" 46 | #include "tf/transform_broadcaster.h" 47 | #include "tf/transform_listener.h" 48 | #include "visualization_msgs/MarkerArray.h" 49 | 50 | #include "line_segment_mapping/g2o_solver.h" 51 | #include "line_segment_mapping/line_segment_extractor.h" 52 | #include "line_segment_mapping/line_segment_mapper.h" 53 | #include "line_segment_mapping/util.h" 54 | 55 | // compute linear index for given map coords 56 | #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) 57 | 58 | namespace line_segment_mapping { 59 | 60 | class SlamKarto { 61 | public: 62 | SlamKarto(); 63 | ~SlamKarto(); 64 | 65 | void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan); 66 | bool mapCallback(nav_msgs::GetMap::Request& req, // NOLINT 67 | nav_msgs::GetMap::Response& res); // NOLINT 68 | 69 | private: 70 | bool getOdomPose(karto::Pose2* karto_pose, const ros::Time& t); 71 | karto::LaserRangeFinder* getLaser( 72 | const sensor_msgs::LaserScan::ConstPtr& scan); 73 | bool addScan(karto::LaserRangeFinder* laser, 74 | const sensor_msgs::LaserScan::ConstPtr& scan, 75 | karto::Pose2* karto_pose); 76 | bool updateMap(); 77 | void publishTransform(); 78 | void publishLoop(double transform_publish_period); 79 | // void publishGraphVisualization(); 80 | void publishLineSegmentMapVisualization(); 81 | 82 | // ROS handles 83 | ros::NodeHandle node_; 84 | tf::TransformListener tf_; 85 | tf::TransformBroadcaster* tfB_; 86 | message_filters::Subscriber* scan_filter_sub_; 87 | tf::MessageFilter* scan_filter_; 88 | ros::Publisher sst_; 89 | ros::Publisher marker_publisher_; 90 | ros::Publisher sstm_; 91 | ros::Publisher line_segment_map_marker_publisher_; 92 | ros::ServiceServer ss_; 93 | 94 | // The map that will be published / send to service callers 95 | nav_msgs::GetMap::Response map_; 96 | 97 | // Storage for ROS parameters 98 | std::string odom_frame_; 99 | std::string map_frame_; 100 | std::string base_frame_; 101 | std::string scan_topic_; 102 | int throttle_scans_; 103 | ros::Duration map_update_interval_; 104 | double resolution_; 105 | boost::mutex map_mutex_; 106 | boost::mutex map_to_odom_mutex_; 107 | 108 | // Karto bookkeeping 109 | std::unique_ptr line_segment_mapper_; 110 | std::unique_ptr line_segment_extractor_; 111 | std::unique_ptr dataset_; 112 | std::unique_ptr solver_; 113 | 114 | std::map lasers_; 115 | std::map lasers_inverted_; 116 | 117 | // Internal state 118 | bool got_map_ = false; 119 | int laser_count_ = 0; 120 | boost::thread* transform_thread_; 121 | tf::Transform map_to_odom_; 122 | bool inverted_laser_; 123 | double range_threshold_; 124 | }; 125 | 126 | SlamKarto::SlamKarto() { 127 | map_to_odom_.setIdentity(); 128 | // Retrieve parameters 129 | ros::NodeHandle private_nh_("~"); 130 | if (!private_nh_.getParam("odom_frame", odom_frame_)) { 131 | odom_frame_ = "odom"; 132 | } 133 | if (!private_nh_.getParam("map_frame", map_frame_)) { 134 | map_frame_ = "map"; 135 | } 136 | if (!private_nh_.getParam("base_frame", base_frame_)) { 137 | base_frame_ = "base_link"; 138 | } 139 | if (!private_nh_.getParam("scan_topic", scan_topic_)) { 140 | scan_topic_ = "base_scan"; 141 | } 142 | if (!private_nh_.getParam("throttle_scans", throttle_scans_)) { 143 | throttle_scans_ = 1; 144 | } 145 | double tmp; 146 | if (!private_nh_.getParam("map_update_interval", tmp)) { 147 | tmp = 5.0; 148 | } 149 | map_update_interval_.fromSec(tmp); 150 | if (!private_nh_.getParam("resolution", resolution_)) { 151 | // Compatibility with slam_gmapping, which uses "delta" to mean 152 | // resolution 153 | if (!private_nh_.getParam("delta", resolution_)) { 154 | resolution_ = 0.05; 155 | } 156 | } 157 | if (!private_nh_.getParam("range_threshold", range_threshold_)) { 158 | range_threshold_ = 12.0; 159 | } 160 | double transform_publish_period; 161 | private_nh_.param("transform_publish_period", transform_publish_period, 0.05); 162 | 163 | // Set up advertisements and subscriptions 164 | tfB_ = new tf::TransformBroadcaster(); 165 | sst_ = node_.advertise("map", 1, true); 166 | sstm_ = node_.advertise("map_metadata", 1, true); 167 | ss_ = node_.advertiseService("dynamic_map", &SlamKarto::mapCallback, this); 168 | scan_filter_sub_ = new message_filters::Subscriber( 169 | node_, scan_topic_, 5); 170 | scan_filter_ = new tf::MessageFilter( 171 | *scan_filter_sub_, tf_, odom_frame_, 5); 172 | scan_filter_->registerCallback( 173 | boost::bind(&SlamKarto::laserCallback, this, _1)); 174 | marker_publisher_ = node_.advertise( 175 | "visualization_marker_array", 1); 176 | line_segment_map_marker_publisher_ = 177 | node_.advertise("line_markers", 1); 178 | 179 | // Create a thread to periodically publish the latest map->odom 180 | // transform; it needs to go out regularly, uninterrupted by potentially 181 | // long periods of computation in our main loop. 182 | transform_thread_ = new boost::thread( 183 | boost::bind(&SlamKarto::publishLoop, this, transform_publish_period)); 184 | 185 | // Initialize Karto structures 186 | line_segment_mapper_ = std::make_unique(); 187 | line_segment_extractor_ = std::make_unique(); 188 | dataset_ = std::make_unique(); 189 | 190 | // Setting General Parameters from the Parameter Server 191 | bool use_scan_matching; 192 | if (private_nh_.getParam("use_scan_matching", use_scan_matching)) { 193 | line_segment_mapper_->setParamUseScanMatching(use_scan_matching); 194 | } 195 | 196 | bool use_scan_barycenter; 197 | if (private_nh_.getParam("use_scan_barycenter", use_scan_barycenter)) { 198 | line_segment_mapper_->setParamUseScanBarycenter(use_scan_barycenter); 199 | } 200 | 201 | double minimum_travel_distance; 202 | if (private_nh_.getParam("minimum_travel_distance", 203 | minimum_travel_distance)) { 204 | line_segment_mapper_->setParamMinimumTravelDistance( 205 | minimum_travel_distance); 206 | } 207 | 208 | double minimum_travel_heading; 209 | if (private_nh_.getParam("minimum_travel_heading", minimum_travel_heading)) { 210 | line_segment_mapper_->setParamMinimumTravelHeading(minimum_travel_heading); 211 | } 212 | 213 | int scan_buffer_size; 214 | if (private_nh_.getParam("scan_buffer_size", scan_buffer_size)) { 215 | line_segment_mapper_->setParamScanBufferSize(scan_buffer_size); 216 | } 217 | 218 | double scan_buffer_maximum_scan_distance; 219 | if (private_nh_.getParam("scan_buffer_maximum_scan_distance", 220 | scan_buffer_maximum_scan_distance)) { 221 | line_segment_mapper_->setParamScanBufferMaximumScanDistance( 222 | scan_buffer_maximum_scan_distance); 223 | } 224 | 225 | double link_match_minimum_response_fine; 226 | if (private_nh_.getParam("link_match_minimum_response_fine", 227 | link_match_minimum_response_fine)) { 228 | line_segment_mapper_->setParamLinkMatchMinimumResponseFine( 229 | link_match_minimum_response_fine); 230 | } 231 | 232 | double link_scan_maximum_distance; 233 | if (private_nh_.getParam("link_scan_maximum_distance", 234 | link_scan_maximum_distance)) { 235 | line_segment_mapper_->setParamLinkScanMaximumDistance( 236 | link_scan_maximum_distance); 237 | } 238 | 239 | double loop_search_maximum_distance; 240 | if (private_nh_.getParam("loop_search_maximum_distance", 241 | loop_search_maximum_distance)) { 242 | line_segment_mapper_->setParamLoopSearchMaximumDistance( 243 | loop_search_maximum_distance); 244 | } 245 | 246 | bool do_loop_closing; 247 | if (private_nh_.getParam("do_loop_closing", do_loop_closing)) { 248 | line_segment_mapper_->setParamDoLoopClosing(do_loop_closing); 249 | } 250 | 251 | int loop_match_minimum_chain_size; 252 | if (private_nh_.getParam("loop_match_minimum_chain_size", 253 | loop_match_minimum_chain_size)) { 254 | line_segment_mapper_->setParamLoopMatchMinimumChainSize( 255 | loop_match_minimum_chain_size); 256 | } 257 | 258 | double loop_match_maximum_variance_coarse; 259 | if (private_nh_.getParam("loop_match_maximum_variance_coarse", 260 | loop_match_maximum_variance_coarse)) { 261 | line_segment_mapper_->setParamLoopMatchMaximumVarianceCoarse( 262 | loop_match_maximum_variance_coarse); 263 | } 264 | 265 | double loop_match_minimum_response_coarse; 266 | if (private_nh_.getParam("loop_match_minimum_response_coarse", 267 | loop_match_minimum_response_coarse)) { 268 | line_segment_mapper_->setParamLoopMatchMinimumResponseCoarse( 269 | loop_match_minimum_response_coarse); 270 | } 271 | 272 | double loop_match_minimum_response_fine; 273 | if (private_nh_.getParam("loop_match_minimum_response_fine", 274 | loop_match_minimum_response_fine)) { 275 | line_segment_mapper_->setParamLoopMatchMinimumResponseFine( 276 | loop_match_minimum_response_fine); 277 | } 278 | 279 | // Setting Correlation Parameters from the Parameter Server 280 | 281 | double correlation_search_space_dimension; 282 | if (private_nh_.getParam("correlation_search_space_dimension", 283 | correlation_search_space_dimension)) { 284 | line_segment_mapper_->setParamCorrelationSearchSpaceDimension( 285 | correlation_search_space_dimension); 286 | } 287 | 288 | double correlation_search_space_resolution; 289 | if (private_nh_.getParam("correlation_search_space_resolution", 290 | correlation_search_space_resolution)) { 291 | line_segment_mapper_->setParamCorrelationSearchSpaceResolution( 292 | correlation_search_space_resolution); 293 | } 294 | 295 | double correlation_search_space_smear_deviation; 296 | if (private_nh_.getParam("correlation_search_space_smear_deviation", 297 | correlation_search_space_smear_deviation)) { 298 | line_segment_mapper_->setParamCorrelationSearchSpaceSmearDeviation( 299 | correlation_search_space_smear_deviation); 300 | } 301 | 302 | // Setting Correlation Parameters, Loop Closure Parameters from the Parameter 303 | // Server 304 | 305 | double loop_search_space_dimension; 306 | if (private_nh_.getParam("loop_search_space_dimension", 307 | loop_search_space_dimension)) { 308 | line_segment_mapper_->setParamLoopSearchSpaceDimension( 309 | loop_search_space_dimension); 310 | } 311 | 312 | double loop_search_space_resolution; 313 | if (private_nh_.getParam("loop_search_space_resolution", 314 | loop_search_space_resolution)) { 315 | line_segment_mapper_->setParamLoopSearchSpaceResolution( 316 | loop_search_space_resolution); 317 | } 318 | 319 | double loop_search_space_smear_deviation; 320 | if (private_nh_.getParam("loop_search_space_smear_deviation", 321 | loop_search_space_smear_deviation)) { 322 | line_segment_mapper_->setParamLoopSearchSpaceSmearDeviation( 323 | loop_search_space_smear_deviation); 324 | } 325 | 326 | // Setting Scan Matcher Parameters from the Parameter Server 327 | 328 | double distance_variance_penalty; 329 | if (private_nh_.getParam("distance_variance_penalty", 330 | distance_variance_penalty)) { 331 | line_segment_mapper_->setParamDistanceVariancePenalty( 332 | distance_variance_penalty); 333 | } 334 | 335 | double angle_variance_penalty; 336 | if (private_nh_.getParam("angle_variance_penalty", angle_variance_penalty)) { 337 | line_segment_mapper_->setParamAngleVariancePenalty(angle_variance_penalty); 338 | } 339 | 340 | double fine_search_angle_offset; 341 | if (private_nh_.getParam("fine_search_angle_offset", 342 | fine_search_angle_offset)) { 343 | line_segment_mapper_->setParamFineSearchAngleOffset( 344 | fine_search_angle_offset); 345 | } 346 | 347 | double coarse_search_angle_offset; 348 | if (private_nh_.getParam("coarse_search_angle_offset", 349 | coarse_search_angle_offset)) { 350 | line_segment_mapper_->setParamCoarseSearchAngleOffset( 351 | coarse_search_angle_offset); 352 | } 353 | 354 | double coarse_angle_resolution; 355 | if (private_nh_.getParam("coarse_angle_resolution", 356 | coarse_angle_resolution)) { 357 | line_segment_mapper_->setParamCoarseAngleResolution( 358 | coarse_angle_resolution); 359 | } 360 | 361 | double minimum_angle_penalty; 362 | if (private_nh_.getParam("minimum_angle_penalty", minimum_angle_penalty)) { 363 | line_segment_mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty); 364 | } 365 | 366 | double minimum_distance_penalty; 367 | if (private_nh_.getParam("minimum_distance_penalty", 368 | minimum_distance_penalty)) { 369 | line_segment_mapper_->setParamMinimumDistancePenalty( 370 | minimum_distance_penalty); 371 | } 372 | 373 | bool use_response_expansion; 374 | if (private_nh_.getParam("use_response_expansion", use_response_expansion)) { 375 | line_segment_mapper_->setParamUseResponseExpansion(use_response_expansion); 376 | } 377 | 378 | // Set solver to be used in loop closure 379 | solver_ = std::make_unique(); 380 | line_segment_mapper_->SetScanSolver(solver_.get()); 381 | } 382 | 383 | SlamKarto::~SlamKarto() { 384 | if (transform_thread_) { 385 | transform_thread_->join(); 386 | delete transform_thread_; 387 | } 388 | if (scan_filter_) { 389 | delete scan_filter_; 390 | } 391 | if (scan_filter_sub_) { 392 | delete scan_filter_sub_; 393 | } 394 | } 395 | 396 | void SlamKarto::publishLoop(double transform_publish_period) { 397 | if (transform_publish_period == 0) { 398 | return; 399 | } 400 | 401 | ros::Rate r(1.0 / transform_publish_period); 402 | while (ros::ok()) { 403 | publishTransform(); 404 | r.sleep(); 405 | } 406 | } 407 | 408 | void SlamKarto::publishTransform() { 409 | boost::mutex::scoped_lock lock(map_to_odom_mutex_); 410 | // ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05); 411 | tfB_->sendTransform(tf::StampedTransform(map_to_odom_, ros::Time::now(), 412 | map_frame_, odom_frame_)); 413 | } 414 | 415 | karto::LaserRangeFinder* SlamKarto::getLaser( 416 | const sensor_msgs::LaserScan::ConstPtr& scan) { 417 | // Check whether we know about this laser yet 418 | if (lasers_.find(scan->header.frame_id) == lasers_.end()) { 419 | // New laser; need to create a Karto device for it. 420 | 421 | // Get the laser's pose, relative to base_link. 422 | tf::Stamped ident; 423 | tf::Stamped laser_pose; 424 | ident.setIdentity(); 425 | ident.frame_id_ = scan->header.frame_id; 426 | ident.stamp_ = scan->header.stamp; 427 | try { 428 | tf_.transformPose(base_frame_, ident, laser_pose); 429 | } catch (tf::TransformException& ex) { 430 | ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", 431 | ex.what()); 432 | return nullptr; 433 | } 434 | 435 | double yaw = tf::getYaw(laser_pose.getRotation()); 436 | 437 | ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f", 438 | scan->header.frame_id.c_str(), laser_pose.getOrigin().x(), 439 | laser_pose.getOrigin().y(), yaw); 440 | // To account for lasers that are mounted upside-down, 441 | // we create a point 1m above the laser and transform it into the laser 442 | // frame if the point's z-value is <=0, it is upside-down 443 | 444 | tf::Vector3 v; 445 | v.setValue(0, 0, 1 + laser_pose.getOrigin().z()); 446 | tf::Stamped up(v, scan->header.stamp, base_frame_); 447 | 448 | try { 449 | tf_.transformPoint(scan->header.frame_id, up, up); 450 | ROS_DEBUG("Z-Axis in sensor frame: %.6f", up.z()); 451 | } catch (tf::TransformException& ex) { 452 | ROS_WARN("Unable to determine orientation of laser: %s", ex.what()); 453 | return nullptr; 454 | } 455 | 456 | bool inverse = lasers_inverted_[scan->header.frame_id] = up.z() <= 0; 457 | if (inverse) { 458 | ROS_DEBUG("laser is mounted upside-down"); 459 | } 460 | 461 | // Create a laser range finder device and copy in data from the first 462 | // scan 463 | std::string name = scan->header.frame_id; 464 | karto::LaserRangeFinder* laser = 465 | karto::LaserRangeFinder::CreateLaserRangeFinder( 466 | karto::LaserRangeFinder_Custom, karto::Name(name)); 467 | laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(), 468 | laser_pose.getOrigin().y(), yaw)); 469 | laser->SetRangeThreshold(range_threshold_); 470 | laser->SetMinimumRange(scan->range_min); 471 | laser->SetMaximumRange(scan->range_max); 472 | laser->SetMinimumAngle(scan->angle_min); 473 | laser->SetMaximumAngle(scan->angle_max); 474 | laser->SetAngularResolution(scan->angle_increment); 475 | 476 | // Store this laser device for later 477 | lasers_[scan->header.frame_id] = laser; 478 | 479 | // Add it to the dataset, which seems to be necessary 480 | dataset_->Add(laser); 481 | } 482 | 483 | return lasers_[scan->header.frame_id]; 484 | } 485 | 486 | bool SlamKarto::getOdomPose(karto::Pose2* karto_pose, const ros::Time& t) { 487 | // Get the robot's pose 488 | tf::Stamped ident( 489 | tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0)), 490 | t, base_frame_); 491 | tf::Stamped odom_pose; 492 | try { 493 | tf_.transformPose(odom_frame_, ident, odom_pose); 494 | } catch (tf::TransformException& ex) { 495 | ROS_WARN("Failed to compute odom pose, skipping scan (%s)", ex.what()); 496 | return false; 497 | } 498 | double yaw = tf::getYaw(odom_pose.getRotation()); 499 | 500 | *karto_pose = 501 | karto::Pose2(odom_pose.getOrigin().x(), odom_pose.getOrigin().y(), yaw); 502 | return true; 503 | } 504 | 505 | void SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) { 506 | laser_count_++; 507 | if ((laser_count_ % throttle_scans_) != 0) { 508 | return; 509 | } 510 | 511 | static ros::Time last_map_update(0, 0); 512 | 513 | // Check whether we know about this laser yet 514 | karto::LaserRangeFinder* laser = getLaser(scan); 515 | 516 | if (!laser) { 517 | ROS_WARN("Failed to create laser device for %s; discarding scan", 518 | scan->header.frame_id.c_str()); 519 | return; 520 | } 521 | 522 | karto::Pose2 odom_pose; 523 | if (addScan(laser, scan, &odom_pose)) { 524 | ROS_DEBUG("added scan at pose: %.6f %.6f %.6f", odom_pose.GetX(), 525 | odom_pose.GetY(), odom_pose.GetHeading()); 526 | 527 | // visualization_msgs::MarkerArray marray; 528 | // solver_->publishGraphVisualization(marray); 529 | // marker_publisher_.publish(marray); 530 | 531 | publishLineSegmentMapVisualization(); 532 | 533 | if (!got_map_ || 534 | (scan->header.stamp - last_map_update) > map_update_interval_) { 535 | if (updateMap()) { 536 | last_map_update = scan->header.stamp; 537 | got_map_ = true; 538 | ROS_DEBUG("Updated the map"); 539 | } 540 | } 541 | } 542 | } 543 | 544 | bool SlamKarto::updateMap() { 545 | boost::mutex::scoped_lock lock(map_mutex_); 546 | 547 | karto::OccupancyGrid* occ_grid = karto::OccupancyGrid::CreateFromScans( 548 | line_segment_mapper_->GetAllProcessedScans(), resolution_); 549 | 550 | if (!occ_grid) { 551 | return false; 552 | } 553 | 554 | if (!got_map_) { 555 | map_.map.info.resolution = resolution_; 556 | map_.map.info.origin.position.x = 0.0; 557 | map_.map.info.origin.position.y = 0.0; 558 | map_.map.info.origin.position.z = 0.0; 559 | map_.map.info.origin.orientation.x = 0.0; 560 | map_.map.info.origin.orientation.y = 0.0; 561 | map_.map.info.origin.orientation.z = 0.0; 562 | map_.map.info.origin.orientation.w = 1.0; 563 | } 564 | 565 | // Translate to ROS format 566 | int32_t width = occ_grid->GetWidth(); 567 | int32_t height = occ_grid->GetHeight(); 568 | karto::Vector2 offset = 569 | occ_grid->GetCoordinateConverter()->GetOffset(); 570 | 571 | if (map_.map.info.width != (unsigned int)width || 572 | map_.map.info.height != (unsigned int)height || 573 | map_.map.info.origin.position.x != offset.GetX() || 574 | map_.map.info.origin.position.y != offset.GetY()) { 575 | map_.map.info.origin.position.x = offset.GetX(); 576 | map_.map.info.origin.position.y = offset.GetY(); 577 | map_.map.info.width = width; 578 | map_.map.info.height = height; 579 | map_.map.data.resize(map_.map.info.width * map_.map.info.height); 580 | } 581 | 582 | for (int32_t y = 0; y < height; y++) { 583 | for (int32_t x = 0; x < width; x++) { 584 | // Getting the value at position x,y 585 | uint8_t value = occ_grid->GetValue(karto::Vector2(x, y)); 586 | 587 | switch (value) { 588 | case karto::GridStates_Unknown: 589 | map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1; 590 | break; 591 | 592 | case karto::GridStates_Occupied: 593 | map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100; 594 | break; 595 | 596 | case karto::GridStates_Free: 597 | map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0; 598 | break; 599 | 600 | default: 601 | ROS_WARN("Encountered unknown cell value at %d, %d", x, y); 602 | break; 603 | } 604 | } 605 | } 606 | 607 | // Set the header information on the map 608 | map_.map.header.stamp = ros::Time::now(); 609 | map_.map.header.frame_id = map_frame_; 610 | 611 | sst_.publish(map_.map); 612 | sstm_.publish(map_.map.info); 613 | 614 | delete occ_grid; 615 | 616 | return true; 617 | } 618 | 619 | void SlamKarto::publishLineSegmentMapVisualization() { 620 | const LineSegmentPtrHashTable line_segment_hash_table = 621 | line_segment_mapper_->GetLineSegmentMapManager()->GetLineSegmentMap(); 622 | 623 | visualization_msgs::Marker line_segment_map_marker; 624 | line_segment_map_marker.ns = "karto"; 625 | line_segment_map_marker.id = 0; 626 | line_segment_map_marker.type = visualization_msgs::Marker::LINE_LIST; 627 | line_segment_map_marker.scale.x = 0.1; 628 | line_segment_map_marker.color.r = 1.0; 629 | line_segment_map_marker.color.g = 0.0; 630 | line_segment_map_marker.color.b = 0.0; 631 | line_segment_map_marker.color.a = 1.0; 632 | line_segment_map_marker.pose.orientation = 633 | tf::createQuaternionMsgFromYaw(0.0); 634 | 635 | for (auto hash_iter = line_segment_hash_table.begin(); 636 | hash_iter != line_segment_hash_table.end(); ++hash_iter) { 637 | // if (iter->second->n < 5) continue; 638 | 639 | const LineSegmentPtr merged_line_segment = hash_iter->second; 640 | 641 | geometry_msgs::Point p_start; 642 | p_start.x = merged_line_segment->GetStartPoint().GetX(); 643 | p_start.y = merged_line_segment->GetStartPoint().GetY(); 644 | p_start.z = 0; 645 | line_segment_map_marker.points.push_back(p_start); 646 | geometry_msgs::Point p_end; 647 | p_end.x = merged_line_segment->GetEndPoint().GetX(); 648 | p_end.y = merged_line_segment->GetEndPoint().GetY(); 649 | p_end.z = 0; 650 | line_segment_map_marker.points.push_back(p_end); 651 | } 652 | line_segment_map_marker.header.frame_id = map_frame_; 653 | line_segment_map_marker.header.stamp = ros::Time::now(); 654 | line_segment_map_marker_publisher_.publish(line_segment_map_marker); 655 | } 656 | 657 | bool SlamKarto::addScan(karto::LaserRangeFinder* laser, 658 | const sensor_msgs::LaserScan::ConstPtr& scan, 659 | karto::Pose2* karto_pose) { 660 | if (!getOdomPose(karto_pose, scan->header.stamp)) { 661 | return false; 662 | } 663 | 664 | // Create a vector of doubles for karto 665 | std::vector readings; 666 | 667 | if (lasers_inverted_[scan->header.frame_id]) { 668 | for (auto it = scan->ranges.rbegin(); it != scan->ranges.rend(); ++it) { 669 | readings.push_back(*it); 670 | } 671 | } else { 672 | for (auto it = scan->ranges.begin(); it != scan->ranges.end(); ++it) { 673 | readings.push_back(*it); 674 | } 675 | } 676 | 677 | // create localized range scan 678 | LineSegmentPtrVector line_segments; 679 | line_segment_extractor_->extract_lines(readings, laser, &line_segments); 680 | 681 | if (line_segments.empty()) { 682 | return false; 683 | } 684 | 685 | karto::LocalizedRangeScan* range_scan = 686 | new karto::LocalizedRangeScan(laser->GetName(), readings); 687 | range_scan->SetTime(scan->header.stamp.toSec()); 688 | range_scan->SetOdometricPose(*karto_pose); 689 | range_scan->SetCorrectedPose(*karto_pose); 690 | 691 | for (auto iter = line_segments.begin(); iter != line_segments.end(); ++iter) { 692 | iter->get()->SetScan(range_scan); 693 | } 694 | // Add the localized range scan to the mapper 695 | bool processed; 696 | if ((processed = line_segment_mapper_->Process(range_scan, line_segments))) { 697 | karto::Pose2 corrected_pose = range_scan->GetCorrectedPose(); 698 | tf::Transform corrected_pose_tf( 699 | tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()), 700 | tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)); 701 | 702 | // Compute the map->odom transform 703 | tf::Stamped odom_to_map; 704 | try { 705 | tf_.transformPose(odom_frame_, 706 | tf::Stamped(corrected_pose_tf.inverse(), 707 | scan->header.stamp, base_frame_), 708 | odom_to_map); 709 | } catch (tf::TransformException& ex) { 710 | ROS_ERROR("Transform from base_link to odom failed\n"); 711 | odom_to_map.setIdentity(); 712 | } 713 | 714 | map_to_odom_mutex_.lock(); 715 | map_to_odom_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()), 716 | tf::Point(odom_to_map.getOrigin())) 717 | .inverse(); 718 | map_to_odom_mutex_.unlock(); 719 | 720 | // Add the localized range scan to the dataset (for memory management) 721 | dataset_->Add(range_scan); 722 | } else { 723 | delete range_scan; 724 | } 725 | 726 | return processed; 727 | } 728 | 729 | bool SlamKarto::mapCallback(nav_msgs::GetMap::Request& req, 730 | nav_msgs::GetMap::Response& res) { 731 | UNUSED(req); 732 | boost::mutex::scoped_lock lock(map_mutex_); 733 | if (got_map_ && map_.map.info.width && map_.map.info.height) { 734 | res = map_; 735 | return true; 736 | } 737 | return false; 738 | } 739 | 740 | } // namespace line_segment_mapping 741 | 742 | int main(int argc, char** argv) { 743 | ros::init(argc, argv, "slam_karto"); 744 | 745 | line_segment_mapping::SlamKarto kn; 746 | 747 | ros::spin(); 748 | 749 | return 0; 750 | } 751 | --------------------------------------------------------------------------------