├── CMakeLists.txt ├── README.md ├── assets └── SystemOverview.png ├── backward-cpp ├── BackwardConfig.cmake ├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── backward.cpp ├── backward.hpp ├── builds.sh └── conanfile.py ├── cmake_modules └── FindEigen.cmake ├── lib ├── libSLIM.a ├── libScancontext.a ├── libbackward.a ├── libpmc_slim.a ├── libspmm.a └── libsvm.a ├── modules ├── clipper.cpp ├── clipper.h ├── dbscan │ ├── kdtree_cluster.h │ ├── precomp_cluster.h │ ├── simple_cluster.h │ └── surface_cluster.h ├── extractor.cpp ├── extractor.h ├── factor │ ├── graff_coordinate.h │ ├── laser_edge_factor.cpp │ ├── laser_edge_factor.h │ ├── laser_surf_factor.cpp │ ├── laser_surf_factor.h │ ├── line_info.h │ ├── local_param.cpp │ ├── local_param.h │ ├── marginalization_factor.cpp │ ├── marginalization_factor.h │ ├── math_utility.h │ ├── prior_pose_factor.cpp │ ├── prior_pose_factor.h │ ├── relative_pose_factor.cpp │ ├── relative_pose_factor.h │ └── surface_info.h ├── frame.cpp ├── frame.h ├── json.cpp ├── json.hpp ├── landmark.cpp ├── landmark.h ├── locator.cpp ├── locator.h ├── multi_map_merger.cpp ├── multi_map_merger.h ├── nfr_solver.cpp ├── nfr_solver.h ├── observation.cpp ├── observation.h ├── pcm_solver.cpp ├── pcm_solver.h ├── scene_graph.cpp ├── scene_graph.h ├── scene_graph_node.h ├── semantic_definition.h ├── spmm │ ├── CMakeLists.txt │ ├── device_buffer.h │ ├── macro.h │ ├── spmm.cpp │ └── spmm.h ├── transform.h ├── travel │ ├── aos.hpp │ └── tgs.hpp ├── utility.cpp ├── utility.h ├── vector_map.cpp ├── vector_map.h ├── viewer.cpp ├── viewer.h ├── voxel.cpp └── voxel.h ├── test ├── cloud_visualizer.cpp ├── map_merge.cpp ├── map_visualizer.cpp ├── offline_mapping.cpp └── relocalization.cpp └── thirdparty └── pmc ├── CMakeLists.txt ├── pmc.h ├── pmc_clique_utils.cpp ├── pmc_cores.cpp ├── pmc_driver.cpp ├── pmc_graph.cpp ├── pmc_graph.h ├── pmc_headers.h ├── pmc_heu.cpp ├── pmc_heu.h ├── pmc_input.h ├── pmc_lib.cpp ├── pmc_maxclique.cpp ├── pmc_maxclique.h ├── pmc_neigh_coloring.h ├── pmc_neigh_cores.h ├── pmc_utils.cpp ├── pmc_utils.h ├── pmc_vertex.h ├── pmcx_maxclique.cpp ├── pmcx_maxclique.h ├── pmcx_maxclique_basic.cpp └── pmcx_maxclique_basic.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(SLIM LANGUAGES C CXX) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14 -O3 -fPIC") 6 | 7 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 8 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 9 | 10 | find_package(CUDAToolkit REQUIRED) 11 | 12 | include(GNUInstallDirs) 13 | 14 | # Global CXX flags/options 15 | set(CMAKE_CXX_STANDARD 14) 16 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 17 | set(CMAKE_CXX_EXTENSIONS OFF) 18 | 19 | # Global CUDA CXX flags/options 20 | set(CUDA_HOST_COMPILER ${CMAKE_CXX_COMPILER}) 21 | set(CMAKE_CUDA_STANDARD 14) 22 | set(CMAKE_CUDA_STANDARD_REQUIRED ON) 23 | set(CMAKE_CUDA_EXTENSIONS OFF) 24 | 25 | # Debug options 26 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -O0 -g") 27 | set(CMAKE_CUDA_FLAGS_DEBUG "${CMAKE_CUDA_FLAGS} -O0 -g -lineinfo") 28 | 29 | find_package(Eigen REQUIRED) 30 | find_package(Ceres REQUIRED) 31 | find_package(OpenCV REQUIRED) 32 | find_package(PCL REQUIRED) 33 | find_package(OpenMP REQUIRED) 34 | find_package(Pangolin REQUIRED) 35 | find_package(Boost REQUIRED COMPONENTS regex thread system filesystem) 36 | find_package(Boost REQUIRED COMPONENTS graph filesystem) 37 | 38 | add_definitions(-DBOOST_NO_CXX11_SCOPED_ENUMS) 39 | 40 | add_subdirectory(thirdparty/pmc) 41 | add_subdirectory(modules/spmm) 42 | add_subdirectory(backward-cpp) 43 | 44 | include_directories( 45 | ${EIGEN3_INCLUDE_DIR} 46 | ${CERES_INCLUDE_DIRS} 47 | ${PCL_INCLUDE_DIRS} 48 | ${OpenCV_INCLUDE_DIRS} 49 | ${Pangolin_INCLUDE_DIR} 50 | ${Boost_INCLUDE_DIRS} 51 | ${Boost_INCLUDE_DIRS} 52 | ${CMAKE_CURRENT_LIST_DIR}/include 53 | modules 54 | modules/factor 55 | modules/dbscan 56 | thirdparty 57 | thirdparty/pmc 58 | ) 59 | 60 | add_library(SLIM 61 | modules/vector_map.cpp 62 | modules/viewer.cpp 63 | modules/frame.cpp 64 | modules/landmark.cpp 65 | modules/voxel.cpp 66 | modules/pcm_solver.cpp 67 | modules/extractor.cpp 68 | modules/scene_graph.cpp 69 | modules/multi_map_merger.cpp 70 | modules/clipper.cpp 71 | modules/locator.cpp 72 | modules/nfr_solver.cpp 73 | modules/json.cpp 74 | modules/observation.cpp 75 | modules/utility.cpp 76 | modules/factor/marginalization_factor.cpp 77 | modules/factor/laser_edge_factor.cpp 78 | modules/factor/laser_surf_factor.cpp 79 | modules/factor/relative_pose_factor.cpp 80 | modules/factor/prior_pose_factor.cpp 81 | modules/factor/local_param.cpp 82 | ) 83 | 84 | target_link_libraries(SLIM 85 | ${CERES_LIBRARIES} 86 | ${PCL_LIBRARIES} 87 | ${OpenCV_LIBRARIES} 88 | ${Pangolin_LIBRARY} 89 | ${Boost_LIBRARIES} 90 | ${Boost_LIBRARY_DIRS} 91 | spmm 92 | OpenMP::OpenMP_CXX 93 | pmc_slim 94 | ) 95 | 96 | add_executable(offline_mapping test/offline_mapping.cpp ${BACKWARD_ENABLE}) 97 | target_link_libraries(offline_mapping SLIM ) 98 | add_backward(offline_mapping) 99 | 100 | add_executable(cloud_visualizer test/cloud_visualizer.cpp) 101 | target_link_libraries(cloud_visualizer SLIM ) 102 | 103 | add_executable(map_merge test/map_merge.cpp ${BACKWARD_ENABLE}) 104 | target_link_libraries(map_merge SLIM ) 105 | add_backward(map_merge) 106 | 107 | add_executable(map_visualizer test/map_visualizer.cpp ${BACKWARD_ENABLE}) 108 | target_link_libraries(map_visualizer SLIM ) 109 | add_backward(map_visualizer) 110 | 111 | add_executable(relocalization test/relocalization.cpp ${BACKWARD_ENABLE}) 112 | target_link_libraries(relocalization SLIM ) 113 | add_backward(relocalization) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ##
SLIM: Scalable and Lightweight LiDAR Mapping in Urban Environments
4 | 5 |

6 | arXiv | 7 | Video 8 |

