├── 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 |
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