9 | 10 | 11 |
12 | 17 | 21 |
22 | 23 | > Zehuan Yu, [Zhijian Qiao](https://qiaozhijian.github.io/), Wenyi Liu, [Huan Yin](https://huanyin94.github.io/) and [Shaojie Shen](https://uav.hkust.edu.hk/group/) 24 | > 25 | 26 | 27 | 30 | ## Introduction 31 | This is the official code repository of "SLIM: Scalable and Lightweight LiDAR Mapping in Urban Environments". SLIM is a scalable multi-session SLAM algorithm. It enables the construction of vectorized maps and supports scalable map merging. It provides a effective solution for multi-session mapping and is easily extensible, for example, to crowdsourcing visual vectorized maps in autonomous driving. 32 | 33 |
34 | 35 | **Features**: 36 | + **Vectorized LiDAR Mapping with Bundle Adjustment** 37 | + **Scalable Map Merging for Vectorized Map** 38 | + **Relocalization on Vectorized Map** 39 | + **General Solution for Sparsification in Graph-based SLAM** 40 | 41 | ## Code 42 | Code will be released after manuscript decision. 43 | 44 | 45 | ## Acknowledgements 46 | This work was supported in part by the HKUST Postgraduate Studentship, 47 | and in part by the HKUST-DJI Joint Innovation Laboratory. 48 | 49 | 51 | -------------------------------------------------------------------------------- /assets/SystemOverview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/SLIM/a1e3fd7cce71f09c3152070443b2c2a787a8f756/assets/SystemOverview.png -------------------------------------------------------------------------------- /backward-cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # CMakeLists.txt 3 | # Copyright 2013 Google Inc. All Rights Reserved. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | cmake_minimum_required(VERSION 3.0) 24 | project(backward CXX) 25 | 26 | # Introduce variables: 27 | # * CMAKE_INSTALL_LIBDIR 28 | # * CMAKE_INSTALL_BINDIR 29 | # * CMAKE_INSTALL_INCLUDEDIR 30 | include(GNUInstallDirs) 31 | 32 | include(BackwardConfig.cmake) 33 | 34 | # check if compiler is nvcc or nvcc_wrapper 35 | set(COMPILER_IS_NVCC false) 36 | get_filename_component(COMPILER_NAME ${CMAKE_CXX_COMPILER} NAME) 37 | if (COMPILER_NAME MATCHES "^nvcc") 38 | set(COMPILER_IS_NVCC true) 39 | endif() 40 | 41 | if (DEFINED ENV{OMPI_CXX} OR DEFINED ENV{MPICH_CXX}) 42 | if ( ($ENV{OMPI_CXX} MATCHES "nvcc") OR ($ENV{MPICH_CXX} MATCHES "nvcc") ) 43 | set(COMPILER_IS_NVCC true) 44 | endif() 45 | endif() 46 | 47 | # set CXX standard 48 | set(CMAKE_CXX_STANDARD_REQUIRED True) 49 | set(CMAKE_CXX_STANDARD 11) 50 | if (${COMPILER_IS_NVCC}) 51 | # GNU CXX extensions are not supported by nvcc 52 | set(CMAKE_CXX_EXTENSIONS OFF) 53 | endif() 54 | 55 | ############################################################################### 56 | # COMPILER FLAGS 57 | ############################################################################### 58 | 59 | if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" OR CMAKE_COMPILER_IS_GNUCXX) 60 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 61 | if (NOT ${COMPILER_IS_NVCC}) 62 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pedantic-errors") 63 | endif() 64 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 65 | endif() 66 | 67 | ############################################################################### 68 | # BACKWARD OBJECT 69 | ############################################################################### 70 | 71 | add_library(backward_object OBJECT backward.cpp) 72 | target_compile_definitions(backward_object PRIVATE ${BACKWARD_DEFINITIONS}) 73 | target_include_directories(backward_object PRIVATE ${BACKWARD_INCLUDE_DIRS}) 74 | set(BACKWARD_ENABLE $ CACHE STRING 75 | "Link with this object to setup backward automatically") 76 | 77 | 78 | ############################################################################### 79 | # BACKWARD LIBRARY (Includes backward.cpp) 80 | ############################################################################### 81 | option(BACKWARD_SHARED "Build dynamic backward-cpp shared lib" OFF) 82 | 83 | if(BACKWARD_SHARED) 84 | set(libtype SHARED) 85 | endif() 86 | add_library(backward ${libtype} backward.cpp) 87 | target_compile_definitions(backward PUBLIC ${BACKWARD_DEFINITIONS}) 88 | target_include_directories(backward PUBLIC ${BACKWARD_INCLUDE_DIRS}) 89 | 90 | install( 91 | FILES "backward.hpp" 92 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 93 | ) 94 | install( 95 | FILES "BackwardConfig.cmake" 96 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/backward 97 | ) 98 | -------------------------------------------------------------------------------- /backward-cpp/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2013 Google Inc. All Rights Reserved. 2 | 3 | The MIT License (MIT) 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 9 | of the Software, and to permit persons to whom the Software is furnished to do 10 | so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /backward-cpp/backward.cpp: -------------------------------------------------------------------------------- 1 | // Pick your poison. 2 | // 3 | // On GNU/Linux, you have few choices to get the most out of your stack trace. 4 | // 5 | // By default you get: 6 | // - object filename 7 | // - function name 8 | // 9 | // In order to add: 10 | // - source filename 11 | // - line and column numbers 12 | // - source code snippet (assuming the file is accessible) 13 | 14 | // Install one of the following libraries then uncomment one of the macro (or 15 | // better, add the detection of the lib and the macro definition in your build 16 | // system) 17 | 18 | // - apt-get install libdw-dev ... 19 | // - g++/clang++ -ldw ... 20 | // #define BACKWARD_HAS_DW 1 21 | 22 | // - apt-get install binutils-dev ... 23 | // - g++/clang++ -lbfd ... 24 | // #define BACKWARD_HAS_BFD 1 25 | 26 | // - apt-get install libdwarf-dev ... 27 | // - g++/clang++ -ldwarf ... 28 | // #define BACKWARD_HAS_DWARF 1 29 | 30 | // Regardless of the library you choose to read the debug information, 31 | // for potentially more detailed stack traces you can use libunwind 32 | // - apt-get install libunwind-dev 33 | // - g++/clang++ -lunwind 34 | // #define BACKWARD_HAS_LIBUNWIND 1 35 | 36 | #include "backward.hpp" 37 | 38 | namespace backward { 39 | 40 | backward::SignalHandling sh; 41 | 42 | } // namespace backward 43 | -------------------------------------------------------------------------------- /backward-cpp/builds.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | COMPILERS_CXX98=`cat</dev/null 28 | ( 29 | cd "$builddir" 30 | cmake -DCMAKE_BUILD_TYPE=$buildtype -DBACKWARD_TESTS=ON .. 31 | ) 32 | } 33 | 34 | function build() { 35 | local builddir=$1 36 | shift 37 | make -C "$builddir" $@ 38 | } 39 | 40 | function dotest() { 41 | local builddir=$1 42 | shift 43 | make -C "$builddir" test $@ 44 | return 0 45 | } 46 | 47 | function do_action() { 48 | local lang=$1 49 | local action=$2 50 | shift 2 51 | 52 | for compiler in $COMPILERS; do 53 | local builddir="build_${lang}_${compiler}" 54 | 55 | if [[ $action == "cmake" ]]; then 56 | buildtype=$1 57 | mkbuild $compiler $lang "$buildtype" "$builddir" 58 | [[ $? != 0 ]] && exit 59 | elif [[ $action == "make" ]]; then 60 | build "$builddir" $@ 61 | [[ $? != 0 ]] && exit 62 | elif [[ $action == "test" ]]; then 63 | dotest "$builddir" $@ 64 | [[ $? != 0 ]] && exit 65 | elif [[ $action == "clean" ]]; then 66 | rm -r "$builddir" 67 | else 68 | echo "usage: $0 cmake [debug|release|relwithdbg]|make|test|clean" 69 | exit 255 70 | fi 71 | done 72 | } 73 | 74 | COMPILERS=$COMPILERS_CXX98 75 | do_action c++98 $@ 76 | COMPILERS=$COMPILERS_CXX11 77 | do_action c++11 $@ 78 | -------------------------------------------------------------------------------- /backward-cpp/conanfile.py: -------------------------------------------------------------------------------- 1 | from conans import ConanFile, CMake 2 | import os 3 | 4 | class BackwardCpp(ConanFile): 5 | settings = 'os', 'compiler', 'build_type', 'arch' 6 | name = 'backward' 7 | url = 'https://github.com/bombela/backward-cpp' 8 | license = 'MIT' 9 | version = '1.3.0' 10 | options = { 11 | 'stack_walking_unwind': [True, False], 12 | 'stack_walking_backtrace': [True, False], 13 | 'stack_details_auto_detect': [True, False], 14 | 'stack_details_backtrace_symbol': [True, False], 15 | 'stack_details_dw': [True, False], 16 | 'stack_details_bfd': [True, False], 17 | 'shared': [True, False] 18 | } 19 | default_options = ( 20 | 'stack_walking_unwind=True', 21 | 'stack_walking_backtrace=False', 22 | 'stack_details_auto_detect=True', 23 | 'stack_details_backtrace_symbol=False', 24 | 'stack_details_dw=False', 25 | 'stack_details_bfd=False', 26 | 'shared=False' 27 | ) 28 | exports = 'backward.cpp', 'backward.hpp', 'test/*', 'CMakeLists.txt', 'BackwardConfig.cmake' 29 | generators = 'cmake' 30 | 31 | def build(self): 32 | cmake = CMake(self) 33 | 34 | cmake.configure(defs={'BACKWARD_' + name.upper(): value for name, value in self.options.values.as_list()}) 35 | cmake.build() 36 | 37 | def package(self): 38 | self.copy('backward.hpp', os.path.join('include', 'backward')) 39 | self.copy('*.a', dst='lib') 40 | self.copy('*.so', dst='lib') 41 | self.copy('*.lib', dst='lib') 42 | self.copy('*.dll', dst='lib') 43 | 44 | def package_info(self): 45 | self.cpp_info.libs = ['backward'] 46 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # This find_module finds Eigen3 and assigns variables to standard variable names 16 | # This allows the module to work with ament_export_dependencies 17 | 18 | include(FindPackageHandleStandardArgs) 19 | 20 | # Use Eigen3Config.cmake to do most of the work 21 | find_package(Eigen3 CONFIG QUIET) 22 | 23 | if(Eigen3_FOUND AND NOT EIGEN3_FOUND) 24 | # Special case for Eigen 3.3.4 chocolately package 25 | find_package_handle_standard_args( 26 | Eigen3 27 | VERSION_VAR 28 | Eigen3_VERSION 29 | REQUIRED_VARS 30 | Eigen3_FOUND 31 | ) 32 | else() 33 | find_package_handle_standard_args( 34 | Eigen3 35 | VERSION_VAR 36 | EIGEN3_VERSION_STRING 37 | REQUIRED_VARS 38 | EIGEN3_FOUND 39 | ) 40 | endif() 41 | 42 | # Set standard variable names 43 | # https://cmake.org/cmake/help/v3.5/manual/cmake-developer.7.html#standard-variable-names 44 | set(_standard_vars 45 | INCLUDE_DIRS 46 | DEFINITIONS 47 | ROOT_DIR) 48 | foreach(_suffix ${_standard_vars}) 49 | set(_wrong_var "EIGEN3_${_suffix}") 50 | set(_right_var "Eigen3_${_suffix}") 51 | if(NOT DEFINED ${_right_var} AND DEFINED ${_wrong_var}) 52 | set(${_right_var} "${${_wrong_var}}") 53 | endif() 54 | endforeach() -------------------------------------------------------------------------------- /lib/libSLIM.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/SLIM/a1e3fd7cce71f09c3152070443b2c2a787a8f756/lib/libSLIM.a -------------------------------------------------------------------------------- /lib/libScancontext.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/SLIM/a1e3fd7cce71f09c3152070443b2c2a787a8f756/lib/libScancontext.a -------------------------------------------------------------------------------- /lib/libbackward.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/SLIM/a1e3fd7cce71f09c3152070443b2c2a787a8f756/lib/libbackward.a -------------------------------------------------------------------------------- /lib/libpmc_slim.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/SLIM/a1e3fd7cce71f09c3152070443b2c2a787a8f756/lib/libpmc_slim.a -------------------------------------------------------------------------------- /lib/libspmm.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/SLIM/a1e3fd7cce71f09c3152070443b2c2a787a8f756/lib/libspmm.a -------------------------------------------------------------------------------- /lib/libsvm.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/SLIM/a1e3fd7cce71f09c3152070443b2c2a787a8f756/lib/libsvm.a -------------------------------------------------------------------------------- /modules/dbscan/kdtree_cluster.h: -------------------------------------------------------------------------------- 1 | #ifndef KDTREE_CLUSTER_H 2 | #define KDTREE_CLUSTER_H 3 | 4 | #include 5 | #include "simple_cluster.h" 6 | 7 | namespace SLIM { 8 | 9 | template 10 | class DBSCANKdtreeCluster: public DBSCANSimpleCluster { 11 | protected: 12 | virtual int radiusSearch (int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances) const { 13 | return this->search_method_->radiusSearch(index, radius, k_indices, k_sqr_distances); 14 | } 15 | }; // class DBSCANKdtreeCluster 16 | 17 | #endif // KDTREE_CLUSTER_H 18 | 19 | } -------------------------------------------------------------------------------- /modules/dbscan/precomp_cluster.h: -------------------------------------------------------------------------------- 1 | #ifndef PRECOMP_CLUSTER_H 2 | #define PRECOMP_CLUSTER_H 3 | 4 | #include 5 | #include "simple_cluster.h" 6 | 7 | namespace SLIM { 8 | 9 | template 10 | class DBSCANPrecompCluster : public DBSCANSimpleCluster { 11 | public: 12 | virtual void setInputCloud(pcl::PointCloud::Ptr cloud) { 13 | this->input_cloud_ = cloud; 14 | int size = this->input_cloud_->points.size(); 15 | adjoint_indexes_ = std::vector>(size, std::vector()); 16 | distances_ = std::vector>(size, std::vector()); 17 | precomp(); 18 | } 19 | 20 | protected: 21 | std::vector> distances_; 22 | std::vector> adjoint_indexes_; 23 | 24 | void precomp() { 25 | int size = this->input_cloud_->points.size(); 26 | for (int i = 0; i < size; i++) { 27 | adjoint_indexes_[i].push_back(i); 28 | distances_[i].push_back(0.0); 29 | } 30 | double radius_square = this->eps_ * this->eps_; 31 | for (int i = 0; i < size; i++) { 32 | for (int j = i+1; j < size; j++) { 33 | double distance_x = this->input_cloud_->points[i].x - this->input_cloud_->points[j].x; 34 | double distance_y = this->input_cloud_->points[i].y - this->input_cloud_->points[j].y; 35 | double distance_z = this->input_cloud_->points[i].z - this->input_cloud_->points[j].z; 36 | double distance_square = distance_x * distance_x + distance_y * distance_y + distance_z * distance_z; 37 | if (distance_square <= radius_square) { 38 | adjoint_indexes_[i].push_back(j); 39 | adjoint_indexes_[j].push_back(i); 40 | double distance = std::sqrt(distance_square); 41 | distances_[i].push_back(distance); 42 | distances_[j].push_back(distance); 43 | } 44 | } 45 | } 46 | } 47 | 48 | virtual int radiusSearch(int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances) const { 49 | // radius = eps_ 50 | k_indices = adjoint_indexes_[index]; 51 | k_sqr_distances = distances_[index]; 52 | return k_indices.size(); 53 | } 54 | }; // class DBSCANPrecompCluster 55 | 56 | #endif // PRECOMP_CLUSTER_H 57 | 58 | } -------------------------------------------------------------------------------- /modules/dbscan/simple_cluster.h: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_CLUSTER_H 2 | #define SIMPLE_CLUSTER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace SLIM { 8 | 9 | #define UN_PROCESSED 0 10 | #define PROCESSING 1 11 | #define PROCESSED 2 12 | 13 | inline bool comparePointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b) { 14 | return (a.indices.size () < b.indices.size ()); 15 | } 16 | 17 | template 18 | class DBSCANSimpleCluster { 19 | public: 20 | typedef typename pcl::PointCloud::Ptr PointCloudPtr; 21 | typedef typename pcl::search::KdTree::Ptr KdTreePtr; 22 | virtual void setInputCloud(PointCloudPtr cloud) { 23 | input_cloud_ = cloud; 24 | } 25 | 26 | void setSearchMethod(KdTreePtr tree) { 27 | search_method_ = tree; 28 | } 29 | 30 | void extract(std::vector& cluster_indices) { 31 | std::vector nn_indices; 32 | std::vector nn_distances; 33 | std::vector is_noise(input_cloud_->points.size(), false); 34 | std::vector types(input_cloud_->points.size(), UN_PROCESSED); 35 | for (int i = 0; i < input_cloud_->points.size(); i++) { 36 | if (types[i] == PROCESSED) { 37 | continue; 38 | } 39 | int nn_size = radiusSearch(i, eps_, nn_indices, nn_distances); 40 | if (nn_size < minPts_) { 41 | is_noise[i] = true; 42 | continue; 43 | } 44 | 45 | std::vector seed_queue; 46 | seed_queue.push_back(i); 47 | types[i] = PROCESSED; 48 | 49 | for (int j = 0; j < nn_size; j++) { 50 | if (nn_indices[j] != i) { 51 | seed_queue.push_back(nn_indices[j]); 52 | types[nn_indices[j]] = PROCESSING; 53 | } 54 | } // for every point near the chosen core point. 55 | int sq_idx = 1; 56 | while (sq_idx < seed_queue.size()) { 57 | int cloud_index = seed_queue[sq_idx]; 58 | if (is_noise[cloud_index] || types[cloud_index] == PROCESSED) { 59 | // seed_queue.push_back(cloud_index); 60 | types[cloud_index] = PROCESSED; 61 | sq_idx++; 62 | continue; // no need to check neighbors. 63 | } 64 | nn_size = radiusSearch(cloud_index, eps_, nn_indices, nn_distances); 65 | if (nn_size >= minPts_) { 66 | for (int j = 0; j < nn_size; j++) { 67 | if (types[nn_indices[j]] == UN_PROCESSED) { 68 | seed_queue.push_back(nn_indices[j]); 69 | types[nn_indices[j]] = PROCESSING; 70 | } 71 | } 72 | } 73 | 74 | types[cloud_index] = PROCESSED; 75 | sq_idx++; 76 | } 77 | if (seed_queue.size() >= min_pts_per_cluster_ && seed_queue.size () <= max_pts_per_cluster_) { 78 | pcl::PointIndices r; 79 | r.indices.resize(seed_queue.size()); 80 | for (int j = 0; j < seed_queue.size(); ++j) { 81 | r.indices[j] = seed_queue[j]; 82 | } 83 | // These two lines should not be needed: (can anyone confirm?) -FF 84 | std::sort (r.indices.begin (), r.indices.end ()); 85 | r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); 86 | 87 | r.header = input_cloud_->header; 88 | cluster_indices.push_back (r); // We could avoid a copy by working directly in the vector 89 | } 90 | } // for every point in input cloud 91 | std::sort (cluster_indices.rbegin (), cluster_indices.rend (), comparePointClusters); 92 | } 93 | 94 | void setClusterTolerance(double tolerance) { 95 | eps_ = tolerance; 96 | } 97 | 98 | void setMinClusterSize (int min_cluster_size) { 99 | min_pts_per_cluster_ = min_cluster_size; 100 | } 101 | 102 | void setMaxClusterSize (int max_cluster_size) { 103 | max_pts_per_cluster_ = max_cluster_size; 104 | } 105 | 106 | void setCorePointMinPts(int core_point_min_pts) { 107 | minPts_ = core_point_min_pts; 108 | } 109 | 110 | protected: 111 | PointCloudPtr input_cloud_; 112 | 113 | double eps_ {0.0}; 114 | int minPts_ {1}; // not including the point itself. 115 | int min_pts_per_cluster_ {1}; 116 | int max_pts_per_cluster_ {std::numeric_limits::max()}; 117 | 118 | KdTreePtr search_method_; 119 | 120 | virtual int radiusSearch(int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances) const { 121 | k_indices.clear(); 122 | k_sqr_distances.clear(); 123 | k_indices.push_back(index); 124 | k_sqr_distances.push_back(0); 125 | int size = input_cloud_->points.size(); 126 | double radius_square = radius * radius; 127 | for (int i = 0; i < size; i++) { 128 | if (i == index) { 129 | continue; 130 | } 131 | double distance_x = input_cloud_->points[i].x - input_cloud_->points[index].x; 132 | double distance_y = input_cloud_->points[i].y - input_cloud_->points[index].y; 133 | double distance_z = input_cloud_->points[i].z - input_cloud_->points[index].z; 134 | double distance_square = distance_x * distance_x + distance_y * distance_y + distance_z * distance_z; 135 | if (distance_square <= radius_square) { 136 | k_indices.push_back(i); 137 | k_sqr_distances.push_back(std::sqrt(distance_square)); 138 | } 139 | } 140 | return k_indices.size(); 141 | } 142 | }; // class DBSCANSimpleCluster 143 | 144 | } // namespace SLIM 145 | 146 | #endif // SIMPLE_CLUSTER_H 147 | -------------------------------------------------------------------------------- /modules/dbscan/surface_cluster.h: -------------------------------------------------------------------------------- 1 | #ifndef SURFACE_CLUSTER_H 2 | #define SURFACE_CLUSTER_H 3 | 4 | #include 5 | #include 6 | #include "dbscan/simple_cluster.h" 7 | #include "factor/math_utility.h" 8 | 9 | namespace SLIM { 10 | 11 | #define UN_PROCESSED 0 12 | #define PROCESSING 1 13 | #define PROCESSED 2 14 | 15 | class SurfaceCluster { 16 | public: 17 | typedef typename pcl::PointCloud::Ptr PointCloudPtr; 18 | typedef typename pcl::search::KdTree::Ptr KdTreePtr; 19 | 20 | SurfaceCluster() = default; 21 | 22 | virtual void setInputCloud(PointCloudPtr cloud) { 23 | input_cloud_ = cloud; 24 | } 25 | 26 | void extract(std::vector& cluster_indices) { 27 | std::vector nn_indices; 28 | std::vector nn_distances; 29 | std::vector is_noise(input_cloud_->points.size(), false); 30 | std::vector types(input_cloud_->points.size(), UN_PROCESSED); 31 | for (int i = 0; i < input_cloud_->points.size(); i++) { 32 | if (types[i] == PROCESSED) { 33 | continue; 34 | } 35 | int nn_size = radiusSearch(i, radius_tolerance_, nn_indices, nn_distances); 36 | if (nn_size < min_point_num_) { 37 | is_noise[i] = true; 38 | continue; 39 | } 40 | 41 | std::vector seed_queue; 42 | seed_queue.push_back(i); 43 | types[i] = PROCESSED; 44 | 45 | for (int j = 0; j < nn_size; j++) { 46 | if (nn_indices[j] != i) { 47 | seed_queue.push_back(nn_indices[j]); 48 | types[nn_indices[j]] = PROCESSING; 49 | } 50 | } // for every point near the chosen core point. 51 | int sq_idx = 1; 52 | while (sq_idx < seed_queue.size()) { 53 | int cloud_index = seed_queue[sq_idx]; 54 | if (is_noise[cloud_index] || types[cloud_index] == PROCESSED) { 55 | // seed_queue.push_back(cloud_index); 56 | types[cloud_index] = PROCESSED; 57 | sq_idx++; 58 | continue; // no need to check neighbors. 59 | } 60 | nn_size = radiusSearch(cloud_index, radius_tolerance_, nn_indices, nn_distances); 61 | if (nn_size >= min_point_num_) { 62 | for (int j = 0; j < nn_size; j++) { 63 | if (types[nn_indices[j]] == UN_PROCESSED) { 64 | seed_queue.push_back(nn_indices[j]); 65 | types[nn_indices[j]] = PROCESSING; 66 | } 67 | } 68 | } 69 | 70 | types[cloud_index] = PROCESSED; 71 | sq_idx++; 72 | } 73 | if (seed_queue.size() >= min_pts_per_cluster_ && seed_queue.size () <= max_pts_per_cluster_) { 74 | pcl::PointIndices r; 75 | r.indices.resize(seed_queue.size()); 76 | for (int j = 0; j < seed_queue.size(); ++j) { 77 | r.indices[j] = seed_queue[j]; 78 | } 79 | // These two lines should not be needed: (can anyone confirm?) -FF 80 | std::sort(r.indices.begin(), r.indices.end()); 81 | r.indices.erase(std::unique(r.indices.begin(), r.indices.end()), r.indices.end()); 82 | 83 | r.header = input_cloud_->header; 84 | cluster_indices.push_back(r); // We could avoid a copy by working directly in the vector 85 | } 86 | } // for every point in input cloud 87 | std::sort(cluster_indices.rbegin(), cluster_indices.rend(), comparePointClusters); 88 | } 89 | 90 | void setClusterTolerance(double tolerance) { 91 | radius_tolerance_ = tolerance; 92 | } 93 | 94 | void setAngleTolerance(double tolerance) { 95 | angle_tolerance_ = tolerance; 96 | } 97 | 98 | void setDistTolerance(double tolerance) { 99 | dist_tolerance_ = tolerance; 100 | } 101 | 102 | void setMinClusterSize (int min_cluster_size) { 103 | min_pts_per_cluster_ = min_cluster_size; 104 | } 105 | 106 | void setMaxClusterSize (int max_cluster_size) { 107 | max_pts_per_cluster_ = max_cluster_size; 108 | } 109 | 110 | void setCorePointMinPts(int core_point_min_pts) { 111 | min_point_num_ = core_point_min_pts; 112 | } 113 | 114 | protected: 115 | PointCloudPtr input_cloud_; 116 | 117 | double radius_tolerance_ {0.0}; 118 | double angle_tolerance_{M_PI / 36.0}; 119 | double dist_tolerance_{0.3}; 120 | 121 | int min_point_num_ {1}; // not including the point itself. 122 | int min_pts_per_cluster_ {1}; 123 | int max_pts_per_cluster_ {std::numeric_limits::max()}; 124 | 125 | 126 | virtual int radiusSearch(int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances) const { 127 | k_indices.clear(); 128 | k_sqr_distances.clear(); 129 | k_indices.push_back(index); 130 | k_sqr_distances.push_back(0); 131 | int size = input_cloud_->points.size(); 132 | double radius_sq = radius * radius; 133 | for (int i = 0; i < size; i++) { 134 | if (i == index) { 135 | continue; 136 | } 137 | pcl::PointXYZINormal const& point_i = input_cloud_->points[i]; 138 | pcl::PointXYZINormal const& point_index = input_cloud_->points[index]; 139 | if(std::round(point_i.intensity) != std::round(point_index.intensity)) { 140 | continue; 141 | } 142 | Eigen::Vector3f const centroid_i = point_i.getVector3fMap(); 143 | Eigen::Vector3f const centroid_index = point_index.getVector3fMap(); 144 | double dist_sq = (centroid_i - centroid_index).norm(); 145 | if(dist_sq > radius) { 146 | continue; 147 | } 148 | Eigen::Vector3f const normal_i{point_i.normal_x, point_i.normal_y, point_i.normal_z}; 149 | Eigen::Vector3f const normal_index{point_index.normal_x, point_index.normal_y, point_index.normal_z}; 150 | double theta = std::acos(normal_i.dot(normal_index)); 151 | double dist = std::abs(normal_index.dot(centroid_i - centroid_index)); 152 | 153 | if((theta < angle_tolerance_ || theta > M_PI - angle_tolerance_) && dist < dist_tolerance_) { 154 | k_indices.push_back(i); 155 | k_sqr_distances.push_back(std::sqrt(dist_sq * dist_sq)); 156 | } 157 | } 158 | return k_indices.size(); 159 | } 160 | }; // class SurfaceCluster 161 | } // namespace SLIM 162 | 163 | #endif // SIMPLE_CLUSTER_H -------------------------------------------------------------------------------- /modules/extractor.h: -------------------------------------------------------------------------------- 1 | #ifndef EXTRACTOR_H 2 | #define EXTRACTOR_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #include "semantic_definition.h" 22 | #include "observation.h" 23 | #include "utility.h" 24 | #include "voxel.h" 25 | #include "dbscan/surface_cluster.h" 26 | #include "travel/aos.hpp" 27 | #include "travel/tgs.hpp" 28 | 29 | namespace SLIM 30 | { 31 | struct EIGEN_ALIGN16 VelodynePoint 32 | { 33 | PCL_ADD_POINT4D; 34 | float intensity; 35 | float time; 36 | uint16_t ring; 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | }; 39 | } // namespace SLIM 40 | 41 | POINT_CLOUD_REGISTER_POINT_STRUCT(SLIM::VelodynePoint, 42 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(uint16_t, ring, ring)) 43 | 44 | 45 | struct OusterPoint 46 | { 47 | PCL_ADD_POINT4D; 48 | PCL_ADD_INTENSITY; 49 | uint32_t t; 50 | uint16_t reflectivity; 51 | uint16_t ring; 52 | uint16_t ambient; 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 54 | } EIGEN_ALIGN16; 55 | POINT_CLOUD_REGISTER_POINT_STRUCT (OusterPoint, 56 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) 57 | (uint32_t, t, t) (uint16_t, reflectivity, reflectivity) 58 | (uint16_t, ring, ring) (uint16_t, ambient, ambient) 59 | ) 60 | 61 | 62 | namespace SLIM 63 | { 64 | 65 | enum PointAttribute 66 | { 67 | NONE = 0, 68 | EDGE_LEFT = 1, 69 | EDGE_RIGHT = 2, 70 | EDGE = 3, 71 | SURFACE = 4, 72 | // GROUND = 5, 73 | // BUILDING = 6, 74 | POLE = 7, 75 | // OTHER = 8, 76 | }; 77 | 78 | class ScanPoint 79 | { 80 | public: 81 | ScanPoint() 82 | { 83 | pos.setZero(); 84 | curvature = -1; 85 | range = 1e8; 86 | null = true; 87 | yaw = 0; 88 | attribute = NONE; 89 | } 90 | 91 | ScanPoint(const Eigen::Vector3f &_pos, const float _yaw) 92 | { 93 | pos = _pos; 94 | range = pos.norm(); 95 | yaw = _yaw; 96 | null = false; 97 | } 98 | 99 | void operator=(const ScanPoint &other) 100 | { 101 | curvature = other.curvature; 102 | grad = other.grad; 103 | yaw = other.yaw; 104 | range = other.range; 105 | null = other.null; 106 | pos = other.pos; 107 | attribute = other.attribute; 108 | } 109 | 110 | float curvature = 0; 111 | float grad = 0; 112 | float yaw = 0; 113 | float range = 1e8; 114 | bool null = true; 115 | Eigen::Vector3f pos; 116 | PointAttribute attribute = NONE; 117 | }; 118 | 119 | template 120 | inline int getQuadrant(PointT pt_in) { 121 | int quadrant = 0; 122 | double x = pt_in.x; 123 | double y = pt_in.y; 124 | if(x > 0 && y >= 0) { 125 | quadrant = 1; 126 | } else if(x <= 0 && y > 0) { 127 | quadrant = 2; 128 | } else if(x < 0 && y <= 0) { 129 | quadrant = 3; 130 | } else { 131 | quadrant = 4; 132 | } 133 | return quadrant; 134 | } 135 | 136 | void solveLine(const pcl::PointCloud& cloud, Eigen::Vector3f& mu, Eigen::Vector3f& normal, Eigen::Vector3f& lambda, Eigen::Matrix3f& sigma); 137 | 138 | class VLPExtractor 139 | { 140 | public: 141 | VLPExtractor() = default; 142 | 143 | ~VLPExtractor() = default; 144 | 145 | void setResolution(const int w, const int h); 146 | 147 | void extract(const pcl::PointCloud::Ptr& cloud); 148 | 149 | void extract(const std::vector>& img); 150 | 151 | void extract(const pcl::PointCloud::Ptr cloud); 152 | 153 | void extractPole(); 154 | 155 | void extractSurface(); 156 | 157 | void extractPole(const pcl::PointCloud::Ptr cloud, int scan_line = 64); 158 | 159 | void extractSurface(const pcl::PointCloud::Ptr cloud, int scan_line = 64); 160 | 161 | void refineSurface(const pcl::PointCloud::Ptr cloud, std::vector::Ptr>& clusters, const uint16_t sem_type); 162 | 163 | void constructOb(); 164 | 165 | pcl::PointCloud::Ptr getEdgeCloud(); 166 | 167 | pcl::PointCloud::Ptr getSurfCloud(); 168 | 169 | pcl::PointCloud::Ptr getRawSurfCloud(); 170 | 171 | public: 172 | std::vector> img_; 173 | int w_, h_; 174 | float fov_up_, fov_down_; 175 | const float kNumCurvSize = 2; 176 | 177 | pcl::PointCloud raw_surf_cloud_; 178 | pcl::PointCloud surf_cloud_; 179 | pcl::PointCloud pole_cloud_; 180 | std::vector surf_voxels_; 181 | std::unordered_map ovmap_; 182 | 183 | std::vector> cluster_surf_; 184 | 185 | std::vector vec_line_feature_; 186 | std::vector vec_surf_feature_; 187 | std::vector::Ptr> line_clusters_; 188 | std::vector::Ptr> road_clusters_; 189 | std::vector::Ptr> struct_clusters_; 190 | Transform Twb_; 191 | }; 192 | 193 | } // namespace SLIM 194 | 195 | #endif 196 | -------------------------------------------------------------------------------- /modules/factor/graff_coordinate.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAFF_COORDINATE_H 2 | #define GRAFF_COORDINATE_H 3 | 4 | #include 5 | 6 | namespace SLIM { 7 | class Graff { 8 | public: 9 | Graff() = default; 10 | 11 | Graff(const Eigen::MatrixXd& A, const Eigen::Vector3d& b) 12 | : A_(A), b_(b) {} 13 | 14 | virtual ~Graff() = default; 15 | 16 | Eigen::MatrixXd A_; 17 | Eigen::Vector3d b_; 18 | }; 19 | 20 | class GraffLine { 21 | public: 22 | GraffLine(const Eigen::Vector3d& A, const Eigen::Vector3d& b) { 23 | coord_.resize(4, 2); 24 | coord_.setZero(); 25 | coord_.block<3, 1>(0, 0) = A; 26 | coord_.block<3, 1>(0, 1) = b; 27 | coord_(3, 1) = 1.0; 28 | // coord_.rightCols<1>() /= std::sqrt(1 + b.squaredNorm()); 29 | } 30 | 31 | ~GraffLine() = default; 32 | 33 | Eigen::MatrixXd get() const { 34 | return coord_; 35 | } 36 | private: 37 | Eigen::MatrixXd coord_; 38 | }; 39 | 40 | class GraffSurface { 41 | public: 42 | GraffSurface(const Eigen::Matrix& A, const Eigen::Vector3d& b) { 43 | coord_.resize(4, 3); 44 | coord_.setZero(); 45 | coord_.block<3, 2>(0, 0) = A; 46 | coord_.block<3, 1>(0, 2) = b; 47 | coord_(3, 2) = 1.0; 48 | // coord_.rightCols<1>() /= std::sqrt(1 + b.squaredNorm()); 49 | } 50 | 51 | ~GraffSurface() = default; 52 | 53 | Eigen::MatrixXd get() const { 54 | return coord_; 55 | } 56 | 57 | private: 58 | Eigen::MatrixXd coord_; 59 | }; 60 | } 61 | 62 | #endif -------------------------------------------------------------------------------- /modules/factor/laser_edge_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef LASER_EDGE_FACTOR_H 2 | #define LASER_EDGE_FACTOR_H 3 | 4 | #include 5 | #include 6 | #include "transform.h" 7 | #include "factor/math_utility.h" 8 | #include "factor/line_info.h" 9 | 10 | namespace SLIM { 11 | class LaserPointToLineFactor : public ceres::SizedCostFunction<3, 7> { 12 | public: 13 | LaserPointToLineFactor(const Eigen::Vector3d p, const Eigen::Vector3d& q, const Eigen::Vector3d& n, const double sqrt_info) 14 | : p_(p), q_(q), n_(n), sqrt_info_(sqrt_info) {} 15 | 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | 18 | private: 19 | Eigen::Vector3d p_, q_, n_; 20 | double sqrt_info_; 21 | }; 22 | 23 | class LaserEdgePriorFactor : public ceres::SizedCostFunction<6, 4> { 24 | public: 25 | LaserEdgePriorFactor(const Eigen::Vector3d& pa, const Eigen::Vector3d& pb, const double sqrt_info) 26 | : pa_(pa), pb_(pb), sqrt_info_(sqrt_info) {} 27 | 28 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 29 | 30 | void CheckJacobian(double const *const *parameters); 31 | 32 | private: 33 | Eigen::Vector3d pa_, pb_; 34 | double sqrt_info_; 35 | 36 | }; 37 | 38 | class LaserEdgeFactor : public ceres::SizedCostFunction<2, 7, 4> { 39 | public: 40 | LaserEdgeFactor(const Eigen::Vector3d& local_point, const double& sqrt_info) 41 | : local_point_(local_point), sqrt_info_(Eigen::Matrix2d::Identity() * sqrt_info) {} 42 | 43 | LaserEdgeFactor(const Eigen::Vector3d& local_point, const Eigen::Matrix2d& sqrt_info) 44 | : local_point_(local_point), sqrt_info_(sqrt_info) {} 45 | 46 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 47 | 48 | void CheckJacobian(double const *const *parameters); 49 | 50 | private: 51 | Eigen::Vector3d local_point_; 52 | Eigen::Matrix2d sqrt_info_; 53 | }; 54 | 55 | class LaserEdge2PFactor : public ceres::SizedCostFunction<4, 7, 4> { 56 | public: 57 | LaserEdge2PFactor(const Eigen::Vector3d& pa, const Eigen::Vector3d& pb, const double& sqrt_info) 58 | : pa_(pa), pb_(pb), sqrt_info_(Eigen::Matrix4d::Identity() * sqrt_info) {} 59 | 60 | LaserEdge2PFactor(const Eigen::Vector3d& pa, const Eigen::Vector3d& pb, const Eigen::Matrix4d& sqrt_info) 61 | : pa_(pa), pb_(pb), sqrt_info_(sqrt_info) {} 62 | 63 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 64 | 65 | void CheckJacobian(double const *const *parameters); 66 | 67 | private: 68 | Eigen::Vector3d pa_, pb_; 69 | Eigen::Matrix4d sqrt_info_; 70 | }; 71 | 72 | 73 | class LaserEdgeOnly2PFactor : public ceres::SizedCostFunction<4, 4> { 74 | public: 75 | LaserEdgeOnly2PFactor(const Eigen::Vector3d& pa, const Eigen::Vector3d& pb, const Transform& Twb, const double& sqrt_info) 76 | : pa_(pa), pb_(pb), Twb_(Twb), sqrt_info_(Eigen::Matrix4d::Identity() * sqrt_info) {} 77 | 78 | LaserEdgeOnly2PFactor(const Eigen::Vector3d& pa, const Eigen::Vector3d& pb, const Transform& Twb, const Eigen::Matrix4d& sqrt_info) 79 | : pa_(pa), pb_(pb), Twb_(Twb), sqrt_info_(sqrt_info) {} 80 | 81 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 82 | 83 | private: 84 | Eigen::Vector3d pa_, pb_; 85 | Transform Twb_; 86 | Eigen::Matrix4d sqrt_info_; 87 | }; 88 | 89 | } 90 | 91 | #endif -------------------------------------------------------------------------------- /modules/factor/laser_surf_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef LASER_SURF_FACTOR_H 2 | #define LASER_SURF_FACTOR_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "transform.h" 9 | #include "factor/math_utility.h" 10 | #include "factor/surface_info.h" 11 | 12 | namespace SLIM { 13 | class LaserPointToPointFactor : public ceres::SizedCostFunction<3, 7> { 14 | public: 15 | LaserPointToPointFactor(const Eigen::Vector3d p, const Eigen::Vector3d& q, const double sqrt_info) 16 | : p_(p), q_(q) 17 | { 18 | sqrt_info_ = Eigen::Matrix3d::Identity() * sqrt_info; 19 | } 20 | 21 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 22 | 23 | private: 24 | Eigen::Vector3d p_, q_; 25 | Eigen::Matrix3d sqrt_info_; 26 | }; 27 | 28 | class LaserPointToSurfaceFactor : public ceres::SizedCostFunction<1, 7> { 29 | public: 30 | LaserPointToSurfaceFactor(const Eigen::Vector3d p, const Eigen::Vector3d& q, const Eigen::Vector3d& n, const double sqrt_info) 31 | : p_(p), q_(q), n_(n), sqrt_info_(sqrt_info) {} 32 | 33 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 34 | 35 | private: 36 | Eigen::Vector3d p_, q_, n_; 37 | double sqrt_info_; 38 | }; 39 | 40 | // class LaserPointToSurfaceGtsamFactor : public gtsam::NoiseModelFactor1 { 41 | // public: 42 | // LaserPointToSurfaceGtsamFactor(const gtsam::SharedNoiseModel &model, gtsam::Key X, const Eigen::Vector3d p, const Eigen::Vector3d& q, const Eigen::Vector3d& n) 43 | // : NoiseModelFactor1(model, X), p_(p), q_(q), n_(n) {} 44 | 45 | // gtsam::Vector evaluateError(const gtsam::Pose3 &X, boost::optional H = boost::none) const; 46 | 47 | // /// @return a deep copy of this factor 48 | // virtual gtsam::NonlinearFactor::shared_ptr clone() const { 49 | // return boost::static_pointer_cast( 50 | // gtsam::NonlinearFactor::shared_ptr(new LaserPointToSurfaceGtsamFactor(*this))); 51 | // } 52 | 53 | // private: 54 | // Eigen::Vector3d p_, q_, n_; 55 | // }; 56 | 57 | 58 | class LaserSurfPriorFactor : public ceres::SizedCostFunction<4, 3> { 59 | public: 60 | LaserSurfPriorFactor(const Eigen::Vector3d& pa, const Eigen::Vector3d& pb, 61 | const Eigen::Vector3d& pc, const Eigen::Vector3d& pd, 62 | const double& sqrt_info) 63 | : pa_(pa), pb_(pb), pc_(pc), pd_(pd), sqrt_info_(sqrt_info) {} 64 | 65 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 66 | 67 | void CheckResidual(double const *const *parameters, double *residuals) const; 68 | 69 | void CheckJacobian(double const *const *parameters); 70 | 71 | private: 72 | Eigen::Vector3d pa_, pb_, pc_, pd_; 73 | double sqrt_info_; 74 | }; 75 | 76 | class LaserSurfFactor : public ceres::SizedCostFunction<1, 7, 3> { 77 | public: 78 | LaserSurfFactor(const Eigen::Vector3d& local_point, const double& sqrt_info) 79 | : local_point_(local_point), sqrt_info_(sqrt_info) {} 80 | 81 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 82 | 83 | void CheckResidual(double const *const *parameters, double *residuals) const; 84 | 85 | void CheckJacobian(double const *const *parameters); 86 | 87 | private: 88 | Eigen::Vector3d local_point_; 89 | double sqrt_info_; 90 | }; 91 | 92 | class LaserSurf4PFactor : public ceres::SizedCostFunction<4, 7, 3> { 93 | public: 94 | LaserSurf4PFactor(const std::vector& vertices, const double& sqrt_info) 95 | : vertices_(vertices) { 96 | sqrt_info_ = Eigen::Matrix4d::Identity() * sqrt_info; 97 | } 98 | 99 | LaserSurf4PFactor(const std::vector& vertices, const Eigen::Matrix4d& sqrt_info) 100 | : vertices_(vertices), sqrt_info_(sqrt_info) {} 101 | 102 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 103 | 104 | void CheckJacobian(double const *const *parameters); 105 | 106 | private: 107 | std::vector vertices_; 108 | Eigen::Matrix4d sqrt_info_; 109 | }; 110 | 111 | 112 | class LaserSurf3PFactor : public ceres::SizedCostFunction<3, 7, 3> { 113 | public: 114 | LaserSurf3PFactor(const std::vector& vertices, const double& sqrt_info) 115 | : vertices_(vertices) { 116 | sqrt_info_ = Eigen::Matrix3d::Identity() * sqrt_info; 117 | } 118 | 119 | LaserSurf3PFactor(const std::vector& vertices, const Eigen::Matrix3d& sqrt_info) 120 | : vertices_(vertices), sqrt_info_(sqrt_info) {} 121 | 122 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 123 | 124 | void CheckJacobian(double const *const *parameters); 125 | 126 | private: 127 | std::vector vertices_; 128 | Eigen::Matrix3d sqrt_info_; 129 | }; 130 | 131 | 132 | class LaserSurfOnly3PFactor : public ceres::SizedCostFunction<3, 3> { 133 | public: 134 | LaserSurfOnly3PFactor(const std::vector& vertices, const Transform& Twb, const double& sqrt_info) 135 | : vertices_(vertices), Twb_(Twb) { 136 | sqrt_info_ = Eigen::Matrix3d::Identity() * sqrt_info; 137 | } 138 | 139 | LaserSurfOnly3PFactor(const std::vector& vertices, const Transform& Twb, const Eigen::Matrix3d& sqrt_info) 140 | : vertices_(vertices), Twb_(Twb) , sqrt_info_(sqrt_info) {} 141 | 142 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 143 | 144 | private: 145 | std::vector vertices_; 146 | Transform Twb_; 147 | Eigen::Matrix3d sqrt_info_; 148 | }; 149 | 150 | 151 | // using namespace gtsam; 152 | // class LaserSurfGtsamFactor : public NoiseModelFactor2 { 153 | // public: 154 | // LaserSurfGtsamFactor(const SharedNoiseModel& noise_model, Key key1, Key key2, const std::vector& vertices) 155 | // : NoiseModelFactor2(noise_model, key1, key2), vertices_(vertices) { 156 | 157 | // } 158 | 159 | // virtual Vector evaluateError(const Pose3& pose, const Vector3& lm, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; 160 | 161 | // virtual gtsam::NonlinearFactor::shared_ptr clone() const { 162 | // return boost::static_pointer_cast( 163 | // gtsam::NonlinearFactor::shared_ptr(new LaserSurfGtsamFactor(*this))); 164 | // } 165 | 166 | // private: 167 | // std::vector vertices_; 168 | // }; 169 | 170 | } 171 | 172 | #endif -------------------------------------------------------------------------------- /modules/factor/line_info.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_INFO_H 2 | #define LINE_INFO_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "factor/math_utility.h" 9 | #include "transform.h" 10 | 11 | namespace SLIM { 12 | class LineInfo { 13 | public: 14 | typedef std::shared_ptr Ptr; 15 | 16 | inline LineInfo() : omega_(¶meters_[0]), scale_(¶meters_[2]) {} 17 | 18 | inline LineInfo(const Eigen::Vector4d& vec) 19 | : omega_(¶meters_[0]), scale_(¶meters_[2]) { 20 | parameters_ = vec; 21 | } 22 | 23 | inline LineInfo(const LineInfo& other) 24 | : omega_(¶meters_[0]), scale_(¶meters_[2]), parameters_(other.parameters_) {} 25 | 26 | inline LineInfo(const Eigen::Vector3d& random_point, const Eigen::Vector3d& direction) 27 | : omega_(¶meters_[0]), scale_(¶meters_[2]) { 28 | parameterize(random_point, direction); 29 | } 30 | 31 | inline void parameterize(const Eigen::Vector3d& random_point, const Eigen::Vector3d& direction) { 32 | Eigen::Matrix3d Rzv = g2R(direction), Rvz = Rzv.transpose(); 33 | Eigen::Vector3d ypr = R2ypr(Rzv); 34 | omega_.x() = ypr(2); 35 | omega_.y() = ypr(1); 36 | 37 | Eigen::Vector3d nearest_point = random_point - direction * direction.transpose() * random_point; 38 | Eigen::Vector3d dv = Rzv * nearest_point; 39 | scale_.x() = dv.x(); 40 | scale_.y() = dv.y(); 41 | } 42 | 43 | inline void transform(const Transform& T) { 44 | Eigen::Vector3d const n = get_normal(); 45 | Eigen::Vector3d const c = get_center(); 46 | parameterize(T * c, T.dcm() * n); 47 | } 48 | 49 | inline Eigen::Matrix3d Rvz() const { 50 | double sinr = std::sin(omega_[0]), cosr = std::cos(omega_[0]), sinp = std::sin(omega_[1]), cosp = std::cos(omega_[1]); 51 | Eigen::Matrix3d Rr, Rp; 52 | Rr << 1, 0, 0, 53 | 0, cosr, sinr, 54 | 0, -sinr, cosr; 55 | Rp << cosp, 0, -sinp, 56 | 0, 1, 0, 57 | sinp, 0, cosp; 58 | 59 | return Rr * Rp; 60 | } 61 | 62 | inline Eigen::Vector3d get_center() const { 63 | return Rvz() * (Eigen::Vector3d::UnitX() * scale_[0] + Eigen::Vector3d::UnitY() * scale_[1]); 64 | } 65 | 66 | inline Eigen::Vector3d get_normal() const { 67 | return Rvz() * Eigen::Vector3d::UnitZ(); 68 | } 69 | 70 | inline Eigen::Vector3d distance(const Eigen::Vector3d& q) const { 71 | Eigen::Vector3d const n = get_normal(); 72 | Eigen::Vector3d const c = get_center(); 73 | return (Eigen::Matrix3d::Identity() - n * n.transpose()) * (q - c); 74 | } 75 | 76 | inline Eigen::Vector3d pedal(const Eigen::Vector3d& q) const { 77 | Eigen::Vector3d const n = get_normal(); 78 | Eigen::Vector3d const c = get_center(); 79 | return c + n * n.transpose() * (q - c); 80 | } 81 | 82 | inline Eigen::Matrix jacobian() const { 83 | 84 | Eigen::Matrix jacobian_matrix; 85 | jacobian_matrix.setZero(); 86 | Eigen::Matrix3d R = Rvz(); 87 | Eigen::Vector3d const c = R * (Eigen::Vector3d::UnitX() * scale_[0] + Eigen::Vector3d::UnitY() * scale_[1]); 88 | Eigen::Vector3d const n = R * Eigen::Vector3d::UnitZ(); 89 | double sinr = std::sin(omega_[0]), cosr = std::cos(omega_[0]); 90 | double sinp = std::sin(omega_[1]), cosp = std::cos(omega_[1]); 91 | const double alpha = scale_[0], beta = scale_[1]; 92 | 93 | Eigen::Vector3d scale_w = Eigen::Vector3d::UnitX() * scale_[0] + Eigen::Vector3d::UnitY() * scale_[1]; 94 | 95 | Eigen::Matrix dn_dw; 96 | dn_dw.col(0) << 0, cosr * cosp, -sinr * cosp; 97 | dn_dw.col(1) << -cosp, -sinr * sinp, -cosr * sinp; 98 | 99 | Eigen::Matrix dc_dw; 100 | dc_dw.col(0) << 0, cosr * sinp * alpha - sinr * beta, -sinr * sinp * alpha - cosr * beta; 101 | dc_dw.col(1) << -sinp * alpha, sinr * cosp * alpha, cosr * cosp * alpha; 102 | 103 | Eigen::Matrix dc_ds = R.leftCols<2>(); 104 | 105 | jacobian_matrix.block<3, 2>(0, 0) = dn_dw; 106 | jacobian_matrix.block<3, 2>(3, 0) = dc_dw; 107 | jacobian_matrix.block<3, 2>(3, 2) = dc_ds; 108 | 109 | return jacobian_matrix; 110 | } 111 | 112 | inline Eigen::Matrix& parameters() { 113 | return parameters_; 114 | } 115 | 116 | Eigen::Map omega_; 117 | Eigen::Map scale_; 118 | Eigen::Vector4d parameters_; 119 | }; 120 | } 121 | 122 | #endif -------------------------------------------------------------------------------- /modules/factor/local_param.cpp: -------------------------------------------------------------------------------- 1 | #include "factor/local_param.h" 2 | 3 | namespace SLIM { 4 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const { 5 | Eigen::Map _p(x); 6 | Eigen::Map _q(x + 3); 7 | Eigen::Map dp(delta); 8 | Eigen::Quaterniond dq = deltaQ(Eigen::Map(delta + 3)); 9 | Eigen::Map p(x_plus_delta); 10 | Eigen::Map q(x_plus_delta + 3); 11 | p = _p + dp; 12 | q = (_q * dq).normalized(); 13 | return true; 14 | } 15 | 16 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const { 17 | Eigen::Map> J(jacobian); 18 | J.topRows<6>().setIdentity(); 19 | J.bottomRows<1>().setZero(); 20 | return true; 21 | } 22 | 23 | 24 | bool LineLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const { 25 | Eigen::Map psi(x); 26 | Eigen::Map dpsi(delta); 27 | Eigen::Map psi_plus_dpsi(x_plus_delta); 28 | psi_plus_dpsi = psi + dpsi; 29 | 30 | if(psi_plus_dpsi(0) >= M_PI) { 31 | psi_plus_dpsi(0) = psi_plus_dpsi(0) - 2 * M_PI; 32 | } 33 | else if(psi_plus_dpsi(0) < -M_PI) { 34 | psi_plus_dpsi(0) = psi_plus_dpsi(0) + 2 * M_PI; 35 | } 36 | 37 | if(psi_plus_dpsi(1) >= M_PI) { 38 | psi_plus_dpsi(1) = psi_plus_dpsi(1) - 2 * M_PI; 39 | } 40 | else if(psi_plus_dpsi(1) < -M_PI) { 41 | psi_plus_dpsi(1) = psi_plus_dpsi(1) + 2 * M_PI; 42 | } 43 | return true; 44 | } 45 | 46 | bool LineLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const { 47 | Eigen::Map> J(jacobian); 48 | J.setIdentity(); 49 | return true; 50 | } 51 | 52 | bool SurfaceLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const { 53 | Eigen::Map psi(x); 54 | Eigen::Map dpsi(delta); 55 | Eigen::Map psi_plus_dpsi(x_plus_delta); 56 | psi_plus_dpsi = psi + dpsi; 57 | 58 | if(psi_plus_dpsi(0) >= M_PI) { 59 | psi_plus_dpsi(0) = psi_plus_dpsi(0) - 2 * M_PI; 60 | } 61 | else if(psi_plus_dpsi(0) < -M_PI) { 62 | psi_plus_dpsi(0) = psi_plus_dpsi(0) + 2 * M_PI; 63 | } 64 | 65 | if(psi_plus_dpsi(1) >= M_PI) { 66 | psi_plus_dpsi(1) = psi_plus_dpsi(1) - 2 * M_PI; 67 | } 68 | else if(psi_plus_dpsi(1) < -M_PI) { 69 | psi_plus_dpsi(1) = psi_plus_dpsi(1) + 2 * M_PI; 70 | } 71 | return true; 72 | } 73 | 74 | bool SurfaceLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const { 75 | Eigen::Map> J(jacobian); 76 | J.setIdentity(); 77 | return true; 78 | } 79 | } 80 | 81 | -------------------------------------------------------------------------------- /modules/factor/local_param.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PARAM_H 2 | #define LOCAL_PARAM_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "transform.h" 9 | #include "factor/math_utility.h" 10 | 11 | namespace SLIM { 12 | class PoseLocalParameterization : public ceres::LocalParameterization { 13 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 14 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 15 | virtual int GlobalSize() const { return 7; }; 16 | virtual int LocalSize() const { return 6; }; 17 | }; 18 | 19 | class LineLocalParameterization : public ceres::LocalParameterization { 20 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 21 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 22 | virtual int GlobalSize() const { return 4; }; 23 | virtual int LocalSize() const { return 4; }; 24 | }; 25 | 26 | class SurfaceLocalParameterization : public ceres::LocalParameterization { 27 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 28 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 29 | virtual int GlobalSize() const { return 3; }; 30 | virtual int LocalSize() const { return 3; }; 31 | }; 32 | } 33 | 34 | #endif 35 | 36 | -------------------------------------------------------------------------------- /modules/factor/marginalization_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef MARGINALIZATION_FACTOR_H 2 | #define MARGINALIZATION_FACTOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace SLIM { 10 | const int NUM_THREADS = 4; 11 | 12 | struct ResidualBlockInfo { 13 | ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector _parameter_blocks, std::vector _drop_set) 14 | : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {} 15 | 16 | void Evaluate(); 17 | 18 | ceres::CostFunction *cost_function; 19 | ceres::LossFunction *loss_function; 20 | std::vector parameter_blocks; 21 | std::vector drop_set; 22 | 23 | double **raw_jacobians; 24 | std::vector> jacobians; 25 | Eigen::VectorXd residuals; 26 | 27 | int localSize(int size) { 28 | return size == 7 ? 6 : size; 29 | } 30 | }; 31 | 32 | struct ThreadsStruct { 33 | std::vector sub_factors; 34 | Eigen::MatrixXd A; 35 | Eigen::VectorXd b; 36 | std::unordered_map parameter_block_size; //global size 37 | std::unordered_map parameter_block_idx; //local size 38 | }; 39 | 40 | class MarginalizationInfo { 41 | public: 42 | ~MarginalizationInfo(); 43 | int localSize(int size) const; 44 | int globalSize(int size) const; 45 | void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); 46 | void preMarginalize(); 47 | void marginalize(); 48 | std::vector getParameterBlocks(std::unordered_map &addr_shift); 49 | 50 | std::vector factors; 51 | int m, n; 52 | std::unordered_map parameter_block_size; //global size 53 | int sum_block_size; 54 | std::unordered_map parameter_block_idx; //local size 55 | std::unordered_map parameter_block_data; 56 | 57 | std::vector keep_block_size; //global size 58 | std::vector keep_block_idx; //local size 59 | std::vector keep_block_data; 60 | 61 | Eigen::MatrixXd linearized_jacobians; 62 | Eigen::VectorXd linearized_residuals; 63 | const double eps = 1e-8; 64 | }; 65 | 66 | class MarginalizationFactor : public ceres::CostFunction { 67 | public: 68 | MarginalizationFactor(MarginalizationInfo* _marginalization_info); 69 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 70 | 71 | MarginalizationInfo* marginalization_info; 72 | }; 73 | } 74 | #endif -------------------------------------------------------------------------------- /modules/factor/math_utility.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_UTILITY_H 2 | #define MATH_UTILITY_H 3 | 4 | #include 5 | #include 6 | 7 | namespace SLIM { 8 | 9 | template 10 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) { 11 | Eigen::Matrix ans; 12 | ans << typename Derived::Scalar(0), -q(2), q(1), 13 | q(2), typename Derived::Scalar(0), -q(0), 14 | -q(1), q(0), typename Derived::Scalar(0); 15 | return ans; 16 | } 17 | 18 | template 19 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) { 20 | Eigen::Matrix ans; 21 | ans(0, 0) = q.w(), ans.template block<1, 3>(0, 1) = -q.vec().transpose(); 22 | ans.template block<3, 1>(1, 0) = q.vec(), ans.template block<3, 3>(1, 1) = q.w() * Eigen::Matrix::Identity() + skewSymmetric(q.vec()); 23 | return ans; 24 | } 25 | 26 | template 27 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) { 28 | Eigen::Matrix ans; 29 | ans(0, 0) = p.w(), ans.template block<1, 3>(0, 1) = -p.vec().transpose(); 30 | ans.template block<3, 1>(1, 0) = p.vec(), ans.template block<3, 3>(1, 1) = p.w() * Eigen::Matrix::Identity() - skewSymmetric(p.vec()); 31 | return ans; 32 | } 33 | 34 | template 35 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) { 36 | typedef typename Derived::Scalar Scalar_t; 37 | Eigen::Quaternion dq; 38 | Eigen::Matrix half_theta = theta; 39 | half_theta /= static_cast(2.0); 40 | dq.w() = static_cast(1.0); 41 | dq.x() = half_theta.x(); 42 | dq.y() = half_theta.y(); 43 | dq.z() = half_theta.z(); 44 | return dq; 45 | } 46 | 47 | template 48 | static Eigen::Matrix ExpSO2(const Scalar &theta) { 49 | Eigen::Matrix R; 50 | Scalar cosx = std::cos(theta), sinx = std::sin(theta); 51 | R << cosx, -sinx, sinx, cosx; 52 | return R; 53 | } 54 | 55 | template 56 | static Scalar LogSO2(const Eigen::Matrix &R) { 57 | return std::atan2(R(1, 0), R(0, 0)); 58 | } 59 | 60 | template 61 | static Eigen::Matrix ExpSO3(const Eigen::MatrixBase &theta) { 62 | typedef typename Derived::Scalar Scalar_t; 63 | Scalar_t theta_norm; 64 | Scalar_t theta_sq = theta.squaredNorm(); 65 | Scalar_t imag_factor; 66 | Scalar_t real_factor; 67 | if (theta_sq < 1e-10) { 68 | Scalar_t theta_po4 = theta_sq * theta_sq; 69 | imag_factor = Scalar_t(0.5) - Scalar_t(1.0 / 48.0) * theta_sq + 70 | Scalar_t(1.0 / 3840.0) * theta_po4; 71 | real_factor = Scalar_t(1) - Scalar_t(1.0 / 8.0) * theta_sq + 72 | Scalar_t(1.0 / 384.0) * theta_po4; 73 | } else { 74 | theta_norm = std::sqrt(theta_sq); 75 | Scalar_t half_theta = Scalar_t(0.5) * (theta_norm); 76 | Scalar_t sin_half_theta = std::sin(half_theta); 77 | imag_factor = sin_half_theta / (theta_norm); 78 | real_factor = std::cos(half_theta); 79 | } 80 | Eigen::Quaternion q(real_factor, imag_factor * theta.x(), imag_factor * theta.y(), imag_factor * theta.z()); 81 | return q.toRotationMatrix(); 82 | } 83 | 84 | template 85 | static Eigen::Matrix LogSO3(const Eigen::MatrixBase &R) { 86 | typedef typename Derived::Scalar Scalar_t; 87 | auto q = Eigen::Quaternion(R); 88 | Scalar_t squared_n = q.vec().squaredNorm(); 89 | Scalar_t w = q.w(); 90 | Scalar_t two_atan_nbyw_by_n; 91 | if (squared_n < 1e-10) { 92 | Scalar_t squared_w = w * w; 93 | two_atan_nbyw_by_n = Scalar_t(2) / w - Scalar_t(2.0 / 3.0) * (squared_n) / (w * squared_w); 94 | } else { 95 | Scalar_t n = std::sqrt(squared_n); 96 | Scalar_t atan_nbyw = (w < Scalar_t(0)) ? Scalar_t(std::atan2(-n, -w)) : Scalar_t(std::atan2(n, w)); 97 | two_atan_nbyw_by_n = Scalar_t(2) * atan_nbyw / n; 98 | } 99 | return two_atan_nbyw_by_n * q.vec(); 100 | } 101 | 102 | template 103 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) { 104 | typedef typename Derived::Scalar Scalar_t; 105 | Scalar_t y = ypr(0); 106 | Scalar_t p = ypr(1); 107 | Scalar_t r = ypr(2); 108 | Eigen::Matrix Rz; 109 | Rz << cos(y), -sin(y), 0, 110 | sin(y), cos(y), 0, 111 | 0, 0, 1; 112 | Eigen::Matrix Ry; 113 | Ry << cos(p), 0., sin(p), 114 | 0., 1., 0., 115 | -sin(p), 0., cos(p); 116 | Eigen::Matrix Rx; 117 | Rx << 1., 0., 0., 118 | 0., cos(r), -sin(r), 119 | 0., sin(r), cos(r); 120 | 121 | return Rz * Ry * Rx; 122 | } 123 | 124 | template 125 | static Eigen::Matrix R2ypr(const Eigen::MatrixBase &R) { 126 | typedef typename Derived::Scalar Scalar_t; 127 | Eigen::Matrix n = R.col(0); 128 | Eigen::Matrix o = R.col(1); 129 | Eigen::Matrix a = R.col(2); 130 | 131 | Eigen::Matrix ypr(3); 132 | Scalar_t y = atan2(n(1), n(0)); 133 | Scalar_t p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 134 | Scalar_t r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 135 | ypr(0) = y; 136 | ypr(1) = p; 137 | ypr(2) = r; 138 | 139 | return ypr; 140 | } 141 | 142 | 143 | template 144 | static Eigen::Matrix g2R(const Eigen::MatrixBase &g) { 145 | typedef typename Derived::Scalar Scalar_t; 146 | Eigen::Matrix R0; 147 | Eigen::Matrix ng1 = g.normalized(); 148 | Eigen::Matrix ng2{0, 0, 1.0}; 149 | R0 = Eigen::Quaternion::FromTwoVectors(ng1, ng2).toRotationMatrix(); 150 | Scalar_t yaw = R2ypr(R0).x(); 151 | R0 = ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 152 | return R0; 153 | } 154 | 155 | template 156 | static typename Derived::Scalar graffMetric(const Eigen::MatrixBase &Ya, const Eigen::MatrixBase &Yb) { 157 | typename Derived::Scalar res; 158 | 159 | return res; 160 | } 161 | 162 | // void SolveICP(const std::vector& pts1, const std::vector& pts2, Eigen::Matrix3d& R12, Eigen::Vector3d& t12) { 163 | // assert(pts1.size() == pts2.size()); 164 | // const int n = pts1.size(); 165 | // // step 1. compute center points 166 | // Eigen::Vector3d pm1(0.0, 0.0, 0.0); 167 | // Eigen::Vector3d pm2(0.0, 0.0, 0.0); 168 | 169 | // for(int i = 0; i < n; ++i) { 170 | // pm1 += pts1.at(i); 171 | // pm2 += pts2.at(i); 172 | // } 173 | 174 | // pm1/= (double)n; 175 | // pm2/= (double)n; 176 | 177 | // Eigen::Matrix3d W; 178 | // W.setZero(); 179 | // for(int i = 0; i < n; ++i) { 180 | // W += (pts1.at(i) - pm1) * (pts2.at(i) - pm2).transpose(); 181 | // } 182 | 183 | // Eigen::JacobiSVD svd (W, Eigen::ComputeFullU | Eigen::ComputeFullV); 184 | // Eigen::Matrix3d U = svd.matrixU(); 185 | // Eigen::Matrix3d V = svd.matrixV(); 186 | 187 | // R12 = U * V.transpose(); 188 | 189 | // if(R12.determinant() < 0) { 190 | // R12.block(2, 0, 1, 3) = -R12.block(2, 0, 1, 3); 191 | // } 192 | // // step 3. compute t 193 | // t12 = pm1 - R12 * pm2; 194 | // } 195 | 196 | 197 | } 198 | 199 | #endif -------------------------------------------------------------------------------- /modules/factor/prior_pose_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "factor/prior_pose_factor.h" 2 | 3 | namespace SLIM { 4 | bool PriorPoseFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { 5 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 6 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 7 | 8 | Eigen::Map> residual(residuals); 9 | residual.segment<3>(0) = Pi - prior_pose_.p(); 10 | residual.segment<3>(3) = 2 * (prior_pose_.q().inverse() * Qi).vec(); 11 | residual = sqrt_info_ * residual; 12 | 13 | if(jacobians != nullptr) { 14 | if(jacobians[0] != nullptr) { 15 | Eigen::Map> dr_dTi(jacobians[0]); 16 | dr_dTi.setZero(); 17 | dr_dTi.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 18 | dr_dTi.block<3, 3>(3, 3) = Qleft(prior_pose_.q().inverse() * Qi).bottomRightCorner<3, 3>(); 19 | dr_dTi = sqrt_info_ * dr_dTi; 20 | } 21 | } 22 | return true; 23 | } 24 | } -------------------------------------------------------------------------------- /modules/factor/prior_pose_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef PRIOR_POSE_FACTOR_H 2 | #define PRIOR_POSE_FACTOR_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "transform.h" 9 | #include "factor/math_utility.h" 10 | namespace SLIM { 11 | class PriorPoseFactor : public ceres::SizedCostFunction<6, 7> { 12 | public: 13 | PriorPoseFactor(const Transform& prior_pose, const Eigen::Matrix& sqrt_info) 14 | : prior_pose_(prior_pose), sqrt_info_(sqrt_info) {} 15 | 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | 18 | private: 19 | Transform prior_pose_; 20 | Eigen::Matrix sqrt_info_; 21 | }; 22 | } 23 | #endif -------------------------------------------------------------------------------- /modules/factor/relative_pose_factor.cpp: -------------------------------------------------------------------------------- 1 | #include "factor/relative_pose_factor.h" 2 | 3 | namespace SLIM { 4 | bool RelativePoseFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { 5 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 6 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 7 | Eigen::Matrix3d Ri{Qi.toRotationMatrix()}; 8 | 9 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 10 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 11 | Eigen::Matrix3d Rj{Qj.toRotationMatrix()}; 12 | 13 | Eigen::Vector3d Pij = Ri.transpose() * (Pj - Pi); 14 | Eigen::Quaterniond Qij = Qi.inverse() * Qj; 15 | 16 | Eigen::Map> residual(residuals); 17 | residual.segment<3>(0) = Pij - rel_pose_.p(); 18 | residual.segment<3>(3) = 2 * (rel_pose_.q().inverse() * Qij).vec(); 19 | residual = sqrt_info_ * residual; 20 | 21 | if(jacobians != nullptr) { 22 | if(jacobians[0] != nullptr) { 23 | Eigen::Map> dr_dTi(jacobians[0]); 24 | dr_dTi.setZero(); 25 | Eigen::Matrix3d dt_dti = -Ri.transpose(); 26 | Eigen::Matrix3d dt_dqi = skewSymmetric(Pij); 27 | Eigen::Matrix3d dq_dqi = -(Qright(Qij) * Qleft(rel_pose_.q().inverse())).bottomRightCorner<3, 3>(); 28 | dr_dTi.block<3, 3>(0, 0) = dt_dti; 29 | dr_dTi.block<3, 3>(0, 3) = dt_dqi; 30 | dr_dTi.block<3, 3>(3, 3) = dq_dqi; 31 | dr_dTi = sqrt_info_ * dr_dTi; 32 | } 33 | if(jacobians[1] != nullptr) { 34 | Eigen::Map> dr_dTj(jacobians[1]); 35 | dr_dTj.setZero(); 36 | Eigen::Matrix3d dr_dtj = Ri.transpose(); 37 | Eigen::Matrix3d dr_dqj = Qleft(rel_pose_.q().inverse() * Qij).bottomRightCorner<3, 3>(); 38 | dr_dTj.block<3, 3>(0, 0) = dr_dtj; 39 | dr_dTj.block<3, 3>(3, 3) = dr_dqj; 40 | dr_dTj = sqrt_info_ * dr_dTj; 41 | } 42 | } 43 | return true; 44 | } 45 | } -------------------------------------------------------------------------------- /modules/factor/relative_pose_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef RELATIVE_POSE_FACTOR_H 2 | #define RELATIVE_POSE_FACTOR_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "transform.h" 9 | #include "factor/math_utility.h" 10 | namespace SLIM { 11 | class RelativePoseFactor : public ceres::SizedCostFunction<6, 7, 7> { 12 | public: 13 | RelativePoseFactor(const Transform& rel_pose, const Eigen::Matrix& sqrt_info) 14 | : rel_pose_(rel_pose), sqrt_info_(sqrt_info) {} 15 | 16 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 17 | 18 | private: 19 | Transform rel_pose_; 20 | Eigen::Matrix sqrt_info_; 21 | }; 22 | } 23 | 24 | #endif -------------------------------------------------------------------------------- /modules/factor/surface_info.h: -------------------------------------------------------------------------------- 1 | #ifndef SURFACE_INFO_H 2 | #define SURFACE_INFO_H 3 | 4 | #include 5 | 6 | #include "factor/math_utility.h" 7 | namespace SLIM { 8 | class SurfaceInfo { 9 | public: 10 | typedef std::shared_ptr Ptr; 11 | inline SurfaceInfo() : omega_(¶meters_[0]), dist_(¶meters_[2]) {} 12 | 13 | inline SurfaceInfo(const Eigen::Vector3d& vec) 14 | : omega_(¶meters_[0]), dist_(¶meters_[2]) { 15 | omega_ = vec.head<2>(); 16 | dist_ = vec.tail<1>(); 17 | } 18 | 19 | inline SurfaceInfo(const Eigen::Vector4d& vec) 20 | : omega_(¶meters_[0]), dist_(¶meters_[2]) { 21 | parameterize(vec.head<3>(), vec(3)); 22 | } 23 | 24 | inline SurfaceInfo(const SurfaceInfo& other) 25 | : parameters_(other.parameters_), omega_(¶meters_[0]), dist_(¶meters_[2]) {} 26 | 27 | inline SurfaceInfo(const Eigen::Vector3d& random_point, const Eigen::Vector3d& direction) 28 | : omega_(¶meters_[0]), dist_(¶meters_[2]) { 29 | parameterize(direction, -direction.transpose() * random_point); 30 | } 31 | 32 | inline void parameterize(const Eigen::Vector3d& direction, const double dist) { 33 | Eigen::Matrix3d Rzv = g2R(direction), Rvz = Rzv.transpose(); 34 | Eigen::Vector3d ypr = R2ypr(Rzv); 35 | omega_.x() = ypr(2); 36 | omega_.y() = ypr(1); 37 | dist_(0) = dist; 38 | } 39 | 40 | inline void transform(const Transform& T) { 41 | Eigen::Vector3d n = get_normal(); 42 | Eigen::Vector3d c = pedal(Eigen::Vector3d::Zero()); 43 | Eigen::Vector3d nt = T.dcm() * n; 44 | Eigen::Vector3d ct = T * c; 45 | parameterize(nt, -nt.transpose() * ct); 46 | } 47 | 48 | inline Eigen::Matrix3d Rvz() const { 49 | double sinr = std::sin(omega_[0]), cosr = std::cos(omega_[0]), sinp = std::sin(omega_[1]), cosp = std::cos(omega_[1]); 50 | Eigen::Matrix3d Rr, Rp; 51 | Rr << 1, 0, 0, 52 | 0, cosr, sinr, 53 | 0, -sinr, cosr; 54 | Rp << cosp, 0, -sinp, 55 | 0, 1, 0, 56 | sinp, 0, cosp; 57 | 58 | return Rr * Rp; 59 | } 60 | 61 | inline double get_dist() const { 62 | return dist_(0); 63 | } 64 | 65 | inline Eigen::Vector3d get_normal() const { 66 | return Rvz() * Eigen::Vector3d::UnitZ(); 67 | } 68 | 69 | inline double distance(const Eigen::Vector3d& q) const { 70 | return get_normal().dot(q) + dist_(0); 71 | } 72 | 73 | inline Eigen::Vector3d pedal(const Eigen::Vector3d& q) const { 74 | auto n = get_normal(); 75 | auto d = n.dot(q) + dist_(0); 76 | return q - n * d; 77 | } 78 | 79 | inline Eigen::Matrix subspace() const { 80 | Eigen::Vector3d z = get_normal(); 81 | Eigen::Vector3d x = Eigen::Vector3d::Random().normalized(); 82 | Eigen::Vector3d y = z.cross(x); 83 | Eigen::Vector3d xn, yn, zn; 84 | zn = z; 85 | yn = y - (y.dot(zn)) / (zn.dot(zn)) * zn; 86 | xn = x - (x.dot(zn)) / (zn.dot(zn)) * zn - (x.dot(yn)) / (yn.dot(yn)) * yn; 87 | Eigen::Matrix subspace; 88 | subspace.col(0) = xn.normalized(); 89 | subspace.col(1) = yn.normalized(); 90 | assert(z.dot(subspace.col(0)) < 1e-8); 91 | assert(z.dot(subspace.col(1)) < 1e-8); 92 | return subspace; 93 | } 94 | 95 | inline Eigen::Matrix jacobian() const { 96 | 97 | Eigen::Matrix jacobian_matrix; 98 | jacobian_matrix.setZero(); 99 | Eigen::Matrix3d R = Rvz(); 100 | auto n = R * Eigen::Vector3d::UnitZ(); 101 | double sinr = std::sin(omega_[0]), cosr = std::cos(omega_[0]); 102 | double sinp = std::sin(omega_[1]), cosp = std::cos(omega_[1]); 103 | double d = dist_(0); 104 | 105 | Eigen::Matrix dn_dw; 106 | dn_dw.col(0) << 0, cosr * cosp, -sinr * cosp; 107 | dn_dw.col(1) << -cosp, -sinr * sinp, -cosr * sinp; 108 | 109 | jacobian_matrix.block<3, 2>(0, 0) = dn_dw; 110 | jacobian_matrix(3, 2) = 1.0; 111 | 112 | return jacobian_matrix; 113 | } 114 | 115 | inline Eigen::Matrix& parameters() { 116 | return parameters_; 117 | } 118 | 119 | Eigen::Map omega_; 120 | Eigen::Map> dist_; 121 | Eigen::Vector3d parameters_; 122 | }; 123 | } 124 | 125 | #endif -------------------------------------------------------------------------------- /modules/frame.cpp: -------------------------------------------------------------------------------- 1 | #include "frame.h" 2 | 3 | namespace SLIM { 4 | 5 | uint32_t Frame::factory_id_ = 0; 6 | 7 | Frame::Frame(const uint64_t timestamp, const Transform& Twb) 8 | : id_(factory_id_++), timestamp_(timestamp), Twb_(Twb), Tbl_(Transform()) {} 9 | 10 | Frame::Frame(const uint32_t id, const uint64_t timestamp, const Transform& Twb) 11 | : id_(id), timestamp_(timestamp), Twb_(Twb), Tbl_(Transform()) {} 12 | 13 | Frame::Frame(const uint32_t id, const uint64_t timestamp, const Transform& Twb, 14 | const std::vector& line_obs, 15 | const std::vector& surf_obs) 16 | : id_(id), timestamp_(timestamp), Twb_(Twb), Tbl_(Transform()), line_obs_(line_obs), surface_obs_(surf_obs) {} 17 | 18 | 19 | void Frame::DownSample(pcl::PointCloud::Ptr &cloud, const float resolution) { 20 | pcl::VoxelGrid vf; 21 | vf.setLeafSize(resolution, resolution, resolution); 22 | vf.setInputCloud(cloud); 23 | vf.filter(*cloud); 24 | } 25 | 26 | } // namespace SLIM 27 | -------------------------------------------------------------------------------- /modules/frame.h: -------------------------------------------------------------------------------- 1 | #ifndef FRAME_H 2 | #define FRAME_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include "observation.h" 19 | #include "transform.h" 20 | #include "voxel.h" 21 | #include "utility.h" 22 | #include "dbscan/kdtree_cluster.h" 23 | #include "dbscan/surface_cluster.h" 24 | 25 | namespace SLIM { 26 | 27 | class Frame { 28 | public: 29 | typedef std::shared_ptr Ptr; 30 | Frame() = default; 31 | 32 | Frame(const uint64_t timestamp, const Transform& Twb); 33 | 34 | Frame(const uint32_t id, const uint64_t timestamp, const Transform& Twb); 35 | 36 | Frame(const uint32_t id, const uint64_t timestamp, const Transform& Twb, 37 | const std::vector& line_obs, 38 | const std::vector& surf_obs); 39 | 40 | ~Frame() = default; 41 | 42 | void DownSample(pcl::PointCloud::Ptr &cloud, const float resolution); 43 | 44 | void setCloud(pcl::PointCloud::Ptr cloud) { 45 | cloud_ = cloud; 46 | } 47 | 48 | void SetSemanticColorCloud(pcl::PointCloud::Ptr cloud) { 49 | sem_color_cloud_ = cloud; 50 | } 51 | 52 | pcl::PointCloud::Ptr getGlobalCloud() { 53 | std::lock_guard lock(data_mutex_); 54 | pcl::PointCloud::Ptr global_cloud(new pcl::PointCloud()); 55 | Eigen::Matrix4f Tf = Twb_.matrix().cast(); 56 | for(int i = 0; i < cloud_->size(); ++i) { 57 | pcl::PointXYZI point(cloud_->points[i]); 58 | point.getVector4fMap() = Tf * cloud_->points[i].getVector4fMap(); 59 | global_cloud->push_back(point); 60 | } 61 | return global_cloud; 62 | } 63 | 64 | void setTwb(const Transform& Twb) { 65 | std::lock_guard lock(data_mutex_); 66 | Twb_ = Twb; 67 | } 68 | 69 | void setID(const uint32_t id) { 70 | std::lock_guard lock(data_mutex_); 71 | id_ = id; 72 | } 73 | 74 | void setTime(const uint64_t time) { 75 | std::lock_guard lock(data_mutex_); 76 | timestamp_ = time; 77 | } 78 | 79 | uint32_t id() const { 80 | return id_; 81 | } 82 | 83 | uint64_t timestamp() const { 84 | return timestamp_; 85 | } 86 | 87 | Transform& Twb() { 88 | std::lock_guard lock(data_mutex_); 89 | return Twb_; 90 | } 91 | 92 | Transform const constTwb() const { 93 | return Twb_; 94 | } 95 | 96 | Transform Twl() const { 97 | return Twb_ * Tbl_; 98 | } 99 | 100 | std::vector line_obs() const { 101 | return line_obs_; 102 | } 103 | 104 | std::vector surface_obs() const { 105 | return surface_obs_; 106 | } 107 | 108 | pcl::PointCloud::Ptr sem_color_cloud() { 109 | std::unique_lock lock(data_mutex_); 110 | return sem_color_cloud_; 111 | } 112 | 113 | public: 114 | static uint32_t factory_id_; 115 | uint32_t id_; 116 | uint64_t timestamp_; 117 | Transform Twb_; 118 | Transform gt_Twb_; 119 | Transform Tbl_; 120 | std::vector line_obs_; 121 | std::vector surface_obs_; 122 | 123 | pcl::PointCloud::Ptr cloud_; 124 | pcl::PointCloud::Ptr sem_color_cloud_; 125 | std::string cloud_path_; 126 | 127 | std::mutex data_mutex_; 128 | }; 129 | } // namespace SLIM 130 | 131 | #endif -------------------------------------------------------------------------------- /modules/locator.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCATOR_H 2 | #define LOCATOR_H 3 | 4 | #include "vector_map.h" 5 | #include "frame.h" 6 | #include "scene_graph.h" 7 | #include "viewer.h" 8 | #include "clipper.h" 9 | 10 | namespace SLIM { 11 | 12 | 13 | class Locator { 14 | public: 15 | Locator() = default; 16 | 17 | void setMap(const VectorMap::Ptr& map); 18 | 19 | void setViewer(const Viewer::Ptr& viewer); 20 | 21 | void setTrajFileName(const std::string& traj_file_name); 22 | 23 | bool solveRelativePose(const SceneGraph::Ptr& gref, const SceneGraph::Ptr& gcur, 24 | const clipper::Association& matches, Transform& Trc); 25 | 26 | bool refineRelativePose(const Block::Ptr& block, const pcl::PointCloud::Ptr& sem_cloud, Transform& Tij); 27 | 28 | bool solve(uint64_t timestamp, const pcl::PointCloud::Ptr& cloud); 29 | 30 | void saveTumTrajectory(const std::string &filename); 31 | 32 | bool solve( 33 | uint64_t timestamp, 34 | const pcl::PointCloud::Ptr& pole, 35 | const pcl::PointCloud::Ptr& road, 36 | const pcl::PointCloud::Ptr& building 37 | ); 38 | 39 | // private: 40 | VectorMap::Ptr map_; 41 | Viewer::Ptr viewer_; 42 | std::vector blocks_; 43 | std::vector graphs_; 44 | 45 | std::vector> vec_reloc_info_; 46 | double cur_line_ratio_; 47 | double cur_surf_ratio_; 48 | 49 | Transform cur_Twb_; 50 | Transform last_Twb_; 51 | Transform delta_Twb_; 52 | double last_time_; 53 | bool initialized_ = false; 54 | 55 | std::string traj_file_name_; 56 | std::map trajectory_; 57 | 58 | }; 59 | 60 | } // namespace SLIM 61 | 62 | #endif -------------------------------------------------------------------------------- /modules/multi_map_merger.h: -------------------------------------------------------------------------------- 1 | #ifndef MULTI_MAP_MERGER_H 2 | #define MULTI_MAP_MERGER_H 3 | 4 | #include "vector_map.h" 5 | #include "scene_graph.h" 6 | #include "viewer.h" 7 | #include "pcm_solver.h" 8 | #include "clipper.h" 9 | #include "utility.h" 10 | #include "nfr_solver.h" 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace SLIM { 18 | 19 | 20 | 21 | using Graph = std::unordered_map>; 22 | 23 | std::unordered_set findConnectedComponent(const Graph& graph, int startNode); 24 | 25 | std::vector findConnectedComponents(const Graph& graph); 26 | 27 | 28 | struct ExpInfo { 29 | double nfr_time; 30 | double optimize_time; 31 | double map_size; // MB 32 | uint32_t frame_size, line_size, surf_size; 33 | 34 | }; 35 | 36 | struct LoopInfo { 37 | VectorMap::Ptr mi, mj; 38 | RelPoseInfo rel_pose_info; 39 | LoopInfo(const VectorMap::Ptr _mi, const VectorMap::Ptr _mj, const RelPoseInfo& _rel_pose_info): mi(_mi), mj(_mj), rel_pose_info(_rel_pose_info) {} 40 | }; 41 | 42 | struct MapNode { 43 | typedef std::shared_ptr Ptr; 44 | int id; 45 | std::vector neighbors; 46 | VectorMap::Ptr map; 47 | bool visited; 48 | MapNode(int _id) : id(_id), map(nullptr), visited(false) {} 49 | MapNode(int _id, VectorMap::Ptr _map) : id(_id), map(_map), visited(false) {} 50 | }; 51 | 52 | // DFS遍历 53 | void DFS(MapNode::Ptr node, std::unordered_set& connectedSet); 54 | 55 | std::vector findMaxConnectedGraph(std::vector& graph); 56 | 57 | class MultiMapMerger { 58 | public: 59 | MultiMapMerger() { 60 | 61 | } 62 | 63 | void setViewer(const Viewer::Ptr viewer) { 64 | viewer_ = viewer; 65 | } 66 | 67 | void addSubMap(const VectorMap::Ptr submap) { 68 | map_cache_.push_back(submap); 69 | } 70 | 71 | void findLoopMS(); 72 | 73 | Block::Ptr initBlock(const Frame::Ptr frame, const VectorMap::Ptr map, const float range = 40.0f); 74 | 75 | bool findPairLoop(const VectorMap::Ptr& mi, const VectorMap::Ptr& mj, 76 | const std::vector& blocks0, const std::vector& blocks1, 77 | const std::vector& graphs0, const std::vector& graphs1, std::vector& ov_infos); 78 | 79 | void selectLoop(std::vector& loop_info, std::vector& ov_info); 80 | 81 | void alignMultiMap(); 82 | 83 | void setRefMap(const VectorMap::Ptr ref_map) { 84 | ref_map_ = ref_map; 85 | // std::map trajectory; 86 | // for(auto &f_iter: ref_map_->getKeyFrames()) { 87 | // Transform& Twb = f_iter.second->Twb(); 88 | // trajectory.insert(std::make_pair(f_iter.second->id(), f_iter.second)); 89 | // } 90 | // std::vector odom_info; 91 | // for(auto iter = trajectory.begin(); iter != std::prev(trajectory.end()); ++iter) { 92 | // Frame::Ptr const fi = iter->second; 93 | // Frame::Ptr const fj = std::next(iter)->second; 94 | // Transform const Tij = fi->Twb().inverse() * fj->Twb(); 95 | // odom_info.push_back(RelPoseInfo(fi, fj, Tij)); 96 | // } 97 | // vec_odom_info_.push_back(odom_info); 98 | // merge_num_++; 99 | 100 | // saveTumTrajectory(output_path_ + "/merge_num_" + std::to_string(merge_num_) + "_ori.txt"); 101 | } 102 | 103 | void setCurMap(const VectorMap::Ptr cur_map) { 104 | cur_map_ = cur_map; 105 | // std::map trajectory; 106 | // for(auto &f_iter: cur_map_->getKeyFrames()) { 107 | // Transform& Twb = f_iter.second->Twb(); 108 | // trajectory.insert(std::make_pair(f_iter.second->id(), f_iter.second)); 109 | // } 110 | // cur_odom_info_.clear(); 111 | // for(auto iter = trajectory.begin(); iter != std::prev(trajectory.end()); ++iter) { 112 | // Frame::Ptr const fi = iter->second; 113 | // Frame::Ptr const fj = std::next(iter)->second; 114 | // Transform const Tij = fi->Twb().inverse() * fj->Twb(); 115 | // cur_odom_info_.push_back(RelPoseInfo(fi, fj, Tij)); 116 | // } 117 | // merge_num_++; 118 | } 119 | 120 | void setOutputPath(const std::string& output_path) { 121 | output_path_ = output_path; 122 | createDirectory(output_path_ + "/pgo"); 123 | createDirectory(output_path_ + "/ba"); 124 | createDirectory(output_path_ + "/submap"); 125 | createDirectory(output_path_ + "/summary"); 126 | } 127 | 128 | bool solveRelativePose(const SceneGraph::Ptr& gref, const SceneGraph::Ptr& gcur, 129 | const clipper::Association& matches, Transform& Trc); 130 | 131 | bool refineRelativePose(const Block::Ptr& block_i, const Block::Ptr& block_j, Transform& Tij, Eigen::Matrix& sqrt_info); 132 | 133 | bool refineRelativePoseGNC(const Block::Ptr& block_i, const Block::Ptr& block_j, Transform& Tij); 134 | 135 | std::vector solvePCM(const std::vector& info); 136 | 137 | std::vector searchPotentialOverlap(const std::vector& ref_blocks, 138 | const std::vector& cur_blocks, 139 | const std::set& ref_set, 140 | const std::set& cur_set, 141 | const Transform& Trc); 142 | 143 | void optimizePGO(const VectorMap::Ptr& ref_map, const VectorMap::Ptr& cur_map, const std::vector& overlap); 144 | 145 | void optimizePGO(const std::vector& overlap); 146 | 147 | void optimizeGncPGO(const std::vector& overlap); 148 | 149 | void combineMap(const VectorMap::Ptr& map); 150 | 151 | // void combineMap(VectorMap::Ptr& ref_map, const VectorMap::Ptr& cur_map); 152 | 153 | void combineMap(); 154 | 155 | void optimizeGBA(); 156 | 157 | bool merge(); 158 | 159 | void saveTumTrajectory(const std::string& filename); 160 | 161 | Transform solveAveragePose(const std::vector& ov_info); 162 | 163 | VectorMap::Ptr getRefMap() { 164 | return ref_map_; 165 | } 166 | 167 | VectorMap::Ptr getCurMap() { 168 | return cur_map_; 169 | } 170 | 171 | public: 172 | VectorMap::Ptr ref_map_, cur_map_; 173 | VectorMap::Ptr base_map_; 174 | Viewer::Ptr viewer_; 175 | 176 | std::vector map_cache_; 177 | std::vector map_queue_; 178 | 179 | std::vector ov_info_; 180 | std::vector>> vec_rel_pose_; 181 | std::string output_path_; 182 | 183 | std::vector> vec_odom_info_; 184 | std::vector> vec_loop_info_; 185 | std::vector cur_odom_info_; 186 | std::vector cur_loop_info_; 187 | 188 | std::vector loop_info_; 189 | std::map>> loop_info_map_; 190 | std::map>> ov_info_map_; 191 | std::unordered_map block_map_; 192 | std::unordered_map graph_map_; 193 | 194 | std::vector> block_vec_; 195 | std::vector> graph_vec_; 196 | std::map exp_info_; 197 | 198 | uint32_t merge_num_ = 0; 199 | bool use_ba_; 200 | }; 201 | 202 | } // namespace SLIM 203 | 204 | 205 | 206 | #endif -------------------------------------------------------------------------------- /modules/observation.h: -------------------------------------------------------------------------------- 1 | #ifndef OBSERVATION_H 2 | #define OBSERVATION_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "transform.h" 15 | #include "semantic_definition.h" 16 | 17 | namespace SLIM { 18 | 19 | // pole like feature 20 | class LineOB { 21 | public: 22 | typedef std::shared_ptr Ptr; 23 | LineOB() = default; 24 | 25 | LineOB(const uint16_t semantic_type) 26 | : semantic_type_(semantic_type) {} 27 | 28 | LineOB(const uint16_t semantic_type, const Eigen::Vector3d& pa, const Eigen::Vector3d& pb) 29 | : semantic_type_(semantic_type), point_a_(pa), point_b_(pb), is_active_(true) { 30 | 31 | } 32 | 33 | LineOB(const uint16_t semantic_type, const Eigen::Vector3d& sum, const Eigen::Matrix3d& squared_sum, const uint32_t point_num); 34 | 35 | LineOB(const uint16_t semantic_type, const pcl::PointCloud::Ptr& cloud); 36 | 37 | LineOB(const uint16_t semantic_type, const Eigen::Vector3d& point_a, const Eigen::Vector3d& point_b, const uint32_t point_num); 38 | 39 | bool Init(const pcl::PointCloud::Ptr& cloud); 40 | 41 | void setSqrtInfo(const Eigen::Matrix4d& sqrt_info) { 42 | sqrt_info_ = sqrt_info; 43 | } 44 | 45 | void SetActive() { 46 | is_active_ = true; 47 | } 48 | 49 | void SetInactive() { 50 | is_active_ = false; 51 | } 52 | 53 | // double lambda() const { 54 | // return lambda_(2); 55 | // } 56 | 57 | Eigen::Vector3d centroid() const { 58 | return 0.5 * (point_a_ + point_b_); 59 | } 60 | 61 | Eigen::Vector3d normal() const { 62 | return (point_a_ - point_b_).normalized(); 63 | } 64 | 65 | Eigen::Vector3d point_a() const { 66 | return point_a_; 67 | } 68 | 69 | Eigen::Vector3d point_b() const { 70 | return point_b_; 71 | } 72 | 73 | // Eigen::Matrix3d sigma() const { 74 | // return sigma_; 75 | // } 76 | 77 | uint16_t semantic_type() const { 78 | return semantic_type_; 79 | } 80 | 81 | const Eigen::Matrix4d sqrt_info() const { 82 | return sqrt_info_; 83 | } 84 | 85 | public: 86 | Eigen::Vector3d point_a_, point_b_; 87 | Eigen::Vector3d sum_; 88 | Eigen::Matrix3d squared_sum_; 89 | Eigen::Matrix3d sigma_; 90 | Eigen::Vector3d lambda_; 91 | Eigen::Matrix4d sqrt_info_; 92 | pcl::PointCloud::Ptr cloud_; 93 | uint32_t point_num_; 94 | uint16_t semantic_type_; 95 | bool is_active_ = false; 96 | std::mutex data_mutex_; 97 | }; 98 | 99 | // road, sidewalk, wall, etc. 100 | class SurfaceOB { 101 | public: 102 | typedef std::shared_ptr Ptr; 103 | SurfaceOB() = default; 104 | 105 | SurfaceOB(const uint16_t semantic_type) 106 | : semantic_type_(semantic_type) {} 107 | 108 | SurfaceOB(const uint16_t semantic_type, const Eigen::Vector3d& center, 109 | const Eigen::Vector3d& normal, const std::vector& vertices, 110 | const float ra, const float rb); 111 | 112 | SurfaceOB(const uint16_t semantic_type, const Eigen::Vector3d& sum, const Eigen::Matrix3d& squared_sum, const uint32_t point_num); 113 | 114 | SurfaceOB(const uint16_t semantic_type, const std::vector& vertices, const uint32_t point_num); 115 | 116 | SurfaceOB(const uint16_t semantic_type, const pcl::PointCloud::Ptr cloud); 117 | 118 | // SurfaceOB(const uint16_t semantic_type, const Eigen::Vector3d& center, const Eigen::Vector3d& normal, const float ra, const float rb); 119 | // bool Init(const pcl::PointCloud::Ptr& cloud) { 120 | // int N = cloud->size(); 121 | // sigma_.setZero(); 122 | // center_.setZero(); 123 | // for(int i = 0; i < N; ++i) { 124 | // Eigen::Vector3d point = cloud->points[i].getVector3fMap().cast(); 125 | // center_ += point; 126 | // sigma_ += point * point.transpose(); 127 | // } 128 | // center_ /= N; 129 | // sigma_.noalias() = sigma_ / N - center_ * center_.transpose(); 130 | 131 | // Eigen::SelfAdjointEigenSolver saes(sigma_); 132 | // lambda_ = saes.eigenvalues(); 133 | 134 | // if(lambda_(0) > 0.04 * lambda_(1) || lambda_(1) < 0.2 * lambda_(2)) { 135 | // return false; 136 | // } 137 | 138 | // umat_ = saes.eigenvectors(); 139 | // normal_ = umat_.col(0); 140 | // return true; 141 | // } 142 | 143 | void insertParticle(const Eigen::Vector3d& particle) { 144 | particles.push_back(particle); 145 | } 146 | 147 | void setSqrtInfo(const Eigen::Matrix3d& sqrt_info) { 148 | sqrt_info_ = sqrt_info; 149 | } 150 | 151 | // std::vector vertices() const { 152 | // // std::unique_lock lock(data_mutex_); 153 | // std::vector vertices(4); 154 | // vertices[0] = center_ + umat_.col(1) * std::sqrt(lambda_(1) * 2); 155 | // vertices[1] = center_ + umat_.col(2) * std::sqrt(lambda_(2) * 2); 156 | // vertices[2] = center_ - umat_.col(1) * std::sqrt(lambda_(1) * 2); 157 | // vertices[3] = center_ - umat_.col(2) * std::sqrt(lambda_(2) * 2); 158 | // // vertices[0] = center_ + umat_.col(1) * (lambda_(1)); 159 | // // vertices[1] = center_ + umat_.col(2) * (lambda_(2)); 160 | // // vertices[2] = center_ - umat_.col(1) * (lambda_(1)); 161 | // // vertices[3] = center_ - umat_.col(2) * (lambda_(2)); 162 | // return vertices; 163 | // } 164 | 165 | std::vector vertices() const { 166 | // std::unique_lock lock(data_mutex_); 167 | return vertices_; 168 | } 169 | 170 | uint16_t semantic_type() const { 171 | // std::unique_lock lock(data_mutex_); 172 | return semantic_type_; 173 | } 174 | 175 | void SetActive() { 176 | is_active_ = true; 177 | } 178 | 179 | void SetInactive() { 180 | is_active_ = false; 181 | } 182 | 183 | // double lambda() const { 184 | // return lambda_(0); 185 | // } 186 | 187 | float ra() const { 188 | return ra_; 189 | } 190 | 191 | float rb() const { 192 | return rb_; 193 | } 194 | 195 | Eigen::Vector3d centroid() const { 196 | return center_; 197 | } 198 | 199 | Eigen::Vector3d normal() const { 200 | return normal_; 201 | } 202 | 203 | // Eigen::Matrix3d sigma() const { 204 | // return sigma_; 205 | // } 206 | 207 | const Eigen::Matrix3d sqrt_info() const { 208 | return sqrt_info_; 209 | } 210 | 211 | public: 212 | Eigen::Vector3d center_, normal_; 213 | Eigen::Vector3d sum_; 214 | Eigen::Matrix3d squared_sum_; 215 | // Eigen::Matrix3d sigma_; 216 | // Eigen::Vector3d lambda_; 217 | // Eigen::Matrix3d umat_; 218 | Eigen::Matrix3d sqrt_info_; 219 | std::vector particles; 220 | std::vector vertices_; 221 | pcl::PointCloud::Ptr cloud_; 222 | uint32_t point_num_; 223 | uint16_t semantic_type_; 224 | float ra_, rb_; 225 | bool is_active_ = false; 226 | std::mutex data_mutex_; 227 | }; 228 | 229 | } // namespace SLIM 230 | 231 | 232 | #endif -------------------------------------------------------------------------------- /modules/scene_graph.cpp: -------------------------------------------------------------------------------- 1 | #include "scene_graph.h" 2 | 3 | namespace SLIM { 4 | 5 | void SceneGraph::transform(const Transform& Tsd) { 6 | for(auto node: nodes_) { 7 | node->transform(Tsd); 8 | } 9 | } 10 | 11 | void SceneGraph::buildFromBlock(const Block::Ptr block) { 12 | int graph_node_id = 0; 13 | int line_node_num = 0, surf_node_num = 0; 14 | block_ = block; 15 | Eigen::Vector3d centroid = block->getHostFrame()->Twb().p(); 16 | nodes_.clear(); 17 | // std::vector dists; 18 | std::vector line_nodes; 19 | for(auto &cls_iter: block->getSemLines()) { 20 | if (line_node_num > 3) 21 | continue; 22 | for(auto &lm_iter: cls_iter.second) { 23 | Block::Node::Ptr nd = lm_iter.second; 24 | double dist = (nd->centroid - centroid).norm(); 25 | SceneGraphNode::Ptr node = SceneGraphNode::Ptr( 26 | new SceneGraphNode(graph_node_id++, nd->sem_type, nd->centroid, nd->normal) 27 | ); 28 | node->AddBlockNode(nd); 29 | node->dist_ = dist; 30 | line_nodes.push_back(node); 31 | line_node_num++; 32 | } 33 | } 34 | std::sort(line_nodes.begin(), line_nodes.end(), [&](const SceneGraphNode::Ptr a, const SceneGraphNode::Ptr b){ 35 | return (a->dist_ < b->dist_); 36 | }); 37 | 38 | if(line_nodes.size() > 20) { 39 | nodes_ = std::vector{line_nodes.begin(), line_nodes.begin() + 20}; 40 | } 41 | else { 42 | nodes_ = line_nodes; 43 | } 44 | 45 | surf_vertices_.reset(new pcl::PointCloud()); 46 | 47 | std::vector surf_block_nodes; 48 | for(auto &cls_iter: block->getSemSurfaces()) { 49 | for(auto &lm_iter: cls_iter.second) { 50 | Block::Node::Ptr nd = lm_iter.second; 51 | // if(nd->sem_type != FENCE_ID && nd->sem_type != BUILDING_ID) 52 | // continue; 53 | Eigen::Vector3d const centroid = nd->centroid; 54 | Eigen::Vector3d const normal = nd->normal; 55 | pcl::PointXYZINormal vertex; 56 | vertex.x = centroid(0); 57 | vertex.y = centroid(1); 58 | vertex.z = centroid(2); 59 | vertex.normal_x = normal(0); 60 | vertex.normal_y = normal(1); 61 | vertex.normal_z = normal(2); 62 | vertex.intensity = nd->sem_type; 63 | 64 | surf_vertices_->push_back(vertex); 65 | surf_block_nodes.push_back(nd); 66 | } 67 | } 68 | 69 | SurfaceCluster cluster; 70 | std::vector cluster_res; 71 | cluster.setCorePointMinPts(5); 72 | cluster.setAngleTolerance(M_PI/18); 73 | cluster.setDistTolerance(0.2); 74 | cluster.setClusterTolerance(5.0); 75 | cluster.setMinClusterSize(5); 76 | cluster.setInputCloud(surf_vertices_); 77 | cluster.extract(cluster_res); 78 | 79 | for(int i = 0; i < cluster_res.size(); ++i) { 80 | pcl::PointCloud instance(*surf_vertices_, cluster_res[i].indices); 81 | uint16_t sem_type = static_cast(std::round(instance.points.front().intensity)); 82 | pcl::PointXYZINormal const& p0{instance.points.front()}; 83 | Eigen::Vector3d const v0(p0.x, p0.y, p0.z); 84 | Eigen::Vector3d const n0(p0.normal_x, p0.normal_y, p0.normal_z); 85 | Eigen::Vector3d mu{v0}, normal{n0}; 86 | Eigen::Matrix3d sigma{Eigen::Matrix3d::Zero()}; 87 | // sigma_.setZero(); 88 | for(int j = 1; j < instance.points.size(); ++j) { 89 | pcl::PointXYZINormal const& p{instance.points[j]}; 90 | Eigen::Vector3d const v(p.x, p.y, p.z); 91 | Eigen::Vector3d n(p.normal_x, p.normal_y, p.normal_z); 92 | if(n.dot(n0) < 0) { 93 | n = -n; 94 | } 95 | mu += v; 96 | sigma += v * v.transpose(); 97 | normal += n; 98 | } 99 | mu /= instance.points.size(); 100 | normal.normalize(); 101 | sigma.noalias() = sigma / instance.points.size() - mu * mu.transpose(); 102 | 103 | Eigen::SelfAdjointEigenSolver saes(sigma); 104 | Eigen::Vector3d node_normal = saes.eigenvectors().col(0); 105 | 106 | SceneGraphNode::Ptr node = SceneGraphNode::Ptr( 107 | new SceneGraphNode(graph_node_id, sem_type, mu, normal) 108 | ); 109 | node->setSigma(sigma); 110 | for(auto id: cluster_res[i].indices) { 111 | node->AddBlockNode(surf_block_nodes[id]); 112 | } 113 | nodes_.push_back(node); 114 | graph_node_id++; 115 | surf_node_num++; 116 | } 117 | } 118 | 119 | void SceneGraph::buildSelfAffinity() { 120 | self_loss_mat_.resize(nodes_.size()); 121 | for(int i = 0; i < nodes_.size(); ++i) { 122 | for(int j = 0; j < nodes_.size(); ++j) { 123 | if (i == j) 124 | continue; 125 | 126 | auto const& node_i = nodes_[i]; 127 | auto const& node_j = nodes_[j]; 128 | 129 | Eigen::MatrixXd gi = node_i->graff_coord_; 130 | Eigen::MatrixXd gj = node_j->graff_coord_; 131 | 132 | const Eigen::Vector3d b0i = gi.topRightCorner(3, 1); 133 | gi.topRightCorner(3, 1).setZero(); 134 | gj.topRightCorner(3, 1) -= b0i; 135 | const Eigen::Vector3d b0j = gj.topRightCorner(3, 1); 136 | gj.rightCols<1>() /= std::sqrt(1 + b0j.squaredNorm()); 137 | 138 | auto sigma = Eigen::JacobiSVD(gi.transpose() * gj).singularValues(); 139 | double metric = 0.0; 140 | for(int i = 0; i < sigma.rows(); ++i) { 141 | // double theta = std::tan(std::acos(sigma(i))); 142 | // double theta = std::acos(sigma(i)); 143 | double theta = sigma(i); 144 | metric += theta*theta; 145 | } 146 | metric = std::sqrt(metric); 147 | self_affinity_mat_[i][j] = sigma.norm(); 148 | // self_affinity_mat_[i][j] = metric; 149 | } 150 | } 151 | } 152 | 153 | } // namespace SLIM 154 | -------------------------------------------------------------------------------- /modules/scene_graph.h: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_GRAPH_H 2 | #define SCENE_GRAPH_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "scene_graph_node.h" 13 | #include "vector_map.h" 14 | #include "factor/graff_coordinate.h" 15 | 16 | namespace SLIM { 17 | 18 | class SceneGraph { 19 | public: 20 | 21 | typedef std::shared_ptr Ptr; 22 | 23 | SceneGraph() = default; 24 | 25 | inline Ptr makeShared() { 26 | return Ptr(new SceneGraph(*this)); 27 | } 28 | 29 | inline SceneGraph::Ptr clone() { 30 | SceneGraph::Ptr graph(new SceneGraph); 31 | for(int i = 0; i < this->nodes_.size(); ++i) { 32 | SceneGraphNode node(nodes_[i]); 33 | graph->nodes_.push_back(node.makeShared()); 34 | } 35 | graph->block_ = this->block_; 36 | return graph; 37 | } 38 | 39 | void transform(const Transform& Tsd); 40 | 41 | void buildFromBlock(const Block::Ptr block); 42 | 43 | void buildSelfAffinity(); 44 | 45 | public: 46 | std::vector nodes_; 47 | std::unordered_map> self_affinity_mat_; 48 | std::vector> self_loss_mat_; 49 | 50 | std::vector::Ptr> surf_vertex_clouds_; 51 | pcl::PointCloud::Ptr surf_vertices_; 52 | std::vector graff_coords_; 53 | 54 | Block::Ptr block_; 55 | }; 56 | 57 | } // namespace SLIM 58 | 59 | 60 | #endif -------------------------------------------------------------------------------- /modules/scene_graph_node.h: -------------------------------------------------------------------------------- 1 | #ifndef SEMANTIC_GRAPH_NODE_H 2 | #define SEMANTIC_GRAPH_NODE_H 3 | 4 | #include "observation.h" 5 | #include "landmark.h" 6 | #include "dbscan/surface_cluster.h" 7 | #include "factor/math_utility.h" 8 | #include "factor/graff_coordinate.h" 9 | 10 | namespace SLIM { 11 | 12 | class SceneGraphNode { 13 | public: 14 | typedef std::shared_ptr Ptr; 15 | 16 | enum GeoType { 17 | LINE = 1, 18 | SURFACE = 2, 19 | NONE = 3, 20 | }; 21 | 22 | SceneGraphNode() 23 | : id_(0), semantic_type_(NONE_FEATURE_ID), node_value_(Eigen::Vector3d::Zero()), node_normal_(Eigen::Vector3d::Zero()) { 24 | SetGeoType(NONE_FEATURE_ID); 25 | } 26 | 27 | SceneGraphNode(const uint32_t id, const uint16_t semantic_type, const Eigen::Vector3d& node_value, const Eigen::Vector3d& node_normal) 28 | : id_(id), semantic_type_(semantic_type), node_value_(node_value), node_normal_(node_normal) { 29 | SetGeoType(semantic_type); 30 | setGraffCoord(); 31 | if(geometry_type_ == SURFACE) { 32 | double d = -node_normal_.dot(node_value_); 33 | } 34 | } 35 | 36 | SceneGraphNode(const SceneGraphNode::Ptr other) 37 | : id_(other->id_), semantic_type_(other->semantic_type_), geometry_type_(other->geometry_type_), 38 | node_value_(other->node_value_), node_normal_(other->node_normal_), 39 | vec_line_feature_ptr_(other->vec_line_feature_ptr_), vec_surf_feature_ptr_(other->vec_surf_feature_ptr_), 40 | vec_line_lms_(other->vec_line_lms_), vec_surf_lms_(other->vec_surf_lms_), vec_nodes_(other->vec_nodes_) { 41 | setGraffCoord(); 42 | } 43 | 44 | inline Ptr makeShared() { 45 | return Ptr(new SceneGraphNode(*this)); 46 | } 47 | 48 | void SetGeoType(const uint16_t semantic_type) { 49 | switch (semantic_type_) 50 | { 51 | case NONE_FEATURE_ID: 52 | geometry_type_ = NONE; 53 | break; 54 | 55 | case POLE_ID: 56 | geometry_type_ = LINE; 57 | break; 58 | 59 | case TRAFFIC_SIGN_ID: 60 | geometry_type_ = SURFACE; 61 | break; 62 | 63 | case ROAD_ID: 64 | // geometry_type_ = SURFACE; 65 | geometry_type_ = SURFACE; 66 | break; 67 | 68 | case SIDEWALK_ID: 69 | // geometry_type_ = SURFACE; 70 | geometry_type_ = SURFACE; 71 | break; 72 | 73 | case BUILDING_ID: 74 | // geometry_type_ = SURFACE; 75 | geometry_type_ = SURFACE; 76 | break; 77 | 78 | case FENCE_ID: 79 | // geometry_type_ = SURFACE; 80 | geometry_type_ = SURFACE; 81 | break; 82 | 83 | default: 84 | geometry_type_ = NONE; 85 | break; 86 | } 87 | } 88 | 89 | void setGraffCoord() { 90 | if(geometry_type_ == GeoType::LINE) { 91 | graff_coord_ = GraffLine(node_normal_, node_value_ / 40.0).get(); 92 | line_info_ = LineInfo(node_value_ / 40.0, node_normal_); 93 | } 94 | else if(geometry_type_ == GeoType::SURFACE) { 95 | graff_coord_ = GraffSurface(SurfaceInfo(node_value_, node_normal_).subspace(), node_value_ / 40.0).get(); 96 | surface_info_ = SurfaceInfo(node_value_ / 40.0, node_normal_); 97 | } 98 | } 99 | 100 | void setSigma(const Eigen::Matrix3d& sigma) { 101 | sigma_ = sigma; 102 | } 103 | 104 | uint32_t id() const { 105 | return id_; 106 | } 107 | 108 | uint16_t semantic_type() const { 109 | return semantic_type_; 110 | } 111 | 112 | GeoType geometry_type() const { 113 | return geometry_type_; 114 | } 115 | 116 | Eigen::Vector3d node_value() const { 117 | return node_value_; 118 | } 119 | 120 | Eigen::Vector3d normal_value() const { 121 | return node_normal_; 122 | } 123 | 124 | void AddLineFeaturePtr(const LineOB::Ptr& ptr) { 125 | vec_line_feature_ptr_.push_back(ptr); 126 | } 127 | 128 | void AddSurfFeaturePtr(const SurfaceOB::Ptr& ptr) { 129 | vec_surf_feature_ptr_.push_back(ptr); 130 | } 131 | 132 | void AddLineLandmark(const LineLM::Ptr& ptr) { 133 | vec_line_lms_.push_back(ptr); 134 | } 135 | 136 | void AddSurfLandmark(const SurfaceLM::Ptr& ptr) { 137 | vec_surf_lms_.push_back(ptr); 138 | } 139 | 140 | void AddBlockNode(const Block::Node::Ptr& ptr) { 141 | vec_nodes_.push_back(ptr); 142 | } 143 | 144 | void transform(const Transform& Tsd) { 145 | node_value_ = Tsd * node_value_; 146 | node_normal_ = Tsd.dcm() * node_normal_; 147 | } 148 | 149 | public: 150 | uint32_t id_; // must less than 2^21 151 | uint16_t semantic_type_; 152 | GeoType geometry_type_; 153 | Eigen::Vector3d node_value_; 154 | Eigen::Vector3d node_normal_; 155 | Eigen::Matrix3d sigma_; 156 | double dist_; 157 | std::vector vec_line_feature_ptr_; 158 | std::vector vec_surf_feature_ptr_; 159 | 160 | std::vector vec_line_lms_; 161 | std::vector vec_surf_lms_; 162 | 163 | std::vector vec_nodes_; 164 | Eigen::MatrixXd graff_coord_; 165 | LineInfo line_info_; 166 | SurfaceInfo surface_info_; 167 | }; 168 | } // namespace SLIM 169 | 170 | 171 | 172 | #endif -------------------------------------------------------------------------------- /modules/semantic_definition.h: -------------------------------------------------------------------------------- 1 | #ifndef SEMANTIC_DEFINITION_H 2 | #define SEMANTIC_DEFINITION_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace SLIM { 10 | 11 | #define NONE_FEATURE_ID 0 12 | #define POLE_ID 80 13 | #define ROAD_ID 40 14 | #define SIDEWALK_ID 48 15 | #define BUILDING_ID 50 16 | #define FENCE_ID 51 17 | #define TRAFFIC_SIGN_ID 81 18 | 19 | #define POLE_RESOLUTION 0.5 20 | #define ROAD_RESOLUTION 2.0 21 | #define SIDEWALK_RESOLUTION 2.0 22 | #define BUILDING_RESOLUTION 1.0 23 | #define FENCE_RESOLUTION 1.0 24 | #define TRAFFIC_SIGN_RESOLUTION 0.2 25 | 26 | #define CV_COLOR_BLACK cv::Scalar(0,0,0) // 纯黑 27 | #define CV_COLOR_WHITE cv::Scalar(255,255,255) // 纯白 28 | #define CV_COLOR_RED cv::Scalar(0,0,255) // 纯红 29 | #define CV_COLOR_GREEN cv::Scalar(0,255,0) // 纯绿 30 | #define CV_COLOR_BLUE cv::Scalar(255,0,0) // 纯蓝 31 | 32 | #define CV_COLOR_DARKGRAY cv::Scalar(169,169,169) // 深灰色 33 | #define CV_COLOR_DARKRED cv::Scalar(0,0,169) // 深红色 34 | #define CV_COLOR_ORANGERED cv::Scalar(0,69,255) // 橙红色 35 | 36 | #define CV_COLOR_CHOCOLATE cv::Scalar(30,105,210) // 巧克力色 37 | #define CV_COLOR_GOLD cv::Scalar(10,215,255) // 金色 38 | #define CV_COLOR_YELLOW cv::Scalar(0,255,255) // 纯黄色 39 | 40 | #define CV_COLOR_OLIVE cv::Scalar(0,128,128) // 橄榄色 41 | #define CV_COLOR_LIGHTGREEN cv::Scalar(144,238,144) // 浅绿色 42 | #define CV_COLOR_DARKCYAN cv::Scalar(139,139,0) // 深青色 43 | #define CV_COLOR_CYAN cv::Scalar(255,255,0) // 青色 44 | 45 | #define CV_COLOR_SKYBLUE cv::Scalar(235,206,135) // 天蓝色 46 | #define CV_COLOR_INDIGO cv::Scalar(130,0,75) // 藏青色 47 | #define CV_COLOR_PURPLE cv::Scalar(128,0,128) // 紫色 48 | 49 | #define CV_COLOR_PINK cv::Scalar(203,192,255) // 粉色 50 | #define CV_COLOR_DEEPPINK cv::Scalar(147,20,255) // 深粉色 51 | #define CV_COLOR_VIOLET cv::Scalar(238,130,238) // 紫罗兰 52 | 53 | static std::vector cs_map_color = { 54 | CV_COLOR_DARKCYAN, 55 | CV_COLOR_PURPLE, 56 | CV_COLOR_ORANGERED, 57 | CV_COLOR_YELLOW, 58 | CV_COLOR_INDIGO, 59 | CV_COLOR_DEEPPINK, 60 | CV_COLOR_OLIVE, 61 | CV_COLOR_DARKRED, 62 | CV_COLOR_GREEN, 63 | CV_COLOR_BLUE 64 | }; 65 | 66 | static std::vector random_color_vec = { 67 | cv::Scalar(0, 255, 0), 68 | cv::Scalar(0, 0, 255), 69 | cv::Scalar(245, 150, 100), 70 | cv::Scalar(245, 230, 100), 71 | cv::Scalar(250, 80, 100), 72 | cv::Scalar(150, 60, 30), 73 | cv::Scalar(255, 0, 0), 74 | cv::Scalar(180, 30, 80), 75 | cv::Scalar(50, 50, 255), 76 | cv::Scalar(200, 40, 255), 77 | cv::Scalar(90, 30, 150), 78 | cv::Scalar(255, 0, 255), 79 | cv::Scalar(255, 150, 255), 80 | cv::Scalar(75, 0, 75), 81 | cv::Scalar(75, 0, 175), 82 | cv::Scalar(0, 200, 255), 83 | cv::Scalar(50, 120, 255), 84 | cv::Scalar(0, 150, 255), 85 | cv::Scalar(170, 255, 150), 86 | cv::Scalar(0, 175, 0), 87 | cv::Scalar(0, 60, 135), 88 | cv::Scalar(80, 240, 150), 89 | cv::Scalar(150, 240, 255), 90 | cv::Scalar(0, 0, 255), 91 | cv::Scalar(255, 255, 50), 92 | cv::Scalar(245, 150, 100), 93 | }; 94 | 95 | static std::map kitti_sem_cls_info = { 96 | {0, "unlabeled"}, 97 | {1, "outlier"}, 98 | {10, "car"}, 99 | {11, "bicycle"}, 100 | {13, "bus"}, 101 | {15, "motorcycle"}, 102 | {16, "on-rails"}, 103 | {18, "truck"}, 104 | {20, "other-vehicle"}, 105 | {30, "person"}, 106 | {31, "bicyclist"}, 107 | {32, "motorcyclist"}, 108 | {40, "road"}, 109 | {44, "parking"}, 110 | {48, "sidewalk"}, 111 | {49, "other-ground"}, 112 | {50, "building"}, 113 | {51, "fence"}, 114 | {52, "other-structure"}, 115 | {60, "lane-marking"}, 116 | {70, "vegetation"}, 117 | {71, "trunk"}, 118 | {72, "terrain"}, 119 | {80, "pole"}, 120 | {81, "traffic-sign"}, 121 | {99, "other-object"}, 122 | {252, "moving-car"}, 123 | {253, "moving-bicyclist"}, 124 | {254, "moving-person"}, 125 | {255, "moving-motorcyclist"}, 126 | {256, "moving-on-rails"}, 127 | {257, "moving-bus"}, 128 | {258, "moving-truck"}, 129 | {259, "moving-other-vehicle"} 130 | }; 131 | 132 | static std::map kitti_sem_color_info = { 133 | {0, cv::Scalar(0, 0, 0)}, 134 | {1, cv::Scalar(0, 0, 255)}, 135 | {10, cv::Scalar(245, 150, 100)}, 136 | {11, cv::Scalar(245, 230, 100)}, 137 | {13, cv::Scalar(250, 80, 100)}, 138 | {15, cv::Scalar(150, 60, 30)}, 139 | {16, cv::Scalar(255, 0, 0)}, 140 | {18, cv::Scalar(180, 30, 80)}, 141 | {20, cv::Scalar(255, 0, 0)}, 142 | {30, cv::Scalar(30, 30, 255)}, 143 | {31, cv::Scalar(200, 40, 255)}, 144 | {32, cv::Scalar(90, 30, 150)}, 145 | {40, cv::Scalar(255, 0, 255)}, 146 | {44, cv::Scalar(255, 150, 255)}, 147 | {48, cv::Scalar(75, 0, 75)}, 148 | {49, cv::Scalar(75, 0, 175)}, 149 | {50, cv::Scalar(0, 200, 255)}, 150 | {51, cv::Scalar(50, 120, 255)}, 151 | {52, cv::Scalar(0, 150, 255)}, 152 | {60, cv::Scalar(170, 255, 150)}, 153 | {70, cv::Scalar(0, 175, 0)}, 154 | {71, cv::Scalar(0, 60, 135)}, 155 | {72, cv::Scalar(80, 240, 150)}, 156 | {80, cv::Scalar(150, 240, 255)}, 157 | {81, cv::Scalar(0, 0, 255)}, 158 | {99, cv::Scalar(255, 255, 50)}, 159 | {252, cv::Scalar(245, 150, 100)}, 160 | {253, cv::Scalar(200, 40, 255)}, 161 | {254, cv::Scalar(30, 30, 255)}, 162 | {255, cv::Scalar(90, 30, 150)}, 163 | {256, cv::Scalar(255, 0, 0)}, 164 | {257, cv::Scalar(250, 80, 100)}, 165 | {258, cv::Scalar(180, 30, 80)}, 166 | {259, cv::Scalar(255, 0, 0)} 167 | }; 168 | 169 | static std::string GetSemanticName(const uint16_t sem_id) { 170 | std::string cls_name; 171 | switch (sem_id) 172 | { 173 | case POLE_ID: 174 | cls_name = "POLE"; 175 | break; 176 | 177 | case TRAFFIC_SIGN_ID: 178 | cls_name = "TRAFFIC_SIGN"; 179 | break; 180 | 181 | case ROAD_ID: 182 | cls_name = "ROAD"; 183 | break; 184 | 185 | case SIDEWALK_ID: 186 | cls_name = "SIDEWALK"; 187 | break; 188 | 189 | case BUILDING_ID: 190 | cls_name = "BUILDING"; 191 | break; 192 | 193 | case FENCE_ID: 194 | cls_name = "FENCE"; 195 | break; 196 | 197 | default: 198 | cls_name = "NOT USED"; 199 | break; 200 | } 201 | return cls_name; 202 | } 203 | 204 | static uint16_t GetSemanticId(const std::string& name) { 205 | if(name == "pole") { 206 | return POLE_ID; 207 | } 208 | else if(name == "traffic sign") { 209 | return TRAFFIC_SIGN_ID; 210 | } 211 | else if(name == "road") { 212 | return ROAD_ID; 213 | } 214 | else if(name == "sidewalk") { 215 | return SIDEWALK_ID; 216 | } 217 | else if(name == "building") { 218 | return BUILDING_ID; 219 | } 220 | else if(name == "fence") { 221 | return FENCE_ID; 222 | } 223 | else { 224 | return NONE_FEATURE_ID; 225 | } 226 | } 227 | 228 | static double GetSemanticResolution(const uint16_t sem_id) { 229 | switch (sem_id) { 230 | case POLE_ID: 231 | return POLE_RESOLUTION; 232 | 233 | case TRAFFIC_SIGN_ID: 234 | return TRAFFIC_SIGN_RESOLUTION; 235 | 236 | case ROAD_ID: 237 | return ROAD_RESOLUTION; 238 | 239 | case SIDEWALK_ID: 240 | return SIDEWALK_RESOLUTION; 241 | 242 | case BUILDING_ID: 243 | return BUILDING_RESOLUTION; 244 | 245 | case FENCE_ID: 246 | return FENCE_RESOLUTION; 247 | 248 | default: 249 | return 0; 250 | } 251 | } 252 | 253 | } // namespace SLIM 254 | 255 | #endif -------------------------------------------------------------------------------- /modules/spmm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | project(spmm LANGUAGES C CXX) 4 | 5 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 6 | 7 | if (CMAKE_COMPILER_IS_GNUCXX) 8 | set(CMAKE_CXX_FLAGS "-std=c++14 -O3 -fPIC") 9 | endif() 10 | 11 | find_package(Eigen REQUIRED) 12 | include_directories(${EIGEN3_INCLUDE_DIR}) 13 | 14 | find_package(CUDAToolkit REQUIRED) 15 | 16 | include(GNUInstallDirs) 17 | 18 | # Global CXX flags/options 19 | set(CMAKE_CXX_STANDARD 14) 20 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 21 | set(CMAKE_CXX_EXTENSIONS OFF) 22 | 23 | # Global CUDA CXX flags/options 24 | set(CUDA_HOST_COMPILER ${CMAKE_CXX_COMPILER}) 25 | set(CMAKE_CUDA_STANDARD 14) 26 | set(CMAKE_CUDA_STANDARD_REQUIRED ON) 27 | set(CMAKE_CUDA_EXTENSIONS OFF) 28 | 29 | # Debug options 30 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -O0 -g") 31 | set(CMAKE_CUDA_FLAGS_DEBUG "${CMAKE_CUDA_FLAGS} -O0 -g -lineinfo") 32 | 33 | add_library(spmm STATIC 34 | spmm.cpp 35 | ) 36 | 37 | set_property(TARGET spmm PROPERTY CUDA_ARCHITECTURES OFF) 38 | target_include_directories(spmm 39 | PUBLIC 40 | ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES} 41 | ) 42 | target_link_libraries(spmm 43 | PUBLIC 44 | CUDA::cudart 45 | CUDA::cusparse 46 | CUDA::cublas 47 | CUDA::cusparse 48 | CUDA::cusolver 49 | ) -------------------------------------------------------------------------------- /modules/spmm/device_buffer.h: -------------------------------------------------------------------------------- 1 | #ifndef __DEVICE_BUFFER_H__ 2 | #define __DEVICE_BUFFER_H__ 3 | 4 | #include 5 | #include 6 | #include "macro.h" 7 | 8 | template 9 | struct DeviceBuffer 10 | { 11 | DeviceBuffer() : data(nullptr), size(0) {} 12 | DeviceBuffer(size_t size) : data(nullptr), size(0) { allocate(size); } 13 | ~DeviceBuffer() { destroy(); } 14 | 15 | void allocate(size_t _size) 16 | { 17 | if (data && size >= _size) 18 | return; 19 | 20 | destroy(); 21 | CHECK_CUDA(cudaMalloc(&data, sizeof(T) * _size)); 22 | size = _size; 23 | } 24 | 25 | void destroy() 26 | { 27 | if (data) 28 | CHECK_CUDA(cudaFree(data)); 29 | data = nullptr; 30 | size = 0; 31 | } 32 | 33 | void upload(const T* h_data) 34 | { 35 | CHECK_CUDA(cudaMemcpy(data, h_data, sizeof(T) * size, cudaMemcpyHostToDevice)); 36 | } 37 | 38 | void download(T* h_data) const 39 | { 40 | CHECK_CUDA(cudaMemcpy(h_data, data, sizeof(T) * size, cudaMemcpyDeviceToHost)); 41 | } 42 | 43 | void copyTo(DeviceBuffer& rhs) const 44 | { 45 | CHECK_CUDA(cudaMemcpy(rhs.data, data, sizeof(T) * size, cudaMemcpyDeviceToDevice)); 46 | } 47 | 48 | void copyTo(T* rhs) const 49 | { 50 | CHECK_CUDA(cudaMemcpy(rhs, data, sizeof(T) * size, cudaMemcpyDeviceToDevice)); 51 | } 52 | 53 | void fillZero() 54 | { 55 | CHECK_CUDA(cudaMemset(data, 0, sizeof(T) * size)); 56 | } 57 | 58 | void fill(T value) 59 | { 60 | CHECK_CUDA(cudaMemset(data, value, sizeof(T) * size)); 61 | } 62 | 63 | bool empty() const { return !(data && size > 0); } 64 | 65 | void assign(size_t size, const T* h_data) 66 | { 67 | allocate(size); 68 | upload(h_data); 69 | } 70 | 71 | void print() const { 72 | T* h_data = (T*)malloc(sizeof(T) * size); 73 | download(h_data); 74 | for(int i = 0; i < size; ++i) { 75 | std::cout << h_data[i] << ", "; 76 | } 77 | std::cout << std::endl; 78 | } 79 | 80 | T* data; 81 | size_t size; 82 | }; 83 | 84 | #endif // !__DEVICE_BUFFER_H__ 85 | -------------------------------------------------------------------------------- /modules/spmm/macro.h: -------------------------------------------------------------------------------- 1 | #ifndef __MACRO_H__ 2 | #define __MACRO_H__ 3 | 4 | #include 5 | 6 | #include // cudaMalloc, cudaMemcpy, etc. 7 | #include // cusparseSpGEMM 8 | #include 9 | #include 10 | #include // printf 11 | #include // EXIT_FAILURE 12 | 13 | #define CHECK_CUSOLVER(err) \ 14 | { \ 15 | cusolverStatus_t err_ = (err); \ 16 | if (err_ != CUSOLVER_STATUS_SUCCESS) { \ 17 | printf("cusolver error %d at %s:%d\n", err_, __FILE__, __LINE__); \ 18 | throw std::runtime_error("cusolver error"); \ 19 | } \ 20 | } \ 21 | 22 | // cublas API error checking 23 | #define CHECK_CUBLAS(err) \ 24 | { \ 25 | cublasStatus_t err_ = (err); \ 26 | if (err_ != CUBLAS_STATUS_SUCCESS) { \ 27 | printf("cublas error %d at %s:%d\n", err_, __FILE__, __LINE__); \ 28 | throw std::runtime_error("cublas error"); \ 29 | } \ 30 | } \ 31 | 32 | #define CHECK_CUDA(err) \ 33 | { \ 34 | cudaError_t err_ = (err); \ 35 | if (err_ != cudaSuccess) { \ 36 | printf("CUDA error %d at %s:%d\n", err_, __FILE__, __LINE__); \ 37 | throw std::runtime_error("CUDA error"); \ 38 | } \ 39 | } \ 40 | 41 | #define CHECK_CUSPARSE(err) \ 42 | { \ 43 | cusparseStatus_t err_ = (err); \ 44 | if (err_ != CUSPARSE_STATUS_SUCCESS) { \ 45 | printf("cusparse error %d at %s:%d\n", err_, __FILE__, __LINE__); \ 46 | throw std::runtime_error("cusparse error"); \ 47 | } \ 48 | } \ 49 | 50 | #endif // !__MACRO_H__ 51 | -------------------------------------------------------------------------------- /modules/spmm/spmm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include "device_buffer.h" 14 | #include 15 | #include 16 | 17 | void printGpuMemInfo(); 18 | 19 | struct CusparseHandle 20 | { 21 | CusparseHandle() { init(); } 22 | ~CusparseHandle() { destroy(); } 23 | void init() { cusparseCreate(&handle); } 24 | void destroy() { cusparseDestroy(handle); } 25 | operator cusparseHandle_t() const { return handle; } 26 | CusparseHandle(const CusparseHandle&) = delete; 27 | CusparseHandle& operator=(const CusparseHandle&) = delete; 28 | cusparseHandle_t handle; 29 | }; 30 | 31 | struct CusparseMatDescriptor 32 | { 33 | CusparseMatDescriptor() { init(); } 34 | ~CusparseMatDescriptor() { destroy(); } 35 | 36 | void init() { 37 | cusparseCreateMatDescr(&desc); 38 | cusparseSetMatType(desc, CUSPARSE_MATRIX_TYPE_GENERAL); 39 | cusparseSetMatIndexBase(desc, CUSPARSE_INDEX_BASE_ZERO); 40 | cusparseSetMatDiagType(desc, CUSPARSE_DIAG_TYPE_NON_UNIT); 41 | } 42 | 43 | void destroy() { cusparseDestroyMatDescr(desc); } 44 | operator cusparseMatDescr_t() const { return desc; } 45 | CusparseMatDescriptor(const CusparseMatDescriptor&) = delete; 46 | CusparseMatDescriptor& operator=(const CusparseMatDescriptor&) = delete; 47 | cusparseMatDescr_t desc; 48 | }; 49 | 50 | void convertEigenToCuSparse( 51 | const Eigen::SparseMatrix &mat, int *row, int *col, double *val); 52 | 53 | void convertCuSparseToEigen( 54 | const int *row, 55 | const int *col, 56 | const double *val, 57 | const int num_non0, 58 | const int mat_row, 59 | const int mat_col, 60 | Eigen::SparseMatrix &mat); 61 | 62 | 63 | Eigen::MatrixXd solveCholeskyCUDA(const Eigen::MatrixXd& mat); 64 | 65 | std::vector inverseParallelCUDA(const std::vector& mats, int m); 66 | 67 | class CuDMat { 68 | public: 69 | CuDMat(): rows_(0), cols_(0) {} 70 | ~CuDMat() { 71 | values_.destroy(); 72 | } 73 | 74 | CuDMat(const int rows, const int cols) 75 | : rows_(rows), cols_(cols) { 76 | values_.allocate(rows_ * cols_); 77 | values_.fillZero(); 78 | } 79 | 80 | CuDMat(const Eigen::MatrixXd& mat) { 81 | rows_ = mat.rows(); 82 | cols_ = mat.cols(); 83 | values_.allocate(rows_ * cols_); 84 | values_.upload(mat.data()); 85 | } 86 | 87 | CuDMat(CuDMat&& other) noexcept { 88 | rows_ = other.rows_; 89 | cols_ = other.cols_; 90 | values_.data = other.values_.data; 91 | values_.size = other.values_.size; 92 | other.values_.data = nullptr; 93 | other.values_.size = 0; 94 | } 95 | 96 | 97 | void resize(const int rows, const int cols) { 98 | rows_ = rows; 99 | cols_ = cols; 100 | values_.allocate(rows_ * cols_); 101 | values_.fillZero(); 102 | } 103 | 104 | void release(); 105 | 106 | CuDMat& operator=(CuDMat&& other) noexcept; 107 | 108 | CuDMat operator*(const CuDMat& other) const; 109 | 110 | CuDMat operator+(const CuDMat& other) const; 111 | 112 | CuDMat operator-(const CuDMat& other) const; 113 | 114 | CuDMat transpose() const; 115 | 116 | void addSelf(const CuDMat& other); 117 | 118 | void minusSelf(const CuDMat& other); 119 | 120 | CuDMat inverse(); 121 | 122 | Eigen::MatrixXd toEigenDense() const; 123 | 124 | public: 125 | int rows_, cols_; 126 | DeviceBuffer values_; 127 | }; 128 | 129 | class CuSMat { 130 | public: 131 | CuSMat(): rows_(0), cols_(0), nnz_(0) {} 132 | ~CuSMat() { 133 | values_.destroy(); 134 | rowPtr_.destroy(); 135 | colInd_.destroy(); 136 | } 137 | 138 | CuSMat(const int rows, const int cols) 139 | : rows_(rows), cols_(cols), nnz_(0) { 140 | rowPtr_.allocate(rows_ + 1); 141 | } 142 | 143 | CuSMat(CuSMat&& other) noexcept { 144 | rows_ = other.rows_; 145 | cols_ = other.cols_; 146 | nnz_ = other.nnz_; 147 | 148 | rowPtr_.data = other.rowPtr_.data; 149 | rowPtr_.size = other.rowPtr_.size; 150 | other.rowPtr_.data = nullptr; 151 | other.rowPtr_.size = 0; 152 | 153 | colInd_.data = other.colInd_.data; 154 | colInd_.size = other.colInd_.size; 155 | other.colInd_.data = nullptr; 156 | other.colInd_.size = 0; 157 | 158 | values_.data = other.values_.data; 159 | values_.size = other.values_.size; 160 | other.values_.data = nullptr; 161 | other.values_.size = 0; 162 | } 163 | 164 | void release(); 165 | 166 | CuSMat(const Eigen::SparseMatrix& mat); 167 | 168 | CuSMat& operator=(CuSMat&& other) noexcept; 169 | 170 | CuSMat operator+(const CuSMat& other) const; 171 | 172 | CuSMat operator-(const CuSMat& other) const; 173 | 174 | CuSMat operator*(const CuSMat& other) const; 175 | 176 | CuDMat operator*(const CuDMat& other) const; 177 | 178 | Eigen::SparseMatrix toEigenSparse() const; 179 | 180 | public: 181 | int rows_, cols_, nnz_; 182 | DeviceBuffer values_; 183 | DeviceBuffer rowPtr_; 184 | DeviceBuffer colInd_; 185 | // cusparseSpMatDescr_t mat_; 186 | }; 187 | 188 | void matmul(const CuSMat& A, const CuDMat& B, CuDMat& C); 189 | 190 | void matmul(const CuDMat& A, const CuDMat& B, CuDMat& C); 191 | 192 | CuDMat matmul3DM(const CuDMat& A, const CuDMat& B, const CuDMat& C); 193 | 194 | void matmulAtB(const CuDMat& A, const CuDMat& B, CuDMat& AtB); 195 | 196 | void matmulAtXA(const CuDMat& X, const CuDMat& A, CuDMat& AtXA); 197 | -------------------------------------------------------------------------------- /modules/transform.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORM_H 2 | #define TRANSFORM_H 3 | 4 | #include 5 | #include 6 | 7 | namespace SLIM { 8 | 9 | class Transform { 10 | public: 11 | inline Transform() 12 | : pos_(¶meters_[0]), rot_(¶meters_[3]), dcm_(Eigen::Matrix3d::Identity()) { 13 | pos_ = Eigen::Vector3d(0.0, 0.0, 0.0); 14 | rot_ = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); 15 | } 16 | 17 | inline virtual ~Transform() = default; 18 | 19 | inline Transform(const Transform& other) 20 | : parameters_(other.parameters_), pos_(¶meters_[0]), rot_(¶meters_[3]), dcm_(other.dcm()) {} 21 | 22 | inline Eigen::Map const& p() const noexcept { 23 | return pos_; 24 | } 25 | 26 | inline Eigen::Map const& q() const noexcept { 27 | return rot_; 28 | } 29 | 30 | // inline Eigen::Matrix3d const& dcm() const noexcept { 31 | // dcm_ = rot_.toRotationMatrix(); 32 | // return dcm_; 33 | // } 34 | 35 | inline Eigen::Matrix3d dcm() const noexcept { 36 | return rot_.toRotationMatrix(); 37 | } 38 | 39 | 40 | inline Eigen::Matrix4d matrix() const noexcept { 41 | Eigen::Matrix4d mat = Eigen::Matrix4d::Identity(); 42 | mat.block<3, 3>(0, 0) = rot_.toRotationMatrix(); 43 | mat.block<3, 1>(0, 3) = pos_; 44 | return mat; 45 | } 46 | 47 | inline Eigen::Matrix& parameters() { 48 | return parameters_; 49 | } 50 | 51 | inline Transform(const Eigen::Vector3d& p, const Eigen::Matrix3d& dcm) noexcept 52 | : pos_(¶meters_[0]), rot_(¶meters_[3]) { 53 | pos_ = p; 54 | rot_ = Eigen::Quaterniond(dcm); 55 | dcm_ = dcm; 56 | } 57 | 58 | inline Transform(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) noexcept 59 | : pos_(¶meters_[0]), rot_(¶meters_[3]) { 60 | pos_ = p; 61 | rot_ = q.normalized(); 62 | dcm_ = rot_.toRotationMatrix(); 63 | } 64 | 65 | inline Transform(const Eigen::Matrix4d& mat) noexcept 66 | : pos_(¶meters_[0]), rot_(¶meters_[3]) { 67 | pos_ = mat.block<3, 1>(0, 3); 68 | rot_ = Eigen::Quaterniond(mat.block<3, 3>(0, 0)).normalized(); 69 | dcm_ = mat.block<3, 3>(0, 0); 70 | } 71 | 72 | inline Transform inverse() const { 73 | return Transform(-(rot_.inverse() * pos_), rot_.inverse()); 74 | } 75 | 76 | inline void Set(const Eigen::Vector3d& p, const Eigen::Matrix3d& dcm) { 77 | pos_ = p; 78 | rot_ = Eigen::Quaterniond(dcm).normalized(); 79 | dcm_ = dcm; 80 | } 81 | 82 | inline void Set(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) { 83 | pos_ = p; 84 | rot_ = q.normalized(); 85 | dcm_ = rot_.toRotationMatrix(); 86 | } 87 | 88 | inline void SetIdentity() { 89 | pos_.setZero(); 90 | rot_.setIdentity(); 91 | dcm_.setIdentity(); 92 | } 93 | 94 | inline Transform operator*(const Transform& other) const { 95 | return Transform(pos_ + (rot_ * other.pos_), rot_ * other.rot_); 96 | } 97 | 98 | inline Transform& operator=(const Transform& other) & { 99 | if(this != &other) { 100 | this->Set(other.p(), other.q()); 101 | } 102 | return *this; 103 | } 104 | 105 | inline Eigen::Vector3d operator*(const Eigen::Vector3d& p) const { 106 | return rot_ * p + pos_; 107 | } 108 | 109 | protected: 110 | Eigen::Matrix parameters_; 111 | Eigen::Map pos_; 112 | Eigen::Map rot_; 113 | Eigen::Matrix3d dcm_; 114 | 115 | }; 116 | 117 | } 118 | 119 | 120 | #endif -------------------------------------------------------------------------------- /modules/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | namespace SLIM { 4 | 5 | 6 | bool directoryExists(const std::string& path) { 7 | std::ifstream dir(path); 8 | return dir.good(); 9 | } 10 | 11 | bool createDirectory(const std::string& path) { 12 | 13 | if (directoryExists(path)) { 14 | std::cout << path + " exists!" << std::endl; 15 | return true; 16 | } 17 | std::cout << "Create Path: " << path << std::endl; 18 | 19 | if (mkdir(path.c_str(), 0777) == 0) { 20 | return true; 21 | } 22 | return false; 23 | } 24 | 25 | 26 | void removeFilesInFolder(const std::string& folderPath) { 27 | DIR* dir = opendir(folderPath.c_str()); 28 | if (dir == nullptr) { 29 | std::cout << "Failed to open directory." << std::endl; 30 | return; 31 | } 32 | 33 | struct dirent* entry; 34 | while ((entry = readdir(dir)) != nullptr) { 35 | if (strcmp(entry->d_name, ".") != 0 && strcmp(entry->d_name, "..") != 0) { 36 | std::string filePath = folderPath + "/" + entry->d_name; 37 | if (remove(filePath.c_str()) != 0) { 38 | std::cout << "Failed to remove file: " << filePath << std::endl; 39 | } 40 | } 41 | } 42 | 43 | closedir(dir); 44 | } 45 | 46 | } // namespace SLIM 47 | -------------------------------------------------------------------------------- /modules/utility.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILITY_H 2 | #define UTILITY_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "semantic_definition.h" 14 | 15 | #include 16 | 17 | #include 18 | 19 | namespace SLIM { 20 | 21 | class TicToc { 22 | public: 23 | TicToc() { 24 | tic(); 25 | } 26 | 27 | void tic() { 28 | start = std::chrono::system_clock::now(); 29 | } 30 | 31 | double toc() { 32 | end = std::chrono::system_clock::now(); 33 | std::chrono::duration elapsed_seconds = end - start; 34 | return elapsed_seconds.count() * 1000; 35 | } 36 | 37 | private: 38 | std::chrono::time_point start, end; 39 | }; 40 | 41 | template 42 | void reduceVector(std::vector &v, const std::vector& set) { 43 | int j = 0; 44 | for (int i = 0; i < int(set.size()); i++) { 45 | v[j++] = v[set[i]]; 46 | } 47 | v.resize(j); 48 | } 49 | 50 | class MatrixVisualizer { 51 | public: 52 | MatrixVisualizer() = default; 53 | ~MatrixVisualizer() = default; 54 | 55 | MatrixVisualizer(const uint32_t row, const uint32_t col) { 56 | scale_factor_ = (float)max_dim_ / (float)std::max(row, col); 57 | if(scale_factor_ >= 1) { 58 | uint32_t scale = (uint32_t)std::round(scale_factor_); 59 | vis_mat_ = cv::Mat(row * scale, col * scale, CV_8UC3, cv::Scalar(0, 0, 0)); 60 | } 61 | } 62 | 63 | void render(const Eigen::MatrixXd& mat, const std::string& title) { 64 | uint32_t row = mat.rows(), col = mat.cols(); 65 | double normalize_coeff = std::max(std::abs(mat.maxCoeff()), std::abs(mat.minCoeff())); 66 | vis_mat_ = cv::Mat(row, col, CV_8UC3, cv::Scalar(255, 255, 255)); 67 | for(int i = 0; i < row; ++i) { 68 | for(int j = 0; j < col; ++j) { 69 | if(mat(i, j) > 0) { 70 | double scale = std::max(std::min(1.0, mat(i, j) / max_element_), 0.4); 71 | auto color = cv::Vec3b((uchar)std::round(255 - (255 - CV_COLOR_DEEPPINK[0]) * scale), 72 | (uchar)std::round(255 - (255 - CV_COLOR_DEEPPINK[1]) * scale), 73 | (uchar)std::round(255 - (255 - CV_COLOR_DEEPPINK[2]) * scale)); 74 | // auto color = cv::Vec3b((uchar)std::round(255 * (1-scale) - CV_COLOR_DEEPPINK[0] * scale), 75 | // (uchar)std::round(255 * (1-scale) - CV_COLOR_DEEPPINK[1] * scale), 76 | // (uchar)std::round(255 * (1-scale) - CV_COLOR_DEEPPINK[2] * scale)); 77 | vis_mat_.at(i, j) = color; 78 | // vis_mat_.at(i, j) = cv::Vec3b(CV_COLOR_DEEPPINK[0], CV_COLOR_DEEPPINK[1], CV_COLOR_DEEPPINK[2]); 79 | } 80 | else if(mat(i, j) < 0) { 81 | double scale = -std::min(-0.4, std::max(-1.0, mat(i, j) / max_element_)); 82 | 83 | // auto color = cv::Vec3b((uchar)std::round(255 * (1-scale) - CV_COLOR_DARKCYAN[0] * scale), 84 | // (uchar)std::round(255 * (1-scale) - CV_COLOR_DARKCYAN[1] * scale), 85 | // (uchar)std::round(255 * (1-scale) - CV_COLOR_DARKCYAN[2] * scale)); 86 | auto color = cv::Vec3b((uchar)std::round(255 - (255 - CV_COLOR_DARKCYAN[0]) * scale), 87 | (uchar)std::round(255 - (255 - CV_COLOR_DARKCYAN[1]) * scale), 88 | (uchar)std::round(255 - (255 - CV_COLOR_DARKCYAN[2]) * scale)); 89 | vis_mat_.at(i, j) = color; 90 | // vis_mat_.at(i, j) = cv::Vec3b(CV_COLOR_DARKCYAN[0], CV_COLOR_DARKCYAN[1], CV_COLOR_DARKCYAN[2]); 91 | } 92 | else { 93 | vis_mat_.at(i, j) = cv::Vec3b(255, 255, 255); 94 | } 95 | } 96 | } 97 | cv::imshow("vis", vis_mat_); 98 | cv::waitKey(0); 99 | if(title.length() > 0) 100 | cv::imwrite(title, vis_mat_); 101 | } 102 | 103 | float scale_factor_; 104 | const uint32_t max_dim_ = 640; 105 | const double max_element_ = 10; 106 | cv::Mat vis_mat_; 107 | }; 108 | 109 | bool createDirectory(const std::string& path); 110 | 111 | bool directoryExists(const std::string& path); 112 | 113 | void removeFilesInFolder(const std::string& folderPath); 114 | 115 | } // namespace SLIM 116 | 117 | 118 | 119 | 120 | 121 | #endif -------------------------------------------------------------------------------- /test/cloud_visualizer.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "extractor.h" 9 | #include "semantic_definition.h" 10 | #include "vector_map.h" 11 | #include "viewer.h" 12 | #include "factor/line_info.h" 13 | #include "factor/surface_info.h" 14 | #include 15 | 16 | // #include . 17 | 18 | std::string map_save_path, seq, odom_save_path, gt_traj_save_path, cloud_path, pose_file; 19 | double start, duration; 20 | 21 | VectorMap::Ptr vector_map; 22 | Viewer::Ptr viewer; 23 | std::thread visualization; 24 | 25 | // bool init = false; 26 | // SLIM::Transform last_Twb; 27 | 28 | struct KfInfo { 29 | std::string filename; 30 | SLIM::Transform Twb; 31 | }; 32 | 33 | std::vector readPath(const std::string& dirname) { 34 | std::vector filenames; 35 | // sweep the directory 36 | DIR* dir; 37 | if ((dir = opendir(dirname.c_str())) == nullptr) { 38 | throw std::runtime_error("directory " + dirname + " does not exist"); 39 | } 40 | dirent* dp; 41 | for (dp = readdir(dir); dp != nullptr; dp = readdir(dir)) { 42 | const std::string filename = dp->d_name; 43 | if (filename == "." || filename == "..") { 44 | continue; 45 | } 46 | filenames.push_back(dirname + "/" + filename); 47 | } 48 | closedir(dir); 49 | 50 | std::sort(filenames.begin(), filenames.end()); 51 | return filenames; 52 | } 53 | 54 | std::map readTrajectory(const std::string& file_name) { 55 | std::map trajectory; 56 | std::ifstream fin(file_name); 57 | std::string line; 58 | while (std::getline(fin, line)) { 59 | std::istringstream iss(line); 60 | uint64_t timestamp; 61 | if (!(iss >> timestamp)) { 62 | break; 63 | } 64 | double data[7]; 65 | if (!(iss >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5] >> data[6])) { 66 | break; 67 | } 68 | std::string name; 69 | iss >> name; 70 | 71 | Eigen::Vector3d t(data[0], data[1], data[2]); 72 | Eigen::Quaterniond q(data[6], data[3], data[4], data[5]); 73 | SLIM::Transform T(t, q); 74 | 75 | KfInfo info; 76 | info.Twb = T; 77 | info.filename = name; 78 | trajectory.insert(std::make_pair(timestamp, info)); 79 | } 80 | fin.close(); 81 | return trajectory; 82 | } 83 | 84 | 85 | int main(int argc, char** argv) { 86 | 87 | 88 | // std::string traj_file = argv[1]; 89 | std::string map_file = argv[1]; 90 | double intensity = std::stod(argv[2]); 91 | // auto traj = readTrajectory(traj_file); 92 | VectorMap::Ptr map(new VectorMap()); 93 | map->loadJsonFile(map_file); 94 | pcl::visualization::PCLVisualizer viewer("viewer"); 95 | viewer.setBackgroundColor(255, 255, 255); 96 | 97 | auto keyframes = map->getKeyFrames(); 98 | for(auto kf_iter: keyframes) { 99 | SLIM::Transform T = kf_iter.second->Twb(); 100 | Eigen::Vector3f t = T.p().cast(); 101 | Eigen::Matrix3f R = T.dcm().cast(); 102 | Eigen::Affine3f transform = Eigen::Affine3f::Identity(); 103 | transform.translation() = t; // 平移向量 104 | transform.rotate(R); // 绕Z轴旋转45度 105 | 106 | // 添加坐标系 107 | viewer.addCoordinateSystem(1.0, transform); 108 | } 109 | 110 | 111 | int id = 0; 112 | for(auto kf_iter: keyframes) { 113 | SLIM::Transform T = kf_iter.second->Twb(); 114 | std::string filename = kf_iter.second->cloud_path_; 115 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 116 | if(pcl::io::loadPCDFile(filename, *cloud) == -1) { 117 | throw std::runtime_error("Read PCD Failed!\n"); 118 | } 119 | 120 | pcl::VoxelGrid vf; 121 | vf.setLeafSize(0.2, 0.2, 0.2); 122 | vf.setInputCloud(cloud); 123 | vf.filter(*cloud); 124 | 125 | Eigen::Matrix4f Tf = T.matrix().cast(); 126 | for(auto &p: cloud->points) { 127 | Eigen::Vector3f gp = Tf.block<3, 3>(0, 0) * p.getVector3fMap() + Tf.block<3, 1>(0, 3); 128 | p.getVector3fMap() = gp; 129 | } 130 | 131 | std::string label = "cloud_" + std::to_string(id++); 132 | // 将点云添加到PCLVisualizer中 133 | viewer.addPointCloud(cloud, label); 134 | 135 | // 设置点云透明度 136 | viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, intensity, label); 137 | viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 135.0/255.0, 206.0/255.0, 235.0/255.0, label); 138 | } 139 | 140 | auto line_lms = map->getSemLines(); 141 | auto surf_lms = map->getSemSurfaces(); 142 | for(auto &sem_iter: line_lms) { 143 | for(auto &lm_iter: sem_iter.second) { 144 | LineLM::Ptr& lm = lm_iter.second; 145 | lm->double2vector(); 146 | auto pa = lm->pa(), pb = lm->pb(); 147 | pcl::PointXYZ p0(pa(0), pa(1), pa(2)), p1(pb(0), pb(1), pb(2)); 148 | std::string label = "line_" + std::to_string(lm->id()); 149 | viewer.addLine(p0, p1, 0, 255, 255, label); 150 | viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, label); 151 | } 152 | } 153 | 154 | for(auto &sem_iter: surf_lms) { 155 | for(auto &lm_iter: sem_iter.second) { 156 | SurfaceLM::Ptr& lm = lm_iter.second; 157 | lm->double2vector(); 158 | auto vertices = lm->vertices(); 159 | 160 | pcl::PointCloud::Ptr surf(new pcl::PointCloud()); 161 | surf->points.resize(4); 162 | surf->points[0] = pcl::PointXYZ(vertices[0](0), vertices[0](1), vertices[0](2)); 163 | surf->points[1] = pcl::PointXYZ(vertices[1](0), vertices[1](1), vertices[1](2)); 164 | surf->points[2] = pcl::PointXYZ(vertices[2](0), vertices[2](1), vertices[2](2)); 165 | surf->points[3] = pcl::PointXYZ(vertices[3](0), vertices[3](1), vertices[3](2)); 166 | 167 | pcl::Vertices vertices0; 168 | vertices0.vertices.resize(3); 169 | vertices0.vertices[0] = 0; 170 | vertices0.vertices[1] = 1; 171 | vertices0.vertices[2] = 2; 172 | 173 | pcl::Vertices vertices1; 174 | vertices1.vertices.resize(3); 175 | vertices1.vertices[0] = 0; 176 | vertices1.vertices[1] = 1; 177 | vertices1.vertices[2] = 3; 178 | 179 | std::vector all_vertices; 180 | all_vertices.push_back(vertices0); 181 | all_vertices.push_back(vertices1); 182 | 183 | std::string label; 184 | if(lm->semantic_type() == ROAD_ID) { 185 | label = "road_" + std::to_string(lm->id()); 186 | } 187 | else { 188 | label = "building_" + std::to_string(lm->id()); 189 | } 190 | viewer.addPolygonMesh(surf, all_vertices, label); 191 | 192 | if(lm->semantic_type() == ROAD_ID) { 193 | viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255.0/255.0, 0.0/255.0, 255.0/255.0, label); 194 | } 195 | else { 196 | viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255.0/255.0, 69.0/255.0, 0.0/255.0, label); 197 | } 198 | 199 | // viewer.addLine(p0, p1, 0, 255, 255, label); 200 | // viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, label); 201 | } 202 | } 203 | 204 | viewer.addCoordinateSystem(); 205 | viewer.spin(); 206 | return 0; 207 | } -------------------------------------------------------------------------------- /test/map_merge.cpp: -------------------------------------------------------------------------------- 1 | #include "extractor.h" 2 | #include "semantic_definition.h" 3 | #include "vector_map.h" 4 | #include "viewer.h" 5 | #include "multi_map_merger.h" 6 | #include "pcm_solver.h" 7 | #include "utility.h" 8 | #include "clipper.h" 9 | #include 10 | 11 | std::vector readPath(const std::string& dirname) { 12 | std::vector filenames; 13 | // sweep the directory 14 | DIR* dir; 15 | if ((dir = opendir(dirname.c_str())) == nullptr) { 16 | throw std::runtime_error("directory " + dirname + " does not exist"); 17 | } 18 | dirent* dp; 19 | for (dp = readdir(dir); dp != nullptr; dp = readdir(dir)) { 20 | const std::string filename = dp->d_name; 21 | if (filename == "." || filename == "..") { 22 | continue; 23 | } 24 | std::string extension; 25 | size_t dotPos = filename.find_last_of("."); 26 | if (dotPos != std::string::npos && dotPos < filename.length() - 1) { 27 | extension = filename.substr(dotPos + 1); 28 | } 29 | else { 30 | continue; 31 | } 32 | if(extension != "json") 33 | continue; 34 | 35 | filenames.push_back(dirname + "/" + filename); 36 | } 37 | closedir(dir); 38 | 39 | std::sort(filenames.begin(), filenames.end()); 40 | return filenames; 41 | } 42 | 43 | int main(int argc, char** argv) { 44 | 45 | bool use_ba = true; 46 | std::string cloud_path, map_save_path, output_path; 47 | map_save_path = argv[1]; 48 | output_path = argv[2]; 49 | 50 | createDirectory(output_path); 51 | 52 | Viewer::Ptr viewer = Viewer::Ptr(new Viewer); 53 | std::thread vis_thread = std::thread(&Viewer::Run, viewer); 54 | 55 | std::vector files = readPath(map_save_path); 56 | SLIM::MultiMapMerger merger; 57 | merger.setOutputPath(output_path); 58 | merger.setViewer(viewer); 59 | std::vector submaps(files.size()); 60 | for(int i = 0; i < files.size(); ++i) { 61 | submaps[i] = VectorMap::Ptr(new VectorMap); 62 | } 63 | 64 | #pragma omp parallel for shared(submaps, files) 65 | for(int i = 0; i < files.size(); ++i) { 66 | submaps[i]->loadJsonFile(files[i]); 67 | } 68 | merger.ref_map_ = submaps[0]; 69 | SLIM::Transform T_os = submaps[0]->getKeyFrames()[0]->Twb(); 70 | Eigen::Vector3d p = T_os.p() + Eigen::Vector3d(40.0, 40.0, 40.0); 71 | T_os = SLIM::Transform(p, T_os.dcm()); 72 | merger.ref_map_->transform(T_os.inverse()); 73 | 74 | // submaps[0]->extractSparseStruct(); 75 | std::vector cache_map; 76 | std::vector cache_id; 77 | std::vector order; 78 | 79 | 80 | cv::Mat bottom(100, 100, CV_8U, cv::Scalar(255)); 81 | viewer->SetRefMap(submaps[0]); 82 | cv::imshow("bottom", bottom); 83 | cv::waitKey(0); 84 | 85 | for(int i = 1; i < submaps.size(); i++) { 86 | merger.cur_map_ = submaps[i]; 87 | T_os = submaps[i]->getKeyFrames()[0]->Twb(); 88 | merger.cur_map_->transform(T_os.inverse()); 89 | bool res = merger.merge(); 90 | if(!res) { 91 | cache_map.push_back(submaps[i]); 92 | cache_id.push_back(i); 93 | } 94 | else { 95 | order.push_back(i); 96 | } 97 | } 98 | 99 | for(int i = 0; i < cache_map.size(); i++) { 100 | merger.cur_map_ = cache_map[i]; 101 | bool res = merger.merge(); 102 | if(res) { 103 | order.push_back(cache_id[i]); 104 | } 105 | } 106 | std::cout << "cache size: " << cache_map.size() << std::endl; 107 | std::cout << "merge order: " << std::endl; 108 | for(auto num: order) { 109 | std::cout << num << std::endl; 110 | } 111 | vis_thread.join(); 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /thirdparty/pmc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.0) 3 | project(pmc_slim CXX) 4 | 5 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 6 | 7 | add_library(pmc_slim STATIC 8 | pmc_utils.cpp 9 | pmc_graph.cpp 10 | pmc_clique_utils.cpp 11 | pmc_heu.cpp 12 | pmc_maxclique.cpp 13 | pmcx_maxclique.cpp 14 | pmcx_maxclique_basic.cpp 15 | pmc_cores.cpp 16 | ) 17 | 18 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #ifndef __PMC_H__ 21 | #define __PMC_H__ 22 | 23 | #include "pmc_headers.h" 24 | #include "pmc_input.h" 25 | #include "pmc_utils.h" 26 | 27 | #include "pmc_heu.h" 28 | #include "pmc_maxclique.h" 29 | #include "pmcx_maxclique.h" 30 | #include "pmcx_maxclique_basic.h" 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_cores.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #include "pmc_graph.h" 21 | 22 | using namespace pmc; 23 | 24 | void pmc_graph::induced_cores_ordering( 25 | vector& V, 26 | vector& E, 27 | int* &pruned) { 28 | 29 | long long n, d, i, j, start, num, md; 30 | long long v, u, w, du, pu, pw, md_end; 31 | n = vertices.size(); 32 | 33 | vector pos_tmp(n); 34 | vector core_tmp(n); 35 | vector order_tmp(n); 36 | 37 | md = 0; 38 | for(v=1; v md) md = core_tmp[v]; 41 | } 42 | 43 | md_end = md+1; 44 | vector < int > bin(md_end,0); 45 | 46 | for (v=1; v < n; v++) bin[core_tmp[v]]++; 47 | 48 | start = 1; 49 | for (d=0; d < md_end; d++) { 50 | num = bin[d]; 51 | bin[d] = start; 52 | start = start + num; 53 | } 54 | 55 | for (v=1; v 1; d--) bin[d] = bin[d-1]; 62 | bin[0] = 1; 63 | 64 | for (i = 1; i < n; i++) { 65 | v=order_tmp[i]; 66 | for (j = V[v-1]; j < V[v]; j++) { 67 | u = E[j] + 1; 68 | if (core_tmp[u] > core_tmp[v]) { 69 | du = core_tmp[u]; pu = pos_tmp[u]; 70 | pw = bin[du]; w = order_tmp[pw]; 71 | if (u != w) { 72 | pos_tmp[u] = pw; order_tmp[pu] = w; 73 | pos_tmp[w] = pu; order_tmp[pw] = u; 74 | } 75 | bin[du]++; core_tmp[u]--; 76 | } 77 | } 78 | } 79 | 80 | for (v=0; v pos(n); 100 | if (kcore_order.size() > 0) { 101 | vector tmp(n,0); 102 | kcore = tmp; 103 | kcore_order = tmp; 104 | } 105 | else { 106 | kcore_order.resize(n); 107 | kcore.resize(n); 108 | } 109 | 110 | md = 0; 111 | for (v=1; v md) md = kcore[v]; 114 | } 115 | 116 | md_end = md+1; 117 | vector < int > bin(md_end,0); 118 | 119 | for (v=1; v < n; v++) bin[kcore[v]]++; 120 | 121 | start = 1; 122 | for (d=0; d < md_end; d++) { 123 | num = bin[d]; 124 | bin[d] = start; 125 | start = start + num; 126 | } 127 | 128 | // bucket sort 129 | for (v=1; v 1; d--) bin[d] = bin[d-1]; 136 | bin[0] = 1; 137 | 138 | // kcores 139 | for (i=1; i kcore[v]) { 144 | du = kcore[u]; pu = pos[u]; 145 | pw = bin[du]; w = kcore_order[pw]; 146 | if (u != w) { 147 | pos[u] = pw; kcore_order[pu] = w; 148 | pos[w] = pu; kcore_order[pw] = u; 149 | } 150 | bin[du]++; kcore[u]--; 151 | } 152 | } 153 | } 154 | 155 | for (v = 0; v < n-1; v++) { 156 | kcore[v] = kcore[v+1] + 1; // K + 1 157 | kcore_order[v] = kcore_order[v+1]-1; 158 | } 159 | max_core = kcore[kcore_order[num_vertices()-1]] - 1; 160 | 161 | bin.clear(); 162 | pos.clear(); 163 | } 164 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_driver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #include "pmc.h" 21 | 22 | using namespace std; 23 | using namespace pmc; 24 | 25 | int main(int argc, char *argv[]) { 26 | 27 | //! parse command args 28 | input in(argc, argv); 29 | if (in.help) { 30 | usage(argv[0]); 31 | return 0; 32 | } 33 | 34 | //! read graph 35 | pmc_graph G(in.graph_stats,in.graph); 36 | if (in.graph_stats) { G.bound_stats(in.algorithm, in.lb, G); } 37 | 38 | //! ensure wait time is greater than the time to recompute the graph data structures 39 | if (G.num_edges() > 1000000000 && in.remove_time < 120) in.remove_time = 120; 40 | else if (G.num_edges() > 250000000 && in.remove_time < 10) in.remove_time = 10; 41 | cout << "explicit reduce is set to " << in.remove_time << " seconds" < C; 54 | if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input 55 | pmc_heu maxclique(G,in); 56 | in.lb = maxclique.search(G, C); 57 | cout << "Heuristic found clique of size " << in.lb; 58 | cout << " in " << get_time() - seconds << " seconds" <= 0) { 68 | switch(in.algorithm) { 69 | case 0: { 70 | //! k-core pruning, neigh-core pruning/ordering, dynamic coloring bounds/sort 71 | if (G.num_vertices() < in.adj_limit) { 72 | G.create_adj(); 73 | pmcx_maxclique finder(G,in); 74 | finder.search_dense(G,C); 75 | break; 76 | } 77 | else { 78 | pmcx_maxclique finder(G,in); 79 | finder.search(G,C); 80 | break; 81 | } 82 | } 83 | case 1: { 84 | //! k-core pruning, dynamic coloring bounds/sort 85 | if (G.num_vertices() < in.adj_limit) { 86 | G.create_adj(); 87 | pmcx_maxclique_basic finder(G,in); 88 | finder.search_dense(G,C); 89 | break; 90 | } 91 | else { 92 | pmcx_maxclique_basic finder(G,in); 93 | finder.search(G,C); 94 | break; 95 | } 96 | } 97 | case 2: { 98 | //! simple k-core pruning (four new pruning steps) 99 | pmc_maxclique finder(G,in); 100 | finder.search(G,C); 101 | break; 102 | } 103 | default: 104 | cout << "algorithm " << in.algorithm << " not found." < 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include "math.h" 30 | #include "pmc_headers.h" 31 | #include "pmc_utils.h" 32 | #include "pmc_vertex.h" 33 | 34 | 35 | namespace pmc { 36 | class pmc_graph { 37 | private: 38 | // helper functions 39 | void read_mtx(const string& filename); 40 | void read_edges(const string& filename); 41 | void read_metis(const string& filename); 42 | 43 | public: 44 | vector edges; 45 | vector vertices; 46 | vector degree; 47 | int min_degree; 48 | int max_degree; 49 | double avg_degree; 50 | bool is_gstats; 51 | string fn; 52 | bool** adj; 53 | 54 | // constructor 55 | pmc_graph(const string& filename); 56 | pmc_graph(bool graph_stats, const string& filename); 57 | pmc_graph(const string& filename, bool make_adj); 58 | pmc_graph(vector vs, vector es) { 59 | edges = es; 60 | vertices = vs; 61 | vertex_degrees(); 62 | } 63 | pmc_graph(long long nedges, int *ei, int *ej, int offset); 64 | 65 | // destructor 66 | ~pmc_graph(); 67 | 68 | void read_graph(const string& filename); 69 | void create_adj(); 70 | void reduce_graph(int* &pruned); 71 | void reduce_graph( 72 | vector& vs, 73 | vector& es, 74 | int* &pruned, 75 | int id, 76 | int& mc); 77 | 78 | int num_vertices() { return vertices.size() - 1; } 79 | int num_edges() { return edges.size()/2; } 80 | vector * get_vertices(){ return &vertices; } 81 | vector* get_edges(){ return &edges; } 82 | vector* get_degree(){ return °ree; } 83 | vector get_edges_array() { return edges; } 84 | vector get_vertices_array() { return vertices; }; 85 | vector e_v, e_u, eid; 86 | 87 | int vertex_degree(int v) { return vertices[v] - vertices[v+1]; } 88 | long long first_neigh(int v) { return vertices[v]; } 89 | long long last_neigh(int v) { return vertices[v+1]; } 90 | 91 | void sum_vertex_degrees(); 92 | void vertex_degrees(); 93 | void update_degrees(); 94 | void update_degrees(bool flag); 95 | void update_degrees(int* &pruned, int& mc); 96 | double density() { return (double)num_edges() / (num_vertices() * (num_vertices() - 1.0) / 2.0); } 97 | int get_max_degree() { return max_degree; } 98 | int get_min_degree() { return min_degree; } 99 | double get_avg_degree() { return avg_degree; } 100 | 101 | void initialize(); 102 | string get_file_extension(const string& filename); 103 | void basic_stats(double sec); 104 | void bound_stats(int alg, int lb, pmc_graph& G); 105 | 106 | // vertex sorter 107 | void compute_ordering(vector& bound, vector& order); 108 | void compute_ordering(string degree, vector& order); 109 | // edge sorters 110 | void degree_bucket_sort(); 111 | void degree_bucket_sort(bool desc); 112 | 113 | int max_core; 114 | vector kcore; 115 | vector kcore_order; 116 | vector* get_kcores() { return &kcore; } 117 | vector* get_kcore_ordering() { return &kcore_order; } 118 | int get_max_core() { return max_core; } 119 | void update_kcores(int* &pruned); 120 | 121 | void compute_cores(); 122 | void induced_cores_ordering( 123 | vector& V, 124 | vector& E, 125 | int* &pruned); 126 | 127 | // clique utils 128 | int initial_pruning(pmc_graph& G, int* &pruned, int lb); 129 | int initial_pruning(pmc_graph& G, int* &pruned, int lb, bool** &adj); 130 | void order_vertices(vector &V, pmc_graph &G, 131 | int &lb_idx, int &lb, string vertex_ordering, bool decr_order); 132 | 133 | void print_info(vector &C_max, double &sec); 134 | void print_break(); 135 | bool time_left(vector &C_max, double sec, 136 | double time_limit, bool &time_expired_msg); 137 | void graph_stats(pmc_graph& G, int& mc, int id, double &sec); 138 | 139 | void reduce_graph( 140 | vector& vs, 141 | vector& es, 142 | int* &pruned, 143 | pmc_graph& G, 144 | int id, 145 | int& mc); 146 | 147 | bool clique_test(pmc_graph& G, vector C); 148 | }; 149 | 150 | } 151 | #endif 152 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_headers.h: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #ifndef PMC_HEADERS_H_ 21 | #define PMC_HEADERS_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | using namespace std; 35 | 36 | #ifndef LINE_LENGTH 37 | #define LINE_LENGTH 256 38 | #endif 39 | #define NANOSECOND 1000000000 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_heu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #include "pmc_heu.h" 21 | 22 | using namespace pmc; 23 | using namespace std; 24 | 25 | 26 | void pmc_heu::branch(vector& P, int sz, 27 | int& mc, vector& C, vector& ind) { 28 | 29 | if (P.size() > 0) { 30 | 31 | int u = P.back().get_id(); 32 | P.pop_back(); 33 | 34 | for (long long j = (*V)[u]; j < (*V)[u + 1]; j++) ind[(*E)[j]] = 1; 35 | 36 | vector R; 37 | R.reserve(P.size()); 38 | for (int i = 0; i < P.size(); i++) 39 | if (ind[P[i].get_id()]) 40 | if ((*K)[P[i].get_id()] > mc) 41 | R.push_back(P[i]); 42 | 43 | for (long long j = (*V)[u]; j < (*V)[u + 1]; j++) ind[(*E)[j]] = 0; 44 | 45 | int mc_prev = mc; 46 | branch(R, sz + 1, mc, C, ind); 47 | 48 | if (mc > mc_prev) C.push_back(u); 49 | 50 | R.clear(); P.clear(); 51 | } 52 | else if (sz > mc) 53 | mc = sz; 54 | return; 55 | } 56 | 57 | int pmc_heu::search_bounds(pmc_graph& G, 58 | vector& C_max) { 59 | 60 | V = G.get_vertices(); 61 | E = G.get_edges(); 62 | degree = G.get_degree(); 63 | vector C, X; 64 | C.reserve(ub); 65 | C_max.reserve(ub); 66 | vector P, T; 67 | P.reserve(G.get_max_degree()+1); 68 | T.reserve(G.get_max_degree()+1); 69 | vector ind(G.num_vertices(),0); 70 | 71 | bool found_ub = false; 72 | int mc = 0, mc_prev, mc_cur, i, v, k, lb_idx = 0; 73 | 74 | #pragma omp parallel for schedule(dynamic) \ 75 | shared(G, X, mc, C_max, lb_idx) private(i, v, P, mc_prev, mc_cur, C, k) firstprivate(ind) 76 | for (i = G.num_vertices()-1; i >= 0; --i) { 77 | if (found_ub) continue; 78 | 79 | v = (*order)[i]; 80 | mc_prev = mc_cur = mc; 81 | 82 | if ((*K)[v] > mc) { 83 | for (long long j = (*V)[v]; j < (*V)[v + 1]; j++) 84 | if ((*K)[(*E)[j]] > mc) 85 | P.push_back( Vertex((*E)[j], compute_heuristic((*E)[j])) ); 86 | 87 | 88 | if (P.size() > mc_cur) { 89 | std::sort(P.begin(), P.end(), incr_heur); 90 | branch(P, 1 , mc_cur, C, ind); 91 | 92 | if (mc_cur > mc_prev) { 93 | if (mc < mc_cur) { 94 | #pragma omp critical 95 | if (mc < mc_cur) { 96 | mc = mc_cur; 97 | C.push_back(v); 98 | C_max = C; 99 | if (mc >= ub) found_ub = true; 100 | // print_info(C_max); 101 | } 102 | } 103 | } 104 | } 105 | C = X; P = T; 106 | } 107 | } 108 | // cout << "[pmc heuristic]\t mc = " << mc <& C_max, int lb) { 124 | 125 | vector C, X; 126 | C.reserve(ub); 127 | C_max.reserve(ub); 128 | vector P, T; 129 | P.reserve(G.get_max_degree()+1); 130 | T.reserve(G.get_max_degree()+1); 131 | vector ind(G.num_vertices(),0); 132 | 133 | int mc = lb, mc_prev, mc_cur, i; 134 | int lb_idx = 0, v = 0; 135 | for (i = G.num_vertices()-1; i >= 0; i--) { 136 | v = (*order)[i]; 137 | if ((*K)[v] == lb) lb_idx = i; 138 | } 139 | 140 | #pragma omp parallel for schedule(dynamic) \ 141 | shared(G, X, mc, C_max) private(i, v, P, mc_prev, mc_cur, C) firstprivate(ind) 142 | for (i = lb_idx; i <= G.num_vertices()-1; i++) { 143 | 144 | v = (*order)[i]; 145 | mc_prev = mc_cur = mc; 146 | 147 | if ((*K)[v] > mc_cur) { 148 | for (long long j = (*V)[v]; j < (*V)[v + 1]; j++) 149 | if ((*K)[(*E)[j]] > mc_cur) 150 | P.push_back( Vertex((*E)[j], compute_heuristic((*E)[j])) ); 151 | 152 | if (P.size() > mc_cur) { 153 | std::sort(P.begin(), P.end(), incr_heur); 154 | branch(P, 1 , mc_cur, C, ind); 155 | 156 | if (mc_cur > mc_prev) { 157 | if (mc < mc_cur) { 158 | #pragma omp critical 159 | if (mc < mc_cur) { 160 | mc = mc_cur; 161 | C.push_back(v); 162 | C_max = C; 163 | // print_info(C_max); 164 | } 165 | } 166 | } 167 | } 168 | } 169 | C = X; P = T; 170 | } 171 | C.clear(); 172 | cout << "[search_cores]\t mc = " << mc <& C_max) { 178 | return search_bounds(G, C_max); 179 | } 180 | 181 | 182 | inline void pmc_heu::print_info(vector C_max) { 183 | cout << "*** [pmc heuristic: thread " << omp_get_thread_num() + 1; 184 | cout << "] current max clique = " << C_max.size(); 185 | cout << ", time = " << get_time() - sec << " sec" < 29 | 30 | namespace pmc { 31 | 32 | class pmc_heu { 33 | public: 34 | vector* E; 35 | vector* V; 36 | vector* K; 37 | vector* order; 38 | vector* degree; 39 | double sec; 40 | int ub; 41 | string strat; 42 | 43 | pmc_heu(pmc_graph& G, input& params) { 44 | K = G.get_kcores(); 45 | order = G.get_kcore_ordering(); 46 | ub = params.ub; 47 | strat = params.heu_strat; 48 | initialize(); 49 | } 50 | 51 | pmc_heu(pmc_graph& G, int tmp_ub) { 52 | K = G.get_kcores(); 53 | order = G.get_kcore_ordering(); 54 | ub = tmp_ub; 55 | strat = "kcore"; 56 | initialize(); 57 | } 58 | 59 | inline void initialize() { 60 | sec = get_time(); 61 | srand (time(NULL)); 62 | }; 63 | 64 | int strategy(vector& P); 65 | void set_strategy(string s) { strat = s; } 66 | int compute_heuristic(int v); 67 | 68 | static bool desc_heur(Vertex v, Vertex u) { 69 | return (v.get_bound() > u.get_bound()); 70 | } 71 | 72 | static bool incr_heur(Vertex v, Vertex u) { 73 | return (v.get_bound() < u.get_bound()); 74 | } 75 | 76 | int search(pmc_graph& graph, vector& C_max); 77 | int search_cores(pmc_graph& graph, vector& C_max, int lb); 78 | int search_bounds(pmc_graph& graph, vector& C_max); 79 | 80 | inline void branch(vector& P, int sz, 81 | int& mc, vector& C, vector& ind); 82 | 83 | inline void print_info(vector C_max); 84 | }; 85 | }; 86 | #endif 87 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_input.h: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #ifndef PMC_INPUT_H_ 21 | #define PMC_INPUT_H_ 22 | 23 | #include "pmc_headers.h" 24 | #include "pmc_utils.h" 25 | 26 | using namespace std; 27 | 28 | namespace pmc { 29 | 30 | class input { 31 | public: 32 | // instance variables 33 | int algorithm; 34 | int threads; 35 | int experiment; 36 | int lb; 37 | int ub; 38 | int param_ub; 39 | int adj_limit; 40 | double time_limit; 41 | double remove_time; 42 | bool graph_stats; 43 | bool verbose; 44 | bool help; 45 | bool MCE; 46 | bool decreasing_order; 47 | string heu_strat; 48 | string format; 49 | string graph; 50 | string output; 51 | string edge_sorter; 52 | string vertex_search_order; 53 | 54 | input() { 55 | // default values 56 | algorithm = 0; 57 | threads = omp_get_max_threads(); 58 | experiment = 0; 59 | lb = 0; 60 | ub = 0; 61 | param_ub = 0; 62 | adj_limit = 20000; 63 | time_limit = 60 * 60; // max time to search 64 | remove_time = 4.0; // time to wait before reducing graph 65 | verbose = false; 66 | graph_stats = false; 67 | help = false; 68 | MCE = false; 69 | decreasing_order = false; 70 | heu_strat = "kcore"; 71 | vertex_search_order = "deg"; 72 | format = "mtx"; 73 | graph = "data/sample.mtx"; 74 | output = ""; 75 | string edge_sorter = ""; 76 | 77 | // both off, use default alg 78 | if (heu_strat == "0" && algorithm == -1) 79 | algorithm = 0; 80 | 81 | if (threads <= 0) threads = 1; 82 | } 83 | 84 | input(int argc, char *argv[]) { 85 | // default values 86 | algorithm = 0; 87 | threads = omp_get_max_threads(); 88 | experiment = 0; 89 | lb = 0; 90 | ub = 0; 91 | param_ub = 0; 92 | adj_limit = 20000; 93 | time_limit = 60 * 60; // max time to search 94 | remove_time = 4.0; // time to wait before reducing graph 95 | verbose = false; 96 | graph_stats = false; 97 | help = false; 98 | MCE = false; 99 | decreasing_order = false; 100 | heu_strat = "kcore"; 101 | vertex_search_order = "deg"; 102 | format = "mtx"; 103 | graph = "data/sample.mtx"; 104 | output = ""; 105 | string edge_sorter = ""; 106 | 107 | int opt; 108 | while ((opt=getopt(argc,argv,"i:t:f:u:l:o:e:a:r:w:h:k:dgsv")) != EOF) { 109 | switch (opt) { 110 | case 'a': 111 | algorithm = atoi(optarg); 112 | if (algorithm > 9) MCE = true; 113 | break; 114 | case 't': 115 | threads = atoi(optarg); 116 | break; 117 | case 'f': 118 | graph = optarg; 119 | break; 120 | case 's': 121 | graph_stats = true; 122 | break; 123 | case 'u': 124 | param_ub = atoi(optarg); // find k-clique fast 125 | lb = 2; // skip heuristic 126 | break; 127 | case 'k': 128 | param_ub = atoi(optarg); 129 | lb = param_ub-1; 130 | break; 131 | case 'l': 132 | lb = atoi(optarg); 133 | break; 134 | case 'h': 135 | heu_strat = optarg; 136 | break; 137 | case 'v': 138 | verbose = 1; 139 | break; 140 | case 'w': 141 | time_limit = atof(optarg) * 60; // convert minutes to seconds 142 | break; 143 | case 'r': 144 | remove_time = atof(optarg); 145 | break; 146 | case 'e': 147 | edge_sorter = optarg; 148 | break; 149 | case 'o': 150 | vertex_search_order = optarg; 151 | break; 152 | case 'd': 153 | // direction of which vertices are ordered 154 | decreasing_order = true; 155 | break; 156 | case '?': 157 | usage(argv[0]); 158 | break; 159 | default: 160 | usage(argv[0]); 161 | break; 162 | } 163 | } 164 | 165 | // both off, use default alg 166 | if (heu_strat == "0" && algorithm == -1) 167 | algorithm = 0; 168 | 169 | if (threads <= 0) threads = 1; 170 | 171 | if (!fexists(graph.c_str())) { 172 | usage(argv[0]); 173 | exit(-1); 174 | } 175 | 176 | FILE* fin = fopen(graph.c_str(), "r+t"); 177 | if (fin == NULL) { 178 | usage(argv[0]); 179 | exit(-1); 180 | } 181 | fclose(fin); 182 | 183 | cout << "\n\nFile Name ------------------------ " << graph.c_str() << endl; 184 | if (!fexists(graph.c_str()) ) { 185 | cout << "File not found!" << endl; 186 | return; 187 | } 188 | cout << "workers: " << threads < 1000000000 && in.remove_time < 120) in.remove_time = 120; 36 | else if (G.num_edges() > 250000000 && in.remove_time < 10) in.remove_time = 10; 37 | cout << "explicit reduce is set to " << in.remove_time << " seconds" < C; 49 | if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input 50 | pmc_heu maxclique(G,in); 51 | in.lb = maxclique.search(G, C); 52 | cout << "Heuristic found clique of size " << in.lb; 53 | cout << " in " << get_time() - seconds << " seconds" <= 0) { 63 | switch(in.algorithm) { 64 | case 0: { 65 | //! k-core pruning, neigh-core pruning/ordering, dynamic coloring bounds/sort 66 | if (G.num_vertices() < in.adj_limit) { 67 | G.create_adj(); 68 | pmcx_maxclique finder(G,in); 69 | finder.search_dense(G,C); 70 | break; 71 | } 72 | else { 73 | pmcx_maxclique finder(G,in); 74 | finder.search(G,C); 75 | break; 76 | } 77 | } 78 | case 1: { 79 | //! k-core pruning, dynamic coloring bounds/sort 80 | if (G.num_vertices() < in.adj_limit) { 81 | G.create_adj(); 82 | pmcx_maxclique_basic finder(G,in); 83 | finder.search_dense(G,C); 84 | break; 85 | } 86 | else { 87 | pmcx_maxclique_basic finder(G,in); 88 | finder.search(G,C); 89 | break; 90 | } 91 | } 92 | case 2: { 93 | //! simple k-core pruning (four new pruning steps) 94 | pmc_maxclique finder(G,in); 95 | finder.search(G,C); 96 | break; 97 | } 98 | default: 99 | cout << "algorithm " << in.algorithm << " not found." < 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "pmc_headers.h" 29 | #include "pmc_utils.h" 30 | #include "pmc_graph.h" 31 | #include "pmc_input.h" 32 | #include "pmc_vertex.h" 33 | 34 | using namespace std; 35 | 36 | namespace pmc { 37 | 38 | class pmc_maxclique { 39 | public: 40 | vector* edges; 41 | vector* vertices; 42 | vector* bound; 43 | vector* order; 44 | vector* degree; 45 | int param_ub; 46 | int ub; 47 | int lb; 48 | double time_limit; 49 | double sec; 50 | double wait_time; 51 | bool not_reached_ub; 52 | bool time_expired_msg; 53 | bool decr_order; 54 | 55 | string vertex_ordering; 56 | int edge_ordering; 57 | int style_bounds; 58 | int style_dynamic_bounds; 59 | 60 | int num_threads; 61 | 62 | void initialize() { 63 | vertex_ordering = "kcore"; 64 | edge_ordering = 0; 65 | style_bounds = 0; 66 | style_dynamic_bounds = 0; 67 | not_reached_ub = true; 68 | time_expired_msg = true; 69 | decr_order = false; 70 | } 71 | 72 | void setup_bounds(input& params) { 73 | lb = params.lb; 74 | ub = params.ub; 75 | param_ub = params.param_ub; 76 | if (param_ub == 0) 77 | param_ub = ub; 78 | time_limit = params.time_limit; 79 | wait_time = params.remove_time; 80 | sec = get_time(); 81 | 82 | num_threads = params.threads; 83 | } 84 | 85 | 86 | pmc_maxclique(pmc_graph& G, input& params) { 87 | bound = G.get_kcores(); 88 | order = G.get_kcore_ordering(); 89 | setup_bounds(params); 90 | initialize(); 91 | vertex_ordering = params.vertex_search_order; 92 | decr_order = params.decreasing_order; 93 | } 94 | 95 | ~pmc_maxclique() {}; 96 | 97 | int search(pmc_graph& G, vector& sol); 98 | 99 | void branch( 100 | vector &P, 101 | vector& ind, 102 | vector& C, 103 | vector& C_max, 104 | int* &pruned, 105 | int& mc); 106 | 107 | 108 | int search_dense(pmc_graph& G, vector& sol); 109 | 110 | void branch_dense( 111 | vector &P, 112 | vector& ind, 113 | vector& C, 114 | vector& C_max, 115 | int* &pruned, 116 | int& mc, 117 | bool** &adj); 118 | 119 | }; 120 | }; 121 | 122 | #endif 123 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_neigh_coloring.h: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #ifndef PMC_NEIGH_COLORING_H_ 21 | #define PMC_NEIGH_COLORING_H_ 22 | 23 | #include "pmc_vertex.h" 24 | 25 | using namespace std; 26 | 27 | namespace pmc { 28 | 29 | // sequential dynamic greedy coloring and sort 30 | static void neigh_coloring_bound( 31 | vector& vs, 32 | vector& es, 33 | vector &P, 34 | vector& ind, 35 | vector& C, 36 | vector& C_max, 37 | vector< vector >& colors, 38 | int* pruned, 39 | int& mc) { 40 | 41 | int j = 0, u = 0, k = 1, k_prev = 0; 42 | int max_k = 1; 43 | int min_k = mc - C.size() + 1; 44 | colors[1].clear(); colors[2].clear(); 45 | 46 | for (int w=0; w < P.size(); w++) { 47 | u = P[w].get_id(); 48 | k = 1, k_prev = 0; 49 | 50 | for (long long h = vs[u]; h < vs[u + 1]; h++) ind[es[h]] = 1; 51 | 52 | while (k > k_prev) { 53 | k_prev = k; 54 | for (int i = 0; i < colors[k].size(); i++) { 55 | if (ind[colors[k][i]]) { 56 | k++; 57 | break; 58 | } 59 | } 60 | } 61 | 62 | for (long long h = vs[u]; h < vs[u + 1]; h++) ind[es[h]] = 0; 63 | 64 | if (k > max_k) { 65 | max_k = k; 66 | colors[max_k+1].clear(); 67 | } 68 | 69 | colors[k].push_back(u); 70 | if (k < min_k) { 71 | P[j].set_id(u); 72 | j++; 73 | } 74 | } 75 | 76 | if (j > 0) P[j-1].set_bound(0); 77 | if (min_k <= 0) min_k = 1; 78 | 79 | for (k = min_k; k <= max_k; k++) 80 | for (int w = 0; w < colors[k].size(); w++) { 81 | P[j].set_id(colors[k][w]); 82 | P[j].set_bound(k); 83 | j++; 84 | } 85 | } 86 | 87 | // sequential dynamic greedy coloring and sort 88 | static void neigh_coloring_dense( 89 | vector& vs, 90 | vector& es, 91 | vector &P, 92 | vector& ind, 93 | vector& C, 94 | vector& C_max, 95 | vector< vector >& colors, 96 | int& mc, 97 | bool** &adj) { 98 | 99 | int j = 0, u = 0, k = 1, k_prev = 0; 100 | int max_k = 1; 101 | int min_k = mc - C.size() + 1; 102 | 103 | colors[1].clear(); colors[2].clear(); 104 | 105 | for (int w=0; w < P.size(); w++) { 106 | u = P[w].get_id(); 107 | k = 1, k_prev = 0; 108 | 109 | while (k > k_prev) { 110 | k_prev = k; 111 | for (int i = 0; i < colors[k].size(); i++) { //use directly, sort makes it fast! 112 | if (adj[u][colors[k][i]]) { 113 | k++; 114 | break; 115 | } 116 | } 117 | } 118 | 119 | if (k > max_k) { 120 | max_k = k; 121 | colors[max_k+1].clear(); 122 | } 123 | 124 | colors[k].push_back(u); 125 | if (k < min_k) { 126 | P[j].set_id(u); 127 | j++; 128 | } 129 | } 130 | 131 | if (j > 0) P[j-1].set_bound(0); 132 | if (min_k <= 0) min_k = 1; 133 | 134 | for (k = min_k; k <= max_k; k++) 135 | for (int w = 0; w < colors[k].size(); w++) { 136 | P[j].set_id(colors[k][w]); 137 | P[j].set_bound(k); 138 | j++; 139 | } 140 | } 141 | } 142 | #endif 143 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_utils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #include "pmc_utils.h" 21 | 22 | using namespace std; 23 | 24 | bool fexists(const char *filename) { 25 | ifstream ifile(filename); 26 | return ifile.is_open(); 27 | } 28 | 29 | void usage(char *argv0) { 30 | const char *params = 31 | "Usage: %s -a alg -f graphfile -t threads -o ordering -h heu_strat -u upper_bound -l lower_bound -r reduce_wait_time -w time_limit \n" 32 | "\t-a algorithm : Algorithm for solving MAX-CLIQUE: 0 = full, 1 = no neighborhood cores, 2 = only basic k-core pruning steps \n" 33 | "\t-f graph file : Input GRAPH file for computing the Maximum Clique (matrix market format or simple edge list). \n" 34 | "\t-o vertex search ordering : Order in which vertices are searched (default = deg, [kcore, dual_deg, dual_kcore, kcore_deg, rand]) \n" 35 | "\t-d decreasing order : Search vertices in DECREASING order. Note if '-d' is not set, then vertices are searched in increasing order by default. \n" 36 | "\t-e neigh/edge ordering : Ordering of neighbors/edges (default = deg, [kcore, dual_deg, dual_kcore, kcore_deg, rand]) \n" 37 | "\t-h heuristic strategy : Strategy for HEURISTIC method (default = kcore, [deg, dual_deg, dual_kcore, rand, 0 = skip heuristic]) \n" 38 | "\t-u upper_bound : UPPER-BOUND on clique size (default = K-cores).\n" 39 | "\t-l lower_bound : LOWER-BOUND on clique size (default = Estimate using the Fast Heuristic). \n" 40 | "\t-t threads : Number of THREADS for the algorithm to use (default = 1). \n" 41 | "\t-r reduce_wait : Number of SECONDS to wait before inducing the graph based on the unpruned vertices (default = 4 seconds). \n" 42 | "\t-w time_limit : Execution TIME LIMIT spent searching for max clique (default = 7 days) \n" 43 | "\t-k clique size : Solve K-CLIQUE problem: find clique of size k if it exists. Parameterized to be fast. \n" 44 | "\t-s stats : Compute BOUNDS and other fast graph stats \n" 45 | "\t-v verbose : Output additional details to the screen. \n" 46 | "\t-? options : Print out this help menu. \n"; 47 | fprintf(stderr, params, argv0); 48 | exit(-1); 49 | } 50 | 51 | 52 | double get_time() { 53 | timeval t; 54 | gettimeofday(&t, NULL); 55 | return t.tv_sec*1.0 + t.tv_usec/1000000.0; 56 | } 57 | 58 | string memory_usage() { 59 | ostringstream mem; 60 | ifstream proc("/proc/self/status"); 61 | string s; 62 | while(getline(proc, s), !proc.fail()) { 63 | if(s.substr(0, 6) == "VmSize") { 64 | mem << s; 65 | return mem.str(); 66 | } 67 | } 68 | return mem.str(); 69 | } 70 | 71 | void indent(int level, string str) { 72 | for (int i = 0; i < level; i++) 73 | cout << " "; 74 | cout << "(" << level << ") "; 75 | } 76 | 77 | void print_max_clique(vector& C) { 78 | cout << "Maximum clique: "; 79 | for(int i = 0; i < C.size(); i++) 80 | cout << C[i] + 1 << " "; 81 | cout << endl; 82 | } 83 | 84 | void print_n_maxcliques(set< vector > C, int n) { 85 | set< vector >::iterator it; 86 | int mc = 0; 87 | for( it = C.begin(); it != C.end(); it++) { 88 | if (mc < n) { 89 | cout << "Maximum clique: "; 90 | const vector& clq = (*it); 91 | for (int j = 0; j < clq.size(); j++) 92 | cout << clq[j] << " "; 93 | cout < &files) { 109 | DIR *dp; 110 | struct dirent *dirp; 111 | if((dp = opendir(dir.c_str())) == NULL) { 112 | cout << "Error(" << errno << ") opening " << dir << endl; 113 | return errno; 114 | } 115 | 116 | while ((dirp = readdir(dp)) != NULL) { 117 | if (dirp->d_name != ".") 118 | files.push_back(string(dirp->d_name)); 119 | } 120 | closedir(dp); 121 | return 0; 122 | } 123 | 124 | 125 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #ifndef PMC_UTILS_H_ 21 | #define PMC_UTILS_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include "assert.h" 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include "pmc_headers.h" 34 | 35 | 36 | using namespace std; 37 | 38 | bool fexists(const char *filename); 39 | void usage(char *argv0); 40 | 41 | double get_time(); 42 | string memory_usage(); 43 | 44 | void validate(bool condition, const string& msg); 45 | 46 | void indent(int level); 47 | void indent(int level, string str); 48 | void print_max_clique(vector& max_clique_data); 49 | void print_n_maxcliques(set< vector > C, int n); 50 | 51 | int getdir (string dir, vector &files); 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmc_vertex.h: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #ifndef PMC_VERTEX_H_ 21 | #define PMC_VERTEX_H_ 22 | 23 | using namespace std; 24 | 25 | namespace pmc { 26 | class Vertex { 27 | private: 28 | int id, b; 29 | public: 30 | Vertex(int vertex_id, int bound): id(vertex_id), b(bound) {}; 31 | 32 | void set_id(int vid) { id = vid; } 33 | int get_id() { return id; } 34 | 35 | void set_bound(int value) { b = value; } 36 | int get_bound() { return b; } 37 | }; 38 | 39 | static bool decr_bound(Vertex v, Vertex u) { 40 | return (v.get_bound() > u.get_bound()); 41 | } 42 | static bool incr_bound(Vertex v, Vertex u) { 43 | return (v.get_bound() < u.get_bound()); 44 | }; 45 | 46 | inline static void print_mc_info(vector &C_max, double &sec) { 47 | cout << "*** [pmc: thread " << omp_get_thread_num() + 1; 48 | cout << "] current max clique = " << C_max.size(); 49 | cout << ", time = " << get_time() - sec << " sec" < 24 | #include 25 | #include 26 | #include 27 | #include "pmc_headers.h" 28 | #include "pmc_utils.h" 29 | #include "pmc_graph.h" 30 | #include "pmc_input.h" 31 | #include "pmc_vertex.h" 32 | #include "pmc_neigh_cores.h" 33 | #include "pmc_neigh_coloring.h" 34 | #include 35 | 36 | using namespace std; 37 | 38 | namespace pmc { 39 | 40 | class pmcx_maxclique { 41 | public: 42 | vector* edges; 43 | vector* vertices; 44 | vector* bound; 45 | vector* order; 46 | int param_ub; 47 | int ub; 48 | int lb; 49 | double time_limit; 50 | double sec; 51 | double wait_time; 52 | bool not_reached_ub; 53 | bool time_expired_msg; 54 | bool decr_order; 55 | 56 | string vertex_ordering; 57 | int edge_ordering; 58 | int style_bounds; 59 | int style_dynamic_bounds; 60 | 61 | int num_threads; 62 | 63 | void initialize() { 64 | vertex_ordering = "deg"; 65 | edge_ordering = 0; 66 | style_bounds = 0; 67 | style_dynamic_bounds = 0; 68 | not_reached_ub = true; 69 | time_expired_msg = true; 70 | decr_order = false; 71 | } 72 | 73 | void setup_bounds(input& params) { 74 | lb = params.lb; 75 | ub = params.ub; 76 | param_ub = params.param_ub; 77 | if (param_ub == 0) 78 | param_ub = ub; 79 | time_limit = params.time_limit; 80 | wait_time = params.remove_time; 81 | sec = get_time(); 82 | 83 | num_threads = params.threads; 84 | } 85 | 86 | pmcx_maxclique(pmc_graph& G, input& params) { 87 | bound = G.get_kcores(); 88 | order = G.get_kcore_ordering(); 89 | setup_bounds(params); 90 | initialize(); 91 | vertex_ordering = params.vertex_search_order; 92 | decr_order = params.decreasing_order; 93 | } 94 | 95 | ~pmcx_maxclique() {}; 96 | 97 | int search(pmc_graph& G, vector& sol); 98 | inline void branch( 99 | vector& vs, 100 | vector& es, 101 | vector &P, 102 | vector& ind, 103 | vector& C, 104 | vector& C_max, 105 | vector< vector >& colors, 106 | int* &pruned, 107 | int& mc); 108 | 109 | int search_dense(pmc_graph& G, vector& sol); 110 | inline void branch_dense( 111 | vector& vs, 112 | vector& es, 113 | vector &P, 114 | vector& ind, 115 | vector& C, 116 | vector& C_max, 117 | vector< vector >& colors, 118 | int* &pruned, 119 | int& mc, 120 | bool** &adj); 121 | 122 | }; 123 | }; 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /thirdparty/pmc/pmcx_maxclique_basic.h: -------------------------------------------------------------------------------- 1 | /** 2 | ============================================================================ 3 | Name : Parallel Maximum Clique (PMC) Library 4 | Author : Ryan A. Rossi (rrossi@purdue.edu) 5 | Description : A general high-performance parallel framework for computing 6 | maximum cliques. The library is designed to be fast for large 7 | sparse graphs. 8 | 9 | Copyright (C) 2012-2013, Ryan A. Rossi, All rights reserved. 10 | 11 | Please cite the following paper if used: 12 | Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa 13 | Patwary, A Fast Parallel Maximum Clique Algorithm for Large Sparse Graphs 14 | and Temporal Strong Components, arXiv preprint 1302.6256, 2013. 15 | 16 | See http://ryanrossi.com/pmc for more information. 17 | ============================================================================ 18 | */ 19 | 20 | #ifndef PMCX_MAXCLIQUE_BASIC_H_ 21 | #define PMCX_MAXCLIQUE_BASIC_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "pmc_headers.h" 29 | #include "pmc_utils.h" 30 | #include "pmc_graph.h" 31 | #include "pmc_input.h" 32 | #include "pmc_vertex.h" 33 | #include "pmc_neigh_cores.h" 34 | #include "pmc_neigh_coloring.h" 35 | 36 | using namespace std; 37 | 38 | namespace pmc { 39 | 40 | class pmcx_maxclique_basic { 41 | public: 42 | vector* edges; 43 | vector* vertices; 44 | vector* bound; 45 | vector* order; 46 | vector* degree; 47 | int param_ub; 48 | int ub; 49 | int lb; 50 | double time_limit; 51 | double sec; 52 | double wait_time; 53 | bool not_reached_ub; 54 | bool time_expired_msg; 55 | bool decr_order; 56 | 57 | string vertex_ordering; 58 | int edge_ordering; 59 | int style_bounds; 60 | int style_dynamic_bounds; 61 | 62 | int num_threads; 63 | 64 | void initialize() { 65 | vertex_ordering = "deg"; 66 | edge_ordering = 0; 67 | style_bounds = 0; 68 | style_dynamic_bounds = 0; 69 | not_reached_ub = true; 70 | time_expired_msg = true; 71 | decr_order = false; 72 | } 73 | 74 | void setup_bounds(input& params) { 75 | lb = params.lb; 76 | ub = params.ub; 77 | param_ub = params.param_ub; 78 | if (param_ub == 0) 79 | param_ub = ub; 80 | time_limit = params.time_limit; 81 | wait_time = params.remove_time; 82 | sec = get_time(); 83 | 84 | num_threads = params.threads; 85 | } 86 | 87 | 88 | pmcx_maxclique_basic(pmc_graph& G, input& params) { 89 | bound = G.get_kcores(); 90 | order = G.get_kcore_ordering(); 91 | setup_bounds(params); 92 | initialize(); 93 | vertex_ordering = params.vertex_search_order; 94 | decr_order = params.decreasing_order; 95 | } 96 | 97 | ~pmcx_maxclique_basic() {}; 98 | 99 | int search(pmc_graph& G, vector& sol); 100 | 101 | void branch( 102 | vector& vs, 103 | vector& es, 104 | vector &P, 105 | vector& ind, 106 | vector& C, 107 | vector& C_max, 108 | vector< vector >& colors, 109 | int* &pruned, 110 | int& mc); 111 | 112 | 113 | int search_dense(pmc_graph& G, vector& sol); 114 | 115 | void branch_dense( 116 | vector& vs, 117 | vector& es, 118 | vector &P, 119 | vector& ind, 120 | vector& C, 121 | vector& C_max, 122 | vector< vector >& colors, 123 | int* &pruned, 124 | int& mc, 125 | bool** &adj); 126 | 127 | }; 128 | }; 129 | 130 | #endif 131 | --------------------------------------------------------------------------------