├── .gitignore ├── .gitmodules ├── README.md ├── sample_code_cpp ├── README.md ├── data │ ├── corridor.lsc │ └── hall.lsc ├── framework.cfg ├── images │ ├── dependency_graph.png │ ├── odom.png │ └── slam.png └── src │ ├── CMakeLists.txt │ ├── cxxopts │ └── cxxopts.hpp │ ├── launcher │ ├── CMakeLists.txt │ ├── FrameworkFactory │ │ ├── CostFunctionFactory │ │ │ └── CostFunctionFactory.h │ │ ├── DataAssociatorFactory │ │ │ └── DataAssociatorFactory.h │ │ ├── FrameworkFactory.cpp │ │ ├── FrameworkFactory.h │ │ ├── PointCloudMapFactory │ │ │ └── PointCloudMapFactory.h │ │ ├── PoseEstimatorFactory │ │ │ └── PoseEstimatorFactory.h │ │ ├── PoseOptimizerFactory │ │ │ └── PoseOptimizerFactory.h │ │ └── ReferenceScanMakerFactory │ │ │ └── ReferenceScanMakerFactory.h │ ├── MapDrawer │ │ ├── MapDrawer.cpp │ │ └── MapDrawer.h │ ├── SensorDataReader │ │ ├── SensorDataReader.cpp │ │ └── SensorDataReader.h │ └── SlamLauncher │ │ ├── SlamLauncher.cpp │ │ └── SlamLauncher.h │ ├── main.cpp │ ├── slam │ ├── CMakeLists.txt │ ├── CostFunction │ │ ├── CostFunction.h │ │ ├── CostFunction_EuclideanDistance.cpp │ │ ├── CostFunction_EuclideanDistance.h │ │ ├── CostFunction_PerpendicularDistance.cpp │ │ └── CostFunction_PerpendicularDistance.h │ ├── CovarianceCalculator │ │ ├── CovarianceCalculator.cpp │ │ └── CovarianceCalculator.h │ ├── DataAssociator │ │ ├── DataAssociator.h │ │ ├── DataAssociator_GridTable.cpp │ │ ├── DataAssociator_GridTable.h │ │ ├── DataAssociator_LinearSearch.cpp │ │ └── DataAssociator_LinearSearch.h │ ├── LoopDetector │ │ ├── LoopDetector.cpp │ │ └── LoopDetector.h │ ├── PointCloudMap │ │ ├── PointCloudMap.h │ │ ├── PointCloudMap_EntirePoint.cpp │ │ ├── PointCloudMap_EntirePoint.h │ │ ├── PointCloudMap_GridTable.cpp │ │ ├── PointCloudMap_GridTable.h │ │ ├── PointCloudMap_SubMap.cpp │ │ └── PointCloudMap_SubMap.h │ ├── PoseEstimator │ │ ├── PoseEstimator.h │ │ ├── PoseEstimator_ICP.cpp │ │ ├── PoseEstimator_ICP.h │ │ ├── PoseEstimator_NDT.cpp │ │ └── PoseEstimator_NDT.h │ ├── PoseGraph │ │ ├── PoseGraph.cpp │ │ └── PoseGraph.h │ ├── PoseOptimizer │ │ ├── PoseOptimizer.h │ │ ├── PoseOptimizer_GradientDescent.cpp │ │ ├── PoseOptimizer_GradientDescent.h │ │ ├── PoseOptimizer_LineSearch.cpp │ │ └── PoseOptimizer_LineSearch.h │ ├── ReferenceScanMaker │ │ ├── ReferenceScanMaker.h │ │ ├── ReferenceScanMaker_LastScan.cpp │ │ ├── ReferenceScanMaker_LastScan.h │ │ ├── ReferenceScanMaker_LocalMap.cpp │ │ └── ReferenceScanMaker_LocalMap.h │ ├── ScanMatcher │ │ ├── ScanMatcher.cpp │ │ └── ScanMatcher.h │ ├── ScanPreprocessor │ │ ├── ScanPreprocessor.cpp │ │ └── ScanPreprocessor.h │ ├── SlamBackend │ │ ├── SlamBackend.cpp │ │ └── SlamBackend.h │ └── SlamFrontend │ │ ├── SlamFrontend.cpp │ │ └── SlamFrontend.h │ ├── solver │ └── p2o │ │ ├── .gitignore │ │ ├── LICENSE │ │ ├── README.md │ │ ├── p2o.h │ │ └── samples │ │ ├── CMakeLists.txt │ │ ├── build_and_run_p2o.sh │ │ ├── plot_data.plt │ │ └── test_p2o.cpp │ ├── test │ ├── launcher │ │ └── CMakeLists.txt │ ├── slam │ │ └── CMakeLists.txt │ └── utility │ │ ├── CMakeLists.txt │ │ ├── LaserPoint2D_test.cpp │ │ ├── MyUtility_test.cpp │ │ └── Pose2D_test.cpp │ └── utility │ ├── CMakeLists.txt │ ├── GridTable2D.h │ ├── LaserPoint2D.h │ ├── MyUtility │ ├── MyUtility.cpp │ └── MyUtility.h │ ├── Pose2D.h │ └── Scan2D.h ├── sample_code_python ├── data │ ├── killian-e.dat │ ├── killian-small-rot-inf-e.dat │ ├── killian-v.dat │ └── mit-e.dat ├── graph_based_slam.ipynb └── images │ ├── graph_based_slam1.png │ ├── graph_based_slam2.png │ └── graph_based_slam3.png └── tex ├── Graph-Based-SLAM_english.pdf ├── Graph-Based-SLAM_english.tex ├── Graph-Based-SLAM_japanese.pdf ├── Graph-Based-SLAM_japanese.tex └── images ├── 1-1_error_between_edges.png ├── 1-1_error_between_edges.xbb ├── 2-1_coordinate.png ├── 2-1_coordinate.xbb ├── 2-2_rotation_matrix.png ├── 2-2_rotation_matrix.xbb ├── 2-3_pose_representation_matrix.png ├── 2-3_pose_representation_matrix.xbb ├── 2-4_pose_representation_matrix_inverse.png ├── 2-4_pose_representation_matrix_inverse.xbb ├── 2-5_observation_representation_matrix.png ├── 2-5_observation_representation_matrix.xbb ├── 2-6_rotation_matrix.png ├── 2-6_rotation_matrix.xbb ├── 2-7_sensor_model.png ├── 2-7_sensor_model.xbb ├── 3-1_difference_between_nodes.png ├── 3-1_difference_between_nodes.xbb ├── 3-2_error_between_edges.png ├── 3-2_error_between_edges.xbb ├── 4-1_by_deleji_1.png ├── 4-1_by_deleji_1.xbb ├── 4-2_by_deleji_2.png ├── 4-2_by_deleji_2.xbb ├── 4-3_by_deleji_3.png ├── 4-3_by_deleji_3.xbb ├── Graph-Based-SLAM_english-page-001.jpg ├── Graph-Based-SLAM_english-page-003.jpg ├── Graph-Based-SLAM_english-page-005.jpg └── Graph-Based-SLAM_english-page-007.jpg /.gitignore: -------------------------------------------------------------------------------- 1 | sample_code_cpp/src/build/ 2 | sample_code_cpp/.vscode/ 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "sample_code_cpp/src/cmake/DownloadProject"] 2 | path = sample_code_cpp/src/cmake/DownloadProject 3 | url = https://github.com/Crascit/DownloadProject.git 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Graph Based SLAM 2 | 3 | Explanation of Graph based SLAM with C++ and Python sample code 4 | 5 | 6 | ## Explanation 7 | 8 | [tex/Graph-Based-SLAM_english.pdf](./tex/Graph-Based-SLAM_english.pdf) 9 | 10 | | | | | 11 | | :---: | :---: | :---: | 12 | | ![Graph-Based-SLAM_english-page-003.jpg](./tex/images/Graph-Based-SLAM_english-page-003.jpg) | ![Graph-Based-SLAM_english-page-005.jpg](./tex/images/Graph-Based-SLAM_english-page-005.jpg) | ![Graph-Based-SLAM_english-page-007.jpg](./tex/images/Graph-Based-SLAM_english-page-007.jpg) | 13 | 14 | 15 | ## Sample code [C++] 16 | 17 | [sample_code_cpp](./sample_code_cpp/) 18 | 19 | | Odometry only | Sample SLAM | 20 | | :---: | :---: | 21 | | ![odom.png](./sample_code_cpp/images/odom.png) | ![slam.png](./sample_code_cpp/images/slam.png) | 22 | 23 | 24 | ## Sample code [Python] 25 | 26 | [sample_code_python](./sample_code_python/) 27 | 28 | | | | | 29 | | :---: | :---: | :---: | 30 | | ![graph_based_slam1.png](./sample_code_python/images/graph_based_slam1.png) | ![graph_based_slam2.png](./sample_code_python/images/graph_based_slam2.png) | ![graph_based_slam2.png](./sample_code_python/images/graph_based_slam3.png) | -------------------------------------------------------------------------------- /sample_code_cpp/README.md: -------------------------------------------------------------------------------- 1 | # Sample SLAM 2 | 3 | Scan matching as a front-end, Graph based SLAM as a back-end. 4 | Inspired by [furo-org/LittleSLAM](https://github.com/furo-org/LittleSLAM). 5 | 6 | | Odometry only | Sample SLAM | 7 | | :---: | :---: | 8 | | ![odom.png](./images/odom.png) | ![slam.png](./images/slam.png) | 9 | 10 | The framework can be customized in `framework.cfg` by Factory Method as shown below: 11 | 12 | | Module | Type | Description | 13 | | --- | --- | --- | 14 | | PointCloudMap | _EntirePoint | Stores entire laser point | 15 | | | _GridTable | Stores laser points in a grid based manner | 16 | | | _SubMap | Stores laser points in a grid based manner with sub maps | 17 | | ReferenceScanMaker | _LastScan | Last scan as a reference scan for scan matching | 18 | | | _LocalMap | Local map as a reference scan for scan matching | 19 | | PoseEstimator | _ICP | Iterative Closest Point | 20 | | | _NDT | Normal Distribution Transform (WIP) | 21 | | DataAssociator | _LinearSearch | Linear search to find corresponding laser points | 22 | | | _GridTabel | Grid based search to find corresponding laser points | 23 | | PoseOptimizer | _GradientDescent | Gradient descent with a fixed step rate | 24 | | | _LineSearch | Gradient descent with a dynamic step rate determined by line search | 25 | | CostFunction | _EuclideanDistance | Euclidean distance as an error distance | 26 | | | _PerpendicularDistance | Perpendicular distance as an error distance | 27 | 28 |
29 | 30 | ![dependency_graph.png](./images/dependency_graph.png) 31 | 32 | # Pre-requirements 33 | 34 | * Boost 35 | * CMake 36 | * Eigen 37 | * gnuplot 38 | 39 | ## Install on Ubuntu 40 | 41 | ``` 42 | $ sudo apt-get install build-essential libboost-all-dev cmake libeigen3-dev gnuplot gnuplot-x11 43 | ``` 44 | 45 | ## Install on Mac 46 | 47 | ``` 48 | $ brew install boost cmake eigen gnuplot 49 | ``` 50 | 51 | 52 | 53 | # How to build 54 | 55 | ``` 56 | $ git clone --recursive https://github.com/hotsuyuki/Graph-Based-SLAM.git 57 | $ cd Graph-Based-SLAM/ 58 | $ cd sample_code_cpp/src/ 59 | $ mkdir build 60 | $ cd build/ 61 | $ cmake .. 62 | $ cmake --build . 63 | ``` 64 | 65 | This command downloads [google/googletest](https://github.com/google/googletest) using [Crascit/DownloadProject](https://github.com/Crascit/DownloadProject) and produces an executable file named `main` in the `build` directory. 66 | 67 | 68 | 69 | # How to run 70 | 71 | The executable file `main` requires at least two arguments: 72 | 73 | * `data-file` ... path to the input scan data 74 | * `viz-mode` ... visualization mode: *scan* | *odom* | *slam* 75 | 76 | ## (a) Launch SLAM 77 | 78 | ``` 79 | $ ./main --data-file ../../data/corridor.lsc --viz-mode slam 80 | ``` 81 | or 82 | ``` 83 | $ ./main ../../data/corridor.lsc slam 84 | ``` 85 | 86 | ## (b) Odometry only 87 | 88 | ``` 89 | $ ./main --data-file ../../data/corridor.lsc --viz-mode odom 90 | ``` 91 | or 92 | ``` 93 | $ ./main ../../data/corridor.lsc odom 94 | ``` 95 | 96 | ## (c) Check scan data 97 | 98 | ``` 99 | $ ./main --data-file ../../data/corridor.lsc --viz-mode scan 100 | ``` 101 | or 102 | ``` 103 | $ ./main ../../data/corridor.lsc scan 104 | ``` 105 | 106 | The full arguments and options are shown below: 107 | 108 | ``` 109 | usage: 110 | main [OPTION...] data-file viz-mode 111 | 112 | positional arguments: 113 | data-file String: Path to sensor data file (.lsc) 114 | viz-mode String: Visualization mode ... scan | odom | slam 115 | 116 | options: 117 | -h, --help Print usage 118 | ``` 119 | 120 | 121 | 122 | # How to test 123 | 124 | The program can be tested with Google C++ Testing Framework. 125 | 126 | ``` 127 | $ cd Graph-Based-SLAM/ 128 | $ cd sample_code_cpp/src/build/ 129 | $ ./test/utility/utility_test 130 | ``` 131 | 132 | 133 | 134 | # References 135 | 136 | * [furo-org/LittleSLAM](https://github.com/furo-org/LittleSLAM) 137 | * [furo-org/p2o](https://github.com/furo-org/p2o/tree/768dee340de702a6eee61f7b0172a836d1e021ea) 138 | * [TadaoYamaoka/cxxopts/include/cxxopts.hpp](https://github.com/TadaoYamaoka/cxxopts/blob/master/include/cxxopts.hpp) -------------------------------------------------------------------------------- /sample_code_cpp/framework.cfg: -------------------------------------------------------------------------------- 1 | # Framework Config 2 | # Comment starts from '#' or ';' 3 | 4 | 5 | ## [point_cloud_map_type] : entire_point | grid_tabel | sub_map 6 | ### entire_point ... Stores entire laser point 7 | ### grid_tabel ... Stores laser points in a grid based manner 8 | ### sub_map ... Stores laser points in a grid based manner with sub maps 9 | ; point_cloud_map_type = entire_point 10 | ; point_cloud_map_type = grid_table 11 | point_cloud_map_type = sub_map 12 | 13 | 14 | ## [reference_scan_maker_type] : last_scan | local_map 15 | ### last_scan ... Last scan as a reference scan for scan matching 16 | ### local_map ... Local map as a reference scan for scan matching 17 | ; reference_scan_maker_type = last_scan 18 | reference_scan_maker_type = local_map 19 | 20 | 21 | ## [pose_estimator_type] : icp | ndt 22 | ### icp ... Iterative Closest Point 23 | ### ndt ... Normal Distribution Transform 24 | pose_estimator_type = icp 25 | ; pose_estimator_type = ndt 26 | 27 | 28 | ## [data_associator_type] : linear_search | grid_table 29 | ### linear_search ... Linear search to find corresponding laser points 30 | ### grid_tabel ... Grid based search to find corresponding laser points 31 | ; data_associator_type = linear_search 32 | data_associator_type = grid_table 33 | 34 | 35 | ## [pose_optimizer_type] : gradient_descent | line_search 36 | ### gradient_descent ... Gradient descent with a fixed step rate 37 | ### line_search ... Gradient descent with a dynamic step rate determined by line search 38 | pose_optimizer_type = gradient_descent 39 | ; pose_optimizer_type = line_search 40 | 41 | 42 | ## [cost_function_type] : euclidean_distance | euclidean_distance 43 | ### euclidean_distance ... Euclidean distance as an error distance 44 | ### perpendicular_distance ... Perpendicular distance as an error distance 45 | ; cost_function_type = euclidean_distance 46 | cost_function_type = perpendicular_distance 47 | 48 | 49 | ## [is_scan_preprocess] : 0 | 1 50 | ### 0 ... false 51 | ### 1 ... true 52 | ; is_scan_preprocess = 0 53 | is_scan_preprocess = 1 54 | 55 | 56 | ## [is_odometry_fusion] : 0 | 1 57 | ### 0 ... false 58 | ### 1 ... true 59 | ; is_odometry_fusion = 0 60 | is_odometry_fusion = 1 61 | 62 | 63 | ## [is_loop_closure] : 0 | 1 64 | ### 0 ... false 65 | ### 1 ... true 66 | ; is_loop_closure = 0 67 | is_loop_closure = 1 68 | 69 | ### NOTE: If is_loop_closure == 1, then some types are set as shown below. 70 | ### NOTE: point_cloud_map_type == sub_map 71 | ### NOTE: data_associator_type == grid_table 72 | ### NOTE: cost_function_type == perpendicular_distance -------------------------------------------------------------------------------- /sample_code_cpp/images/dependency_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_cpp/images/dependency_graph.png -------------------------------------------------------------------------------- /sample_code_cpp/images/odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_cpp/images/odom.png -------------------------------------------------------------------------------- /sample_code_cpp/images/slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_cpp/images/slam.png -------------------------------------------------------------------------------- /sample_code_cpp/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | project(sample_slam 4 | VERSION 1.0.0 5 | DESCRIPTION "Sample SLAM: Scan matching as a front-end, Graph based SLAM as a back-end" 6 | LANGUAGES CXX 7 | ) 8 | 9 | add_subdirectory(${CMAKE_SOURCE_DIR}/launcher/) 10 | add_subdirectory(${CMAKE_SOURCE_DIR}/slam/) 11 | add_subdirectory(${CMAKE_SOURCE_DIR}/utility/) 12 | 13 | enable_testing() 14 | # add_subdirectory(${CMAKE_SOURCE_DIR}/test/launcher/) 15 | # add_subdirectory(${CMAKE_SOURCE_DIR}/test/slam/) 16 | add_subdirectory(${CMAKE_SOURCE_DIR}/test/utility/) 17 | 18 | add_executable(main ${CMAKE_SOURCE_DIR}/main.cpp) 19 | target_compile_features(main PRIVATE cxx_std_11) 20 | 21 | target_include_directories(main PRIVATE 22 | ${CMAKE_SOURCE_DIR}/cxxopts/ 23 | ) 24 | 25 | target_link_libraries(main 26 | launcher 27 | ) -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | project(launcher_lib 4 | VERSION 1.0.0 5 | DESCRIPTION "Launcher library" 6 | LANGUAGES CXX 7 | ) 8 | 9 | add_library(launcher STATIC 10 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/FrameworkFactory.cpp 11 | ################ 12 | ${PROJECT_SOURCE_DIR}/MapDrawer/MapDrawer.cpp 13 | ${PROJECT_SOURCE_DIR}/SensorDataReader/SensorDataReader.cpp 14 | ${PROJECT_SOURCE_DIR}/SlamLauncher/SlamLauncher.cpp 15 | ) 16 | target_compile_features(launcher PRIVATE cxx_std_11) 17 | 18 | target_include_directories(launcher PUBLIC 19 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/ 20 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/CostFunctionFactory/ 21 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/DataAssociatorFactory/ 22 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/PointCloudMapFactory/ 23 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/PoseEstimatorFactory/ 24 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/PoseOptimizerFactory/ 25 | ${PROJECT_SOURCE_DIR}/FrameworkFactory/ReferenceScanMakerFactory/ 26 | ################ 27 | ${PROJECT_SOURCE_DIR}/MapDrawer/ 28 | ${PROJECT_SOURCE_DIR}/SensorDataReader/ 29 | ${PROJECT_SOURCE_DIR}/SlamLauncher/ 30 | ) 31 | 32 | target_link_libraries(launcher 33 | slam 34 | utility 35 | ) -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/CostFunctionFactory/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_COSTFUNCTIONFACTORY_H_ 2 | #define LAUNCHER_COSTFUNCTIONFACTORY_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "CostFunction.h" 10 | #include "CostFunction_EuclideanDistance.h" 11 | #include "CostFunction_PerpendicularDistance.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | enum class CostFunctionType { 17 | EUCLIDEAN_DISTANCE = 0, 18 | PERPENDICULAR_DISTANCE = 1, 19 | }; 20 | 21 | 22 | class CostFunctionFactory { 23 | public: 24 | CostFunctionFactory() {} 25 | 26 | ~CostFunctionFactory() {} 27 | 28 | static CostFunction* ProduceCostFunctionPtr(const std::string& chars) { 29 | std::map cost_function_type = { 30 | { CostFunctionType::EUCLIDEAN_DISTANCE, "euclidean_distance" }, 31 | { CostFunctionType::PERPENDICULAR_DISTANCE, "perpendicular_distance" }, 32 | }; 33 | 34 | if (chars == cost_function_type[CostFunctionType::EUCLIDEAN_DISTANCE]) { 35 | return new CostFunction_EuclideanDistance; 36 | } else if (chars == cost_function_type[CostFunctionType::PERPENDICULAR_DISTANCE]) { 37 | return new CostFunction_PerpendicularDistance; 38 | } else { 39 | std::cerr << "[CostFunctionFactory::ProduceCostFunctionPtr()] Error: Invalid input \n\n"; 40 | return nullptr; 41 | } 42 | } 43 | 44 | static CostFunction* ProduceCostFunctionPtr(const CostFunctionType& enumerate) { 45 | if (enumerate == CostFunctionType::EUCLIDEAN_DISTANCE) { 46 | return new CostFunction_EuclideanDistance; 47 | } else if (enumerate == CostFunctionType::PERPENDICULAR_DISTANCE) { 48 | return new CostFunction_PerpendicularDistance; 49 | } else { 50 | std::cerr << "[CostFunctionFactory::ProduceCostFunctionPtr()] Error: Invalid input \n\n"; 51 | return nullptr; 52 | } 53 | } 54 | }; 55 | 56 | } // namespace sample_slam 57 | 58 | #endif // LAUNCHER_COSTFUNCTIONFACTORY_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/DataAssociatorFactory/DataAssociatorFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_DATAASSOCIATORFACTORY_H_ 2 | #define LAUNCHER_DATAASSOCIATORFACTORY_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "DataAssociator.h" 10 | #include "DataAssociator_LinearSearch.h" 11 | #include "DataAssociator_GridTable.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | enum class DataAssociatorType { 17 | LINEAR_SEARCH = 0, 18 | GRID_TABLE = 1, 19 | }; 20 | 21 | 22 | class DataAssociatorFactory { 23 | public: 24 | DataAssociatorFactory() {} 25 | 26 | ~DataAssociatorFactory() {} 27 | 28 | static DataAssociator* ProduceDataAssociatorPtr(const std::string& chars) { 29 | std::map data_associator_type = { 30 | { DataAssociatorType::LINEAR_SEARCH, "linear_search" }, 31 | { DataAssociatorType::GRID_TABLE, "grid_table" }, 32 | }; 33 | 34 | if (chars == data_associator_type[DataAssociatorType::LINEAR_SEARCH]) { 35 | return new DataAssociator_LinearSearch; 36 | } else if (chars == data_associator_type[DataAssociatorType::GRID_TABLE]) { 37 | return new DataAssociator_GridTable; 38 | } else { 39 | std::cerr << "[DataAssociatorFactory::ProduceDataAssociatorPtr()] Error: Invalid input \n\n"; 40 | return nullptr; 41 | } 42 | } 43 | 44 | static DataAssociator* ProduceDataAssociatorPtr(const DataAssociatorType& enumerate) { 45 | if (enumerate == DataAssociatorType::LINEAR_SEARCH) { 46 | return new DataAssociator_LinearSearch; 47 | } else if (enumerate == DataAssociatorType::GRID_TABLE) { 48 | return new DataAssociator_GridTable; 49 | } else { 50 | std::cerr << "[DataAssociatorFactory::ProduceDataAssociatorPtr()] Error: Invalid input \n\n"; 51 | return nullptr; 52 | } 53 | } 54 | }; 55 | 56 | } // namespace sample_slam 57 | 58 | #endif // LAUNCHER_DATAASSOCIATORFACTORY_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/FrameworkFactory.cpp: -------------------------------------------------------------------------------- 1 | #include "FrameworkFactory.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | // launcher/FrameworkFactory/ 8 | #include "CostFunctionFactory.h" 9 | #include "DataAssociatorFactory.h" 10 | #include "PointCloudMapFactory.h" 11 | #include "PoseEstimatorFactory.h" 12 | #include "PoseOptimizerFactory.h" 13 | #include "ReferenceScanMakerFactory.h" 14 | // slam/ 15 | #include "LoopDetector.h" 16 | #include "ScanMatcher.h" 17 | #include "SlamFrontend.h" 18 | 19 | 20 | namespace sample_slam { 21 | 22 | #define COMMENT_PREFIX_SEMICOLON ';' 23 | #define COMMENT_PREFIX_SHARP '#' 24 | #define DELIMINATER_SYMBOL '=' 25 | 26 | #define POINT_CLOUD_MAP_TYPE "point_cloud_map_type" 27 | #define REFERENCE_SCAN_MAKER_TYPE "reference_scan_maker_type" 28 | #define POSE_ESTIMATOR_TYPE "pose_estimator_type" 29 | #define DATA_ASSOCIATOR_TYPE "data_associator_type" 30 | #define POSE_OPTIMIZER_TYPE "pose_optimizer_type" 31 | #define COST_FUNCTION_TYPE "cost_function_type" 32 | 33 | #define IS_SCAN_PREPROCESS "is_scan_preprocess" 34 | #define IS_ODOMETRY_FUSION "is_odometry_fusion" 35 | #define IS_LOOP_CLOSURE "is_loop_closure" 36 | 37 | 38 | void FrameworkFactory::CustomizeFramework(std::string framework_config_filename) { 39 | std::ifstream framework_config_ifs(framework_config_filename, std::ios::in); 40 | if (!framework_config_ifs.is_open()) { 41 | std::cerr << "[FrameworkFactory::CustomizeFramework()] "; 42 | std::cerr << "Error: framework_config_ifs.is_open() = " << framework_config_ifs.is_open() << "\n\n"; 43 | return; 44 | } 45 | 46 | // Parses framework config file 47 | // https://www.walletfox.com/course/parseconfigfile.php 48 | std::string line; 49 | while (std::getline(framework_config_ifs, line)) { 50 | // Removes whitespaces 51 | line.erase(std::remove_if(line.begin(), line.end(), isspace), line.end()); 52 | 53 | // Skips if the line is comment (that starts from ';' or '#) or empty 54 | if (line[0] == COMMENT_PREFIX_SEMICOLON || line[0] == COMMENT_PREFIX_SHARP || line.empty()) { 55 | continue; 56 | } 57 | 58 | // Parses into key and value deliminated by '=' 59 | std::size_t equal_pos = line.find(DELIMINATER_SYMBOL); 60 | std::string key = line.substr(0, equal_pos); 61 | std::string value = line.substr(equal_pos + 1); 62 | 63 | std::cout << "[FrameworkFactory::CustomizeFramework()] "; 64 | std::cout << "key = " << key << ", value = " << value << "\n"; 65 | 66 | if (key == POINT_CLOUD_MAP_TYPE) 67 | { 68 | delete point_cloud_map_ptr_; 69 | point_cloud_map_ptr_ = PointCloudMapFactory::ProducePointCloudMapPtr(value); 70 | } 71 | else if (key == REFERENCE_SCAN_MAKER_TYPE) 72 | { 73 | delete reference_scan_maker_ptr_; 74 | reference_scan_maker_ptr_ = ReferenceScanMakerFactory::ProduceReferenceScanMakerPtr(value); 75 | } 76 | else if (key == POSE_ESTIMATOR_TYPE) 77 | { 78 | delete pose_estimator_ptr_; 79 | pose_estimator_ptr_ = PoseEstimatorFactory::ProducePoseEstimatorPtr(value); 80 | } 81 | else if (key == DATA_ASSOCIATOR_TYPE) 82 | { 83 | delete data_associator_ptr_; 84 | data_associator_ptr_ = DataAssociatorFactory::ProduceDataAssociatorPtr(value); 85 | } 86 | else if (key == POSE_OPTIMIZER_TYPE) 87 | { 88 | delete pose_optimizer_ptr_; 89 | pose_optimizer_ptr_ = PoseOptimizerFactory::ProducePoseOptimizerPtr(value); 90 | } 91 | else if (key == COST_FUNCTION_TYPE) 92 | { 93 | delete cost_function_ptr_; 94 | cost_function_ptr_ = CostFunctionFactory::ProduceCostFunctionPtr(value); 95 | } 96 | else if (key == IS_SCAN_PREPROCESS) 97 | { 98 | if (std::stoi(value) == 1) { 99 | is_scan_preprocess_ = true; 100 | } else { 101 | is_scan_preprocess_ = false; 102 | } 103 | } 104 | else if (key == IS_ODOMETRY_FUSION) 105 | { 106 | if (std::stoi(value) == 1) { 107 | is_odometry_fusion_ = true; 108 | } else { 109 | is_odometry_fusion_ = false; 110 | } 111 | } 112 | else if (key == IS_LOOP_CLOSURE) 113 | { 114 | if (std::stoi(value) == 1) { 115 | is_loop_closure_ = true; 116 | 117 | delete point_cloud_map_ptr_; 118 | point_cloud_map_ptr_ = PointCloudMapFactory::ProducePointCloudMapPtr(PointCloudMapType::SUB_MAP); 119 | 120 | delete data_associator_ptr_; 121 | data_associator_ptr_ = DataAssociatorFactory::ProduceDataAssociatorPtr(DataAssociatorType::GRID_TABLE); 122 | 123 | delete cost_function_ptr_; 124 | cost_function_ptr_ = CostFunctionFactory::ProduceCostFunctionPtr(CostFunctionType::PERPENDICULAR_DISTANCE); 125 | } else { 126 | is_loop_closure_ = false; 127 | } 128 | } 129 | } 130 | 131 | std::cout << "\n"; 132 | 133 | if (point_cloud_map_ptr_ == nullptr) { 134 | std::cerr << "[FrameworkFactory::CustomizeFramework()] Error: point_cloud_map_ptr_ == nullptr... \n\n"; 135 | } 136 | if (reference_scan_maker_ptr_ == nullptr) { 137 | std::cerr << "[FrameworkFactory::CustomizeFramework()] Error: reference_scan_maker_ptr_ == nullptr... \n\n"; 138 | } 139 | if (pose_estimator_ptr_ == nullptr) { 140 | std::cerr << "[FrameworkFactory::CustomizeFramework()] Error: pose_estimator_ptr_ == nullptr... \n\n"; 141 | } 142 | if (data_associator_ptr_ == nullptr) { 143 | std::cerr << "[FrameworkFactory::CustomizeFramework()] Error: data_associator_ptr_ == nullptr... \n\n"; 144 | } 145 | if (pose_optimizer_ptr_ == nullptr) { 146 | std::cerr << "[FrameworkFactory::CustomizeFramework()] Error: pose_optimizer_ptr_ == nullptr... \n\n"; 147 | } 148 | if (cost_function_ptr_ == nullptr) { 149 | std::cerr << "[FrameworkFactory::CustomizeFramework()] Error: cost_function_ptr_ == nullptr... \n\n"; 150 | } 151 | 152 | slam_launcher_.SetPointCloudMapPtr(point_cloud_map_ptr_); 153 | slam_launcher_.SetIsScanPreprocess(is_scan_preprocess_); 154 | 155 | SlamFrontend* slam_frontend_ptr = slam_launcher_.GetSlamFrontendPtr(); 156 | slam_frontend_ptr->SetPointCloudMapPtr(point_cloud_map_ptr_); 157 | slam_frontend_ptr->SetIsLoopClosure(is_loop_closure_); 158 | 159 | ScanMatcher* scan_mather_ptr = slam_frontend_ptr->GetScanMatcherPtr(); 160 | scan_mather_ptr->SetPointCloudMapPtr(point_cloud_map_ptr_); 161 | scan_mather_ptr->SetReferenceScanMakerPtr(reference_scan_maker_ptr_); 162 | scan_mather_ptr->SetPoseEstimatorPtr(pose_estimator_ptr_); 163 | scan_mather_ptr->SetIsScanPreprocess(is_scan_preprocess_); 164 | scan_mather_ptr->SetIsOdometryFusion(is_odometry_fusion_); 165 | 166 | ReferenceScanMaker* reference_scan_maker_ptr = scan_mather_ptr->GetReferenceScanMakerPtr(); 167 | reference_scan_maker_ptr->SetPointCloudMapPtr(point_cloud_map_ptr_); 168 | 169 | PoseEstimator* pose_estimator_ptr = scan_mather_ptr->GetPoseEstimatorPtr(); 170 | pose_estimator_ptr->SetDataAssociatorPtr(data_associator_ptr_); 171 | pose_estimator_ptr->SetPoseOptimizerPtr(pose_optimizer_ptr_); 172 | 173 | PoseOptimizer* pose_optimizer_ptr = pose_estimator_ptr->GetPoseOptimizerPtr(); 174 | pose_optimizer_ptr->SetCostFunctionPtr(cost_function_ptr_); 175 | 176 | if (is_loop_closure_) { 177 | LoopDetector* loop_detector_ptr = slam_frontend_ptr->GetLoopDetectorPtr(); 178 | loop_detector_ptr->SetPointCloudMapPtr((PointCloudMap_SubMap*)point_cloud_map_ptr_); 179 | loop_detector_ptr->SetDataAssociatorPtr((DataAssociator_GridTable*)data_associator_ptr_); 180 | loop_detector_ptr->SetCostFunctionPtr((CostFunction_PerpendicularDistance*)cost_function_ptr_); 181 | loop_detector_ptr->SetPoseEstimatorPtr(pose_estimator_ptr_); 182 | 183 | SlamBackend* slam_backend_ptr = slam_frontend_ptr->GetSlamBackendPtr(); 184 | slam_backend_ptr->SetPointCloudMapPtr((PointCloudMap_SubMap*)point_cloud_map_ptr_); 185 | } 186 | 187 | return; 188 | } 189 | 190 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/FrameworkFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_FRAMEWORKFACTORY_H_ 2 | #define LAUNCHER_FRAMEWORKFACTORY_H_ 3 | 4 | 5 | #include 6 | 7 | // launcher/ 8 | #include "SlamLauncher.h" 9 | // slam/ 10 | #include "CostFunction.h" 11 | #include "DataAssociator.h" 12 | #include "PointCloudMap.h" 13 | #include "PoseEstimator.h" 14 | #include "PoseOptimizer.h" 15 | #include "ReferenceScanMaker.h" 16 | 17 | namespace sample_slam { 18 | 19 | class FrameworkFactory { 20 | public: 21 | FrameworkFactory() 22 | : point_cloud_map_ptr_(nullptr), 23 | reference_scan_maker_ptr_(nullptr), 24 | pose_estimator_ptr_(nullptr), 25 | data_associator_ptr_(nullptr), 26 | pose_optimizer_ptr_(nullptr), 27 | cost_function_ptr_(nullptr), 28 | is_scan_preprocess_(false), 29 | is_odometry_fusion_(false), 30 | is_loop_closure_(false) {} 31 | 32 | ~FrameworkFactory() { 33 | delete point_cloud_map_ptr_; 34 | delete reference_scan_maker_ptr_; 35 | delete pose_estimator_ptr_; 36 | delete data_associator_ptr_; 37 | delete pose_optimizer_ptr_; 38 | delete cost_function_ptr_; 39 | } 40 | 41 | SlamLauncher* GetSlamLauncherPtr() { 42 | return &slam_launcher_; 43 | } 44 | 45 | void CustomizeFramework(std::string framework_config_filename); 46 | 47 | 48 | private: 49 | PointCloudMap* point_cloud_map_ptr_; // Pointer to abstract class, which is customizable 50 | ReferenceScanMaker* reference_scan_maker_ptr_; // Pointer to abstract class, which is customizable 51 | PoseEstimator* pose_estimator_ptr_; // Pointer to abstract class, which is customizable 52 | DataAssociator* data_associator_ptr_; // Pointer to abstract class, which is customizable 53 | PoseOptimizer* pose_optimizer_ptr_; // Pointer to abstract class, which is customizable 54 | CostFunction* cost_function_ptr_; // Pointer to abstract class, which is customizable 55 | bool is_scan_preprocess_; 56 | bool is_odometry_fusion_; 57 | bool is_loop_closure_; 58 | 59 | SlamLauncher slam_launcher_; 60 | }; 61 | 62 | } // namespace sample_slam 63 | 64 | 65 | #endif // LAUNCHER_FRAMEWORKFACTORY_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/PointCloudMapFactory/PointCloudMapFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_POINTCLOUDMAPFACTORY_H_ 2 | #define LAUNCHER_POINTCLOUDMAPFACTORY_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "PointCloudMap.h" 10 | #include "PointCloudMap_EntirePoint.h" 11 | #include "PointCloudMap_GridTable.h" 12 | #include "PointCloudMap_SubMap.h" 13 | 14 | 15 | namespace sample_slam { 16 | 17 | enum class PointCloudMapType { 18 | ENTIRE_POINT = 0, 19 | GRID_TABLE = 1, 20 | SUB_MAP = 2, 21 | }; 22 | 23 | 24 | class PointCloudMapFactory { 25 | public: 26 | PointCloudMapFactory() {} 27 | 28 | ~PointCloudMapFactory() {} 29 | 30 | static PointCloudMap* ProducePointCloudMapPtr(const std::string& chars) { 31 | std::map point_cloud_map_type = { 32 | { PointCloudMapType::ENTIRE_POINT, "entire_point" }, 33 | { PointCloudMapType::GRID_TABLE, "grid_table" }, 34 | { PointCloudMapType::SUB_MAP, "sub_map" }, 35 | }; 36 | 37 | if (chars == point_cloud_map_type[PointCloudMapType::ENTIRE_POINT]) { 38 | return new PointCloudMap_EntirePoint; 39 | } else if (chars == point_cloud_map_type[PointCloudMapType::GRID_TABLE]) { 40 | return new PointCloudMap_GridTable; 41 | } else if (chars == point_cloud_map_type[PointCloudMapType::SUB_MAP]) { 42 | return new PointCloudMap_SubMap; 43 | } else { 44 | std::cerr << "[PointCloudMapFactory::ProducePointCloudMapPtr()] Error: Invalid input \n\n"; 45 | return nullptr; 46 | } 47 | } 48 | 49 | static PointCloudMap* ProducePointCloudMapPtr(const PointCloudMapType& enumerate) { 50 | if (enumerate == PointCloudMapType::ENTIRE_POINT) { 51 | return new PointCloudMap_EntirePoint; 52 | } else if (enumerate == PointCloudMapType::GRID_TABLE) { 53 | return new PointCloudMap_GridTable; 54 | } else if (enumerate == PointCloudMapType::SUB_MAP) { 55 | return new PointCloudMap_SubMap; 56 | } else { 57 | std::cerr << "[PointCloudMapFactory::ProducePointCloudMapPtr()] Error: Invalid input \n\n"; 58 | return nullptr; 59 | } 60 | } 61 | }; 62 | 63 | } // namespace sample_slam 64 | 65 | #endif // LAUNCHER_POINTCLOUDMAPFACTORY_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/PoseEstimatorFactory/PoseEstimatorFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_POSEESTIMATORFACTORY_H_ 2 | #define LAUNCHER_POSEESTIMATORFACTORY_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "PoseEstimator.h" 10 | #include "PoseEstimator_ICP.h" 11 | // #include "PoseEstimator_NDT.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | enum class PoseEstimatorType { 17 | ICP = 0, 18 | // NDT = 1, 19 | }; 20 | 21 | 22 | class PoseEstimatorFactory { 23 | public: 24 | PoseEstimatorFactory() {} 25 | 26 | ~PoseEstimatorFactory() {} 27 | 28 | static PoseEstimator* ProducePoseEstimatorPtr(const std::string& chars) { 29 | std::map pose_estimator_type = { 30 | { PoseEstimatorType::ICP, "icp" }, 31 | // { PoseEstimatorType::NDT, "ndt" }, 32 | }; 33 | 34 | if (chars == pose_estimator_type[PoseEstimatorType::ICP]) { 35 | return new PoseEstimator_ICP; 36 | // } else if (chars == pose_estimator_type[PoseEstimatorType::NDT]) { 37 | // return new PoseEstimator_NDT; 38 | } else { 39 | std::cerr << "[PoseEstimatorFactory::ProducePoseEstimatorPtr()] Error: Invalid input \n\n"; 40 | return nullptr; 41 | } 42 | } 43 | }; 44 | 45 | } // namespace sample_slam 46 | 47 | #endif // LAUNCHER_POSEESTIMATORFACTORY_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/PoseOptimizerFactory/PoseOptimizerFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_POSEOPTIMIZER_H_ 2 | #define LAUNCHER_POSEOPTIMIZER_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "PoseOptimizer.h" 10 | #include "PoseOptimizer_GradientDescent.h" 11 | #include "PoseOptimizer_LineSearch.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | enum class PoseOptimizerType { 17 | GRADIENT_DESCENT = 0, 18 | LINE_SEARCH = 1, 19 | }; 20 | 21 | 22 | class PoseOptimizerFactory { 23 | public: 24 | PoseOptimizerFactory() {} 25 | 26 | ~PoseOptimizerFactory() {} 27 | 28 | static PoseOptimizer* ProducePoseOptimizerPtr(const std::string& chars) { 29 | std::map pose_optimizer_type = { 30 | { PoseOptimizerType::GRADIENT_DESCENT, "gradient_descent" }, 31 | { PoseOptimizerType::LINE_SEARCH, "line_search" }, 32 | }; 33 | 34 | if (chars == pose_optimizer_type[PoseOptimizerType::GRADIENT_DESCENT]) { 35 | return new PoseOptimizer_GradientDescent; 36 | } else if (chars == pose_optimizer_type[PoseOptimizerType::LINE_SEARCH]) { 37 | return new PoseOptimizer_LineSearch; 38 | } else { 39 | std::cerr << "[PoseOptimizerFactory::ProducePoseOptimizerPtr()] Error: Invalid input \n\n"; 40 | return nullptr; 41 | } 42 | } 43 | }; 44 | 45 | } // namespace sample_slam 46 | 47 | #endif // LAUNCHER_POSEOPTIMIZER_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/FrameworkFactory/ReferenceScanMakerFactory/ReferenceScanMakerFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_REFERENCESCANMAKERFACTORY_H_ 2 | #define LAUNCHER_REFERENCESCANMAKERFACTORY_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "ReferenceScanMaker.h" 10 | #include "ReferenceScanMaker_LastScan.h" 11 | #include "ReferenceScanMaker_LocalMap.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | enum class ReferenceScanMakerType { 17 | LAST_SCAN = 0, 18 | LOCAL_MAP = 1, 19 | }; 20 | 21 | 22 | class ReferenceScanMakerFactory { 23 | public: 24 | ReferenceScanMakerFactory() {} 25 | 26 | ~ReferenceScanMakerFactory() {} 27 | 28 | static ReferenceScanMaker* ProduceReferenceScanMakerPtr(const std::string& chars) { 29 | std::map reference_scan_maker_type = { 30 | { ReferenceScanMakerType::LAST_SCAN, "last_scan" }, 31 | { ReferenceScanMakerType::LOCAL_MAP, "local_map" }, 32 | }; 33 | 34 | if (chars == reference_scan_maker_type[ReferenceScanMakerType::LAST_SCAN]) { 35 | return new ReferenceScanMaker_LastScan; 36 | } else if (chars == reference_scan_maker_type[ReferenceScanMakerType::LOCAL_MAP]) { 37 | return new ReferenceScanMaker_LocalMap; 38 | } else { 39 | std::cerr << "[ReferenceScanMakerFactory::ProduceReferenceScanMakerPtr()] Error: Invalid input \n\n"; 40 | return nullptr; 41 | } 42 | } 43 | }; 44 | 45 | } // namespace sample_slam 46 | 47 | #endif // LAUNCHER_REFERENCESCANMAKERFACTORY_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/MapDrawer/MapDrawer.cpp: -------------------------------------------------------------------------------- 1 | #include "MapDrawer.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | // Draw point cloud and robot trajectory 9 | void MapDrawer::DrawMapGnuplot(const PointCloudMap& point_cloud_map) { 10 | const std::vector& global_map = point_cloud_map.GetGlobalMap(); 11 | const std::vector& poses = point_cloud_map.GetPoses(); 12 | DrawGnuplot(global_map, poses); 13 | return; 14 | } 15 | 16 | 17 | // Draw only one scan 18 | void MapDrawer::DrawScanGnuplot(const Scan2D& scan) { 19 | std::vector poses{Pose2D(0.0, 0.0, 0.0)}; 20 | DrawGnuplot(scan.laser_points, poses); 21 | return; 22 | } 23 | 24 | 25 | // Main drawing 26 | void MapDrawer::DrawGnuplot(const std::vector& laser_points, const std::vector& poses) { 27 | std::cout << "[MapDrawer::DrawGnuplot()] laser_points.size() = " << laser_points.size() << "\n"; 28 | 29 | // Setting gnuplot 30 | fprintf(pipe_gnuplot_ptr_, "set multiplot \n"); 31 | fprintf(pipe_gnuplot_ptr_, "plot '-' w p pt 7 ps 0.1 lc rgb 0x0, '-' with vector \n"); 32 | 33 | // Draw point cloud 34 | for (std::size_t i = 0; i < laser_points.size(); i += num_skip_laser_point_) { 35 | fprintf(pipe_gnuplot_ptr_, "%lf %lf \n", laser_points[i].x, laser_points[i].y); 36 | } 37 | fprintf(pipe_gnuplot_ptr_, "e \n"); 38 | 39 | // Draw robot trajectory 40 | for (std::size_t i = 0; i < poses.size(); i += num_skip_pose_) { 41 | double cos_yaw = cos(poses[i].yaw); 42 | double sin_yaw = sin(poses[i].yaw); 43 | 44 | double x_axis_x = axis_length_ * cos_yaw; 45 | double x_axis_y = axis_length_ * sin_yaw; 46 | fprintf(pipe_gnuplot_ptr_, "%lf %lf %lf %lf \n", poses[i].x, poses[i].y, x_axis_x, x_axis_y); 47 | 48 | double y_axis_x = axis_length_ * -sin_yaw; 49 | double y_axis_y = axis_length_ * cos_yaw; 50 | fprintf(pipe_gnuplot_ptr_, "%lf %lf %lf %lf \n", poses[i].x, poses[i].y, y_axis_x, y_axis_y); 51 | } 52 | fprintf(pipe_gnuplot_ptr_, "e \n"); 53 | 54 | fflush(pipe_gnuplot_ptr_); 55 | 56 | return; 57 | } 58 | 59 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/MapDrawer/MapDrawer.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_MAPDRAWER_H_ 2 | #define LAUNCHER_MAPDRAWER_H_ 3 | 4 | 5 | #include 6 | #include 7 | 8 | // slam/ 9 | #include "PointCloudMap.h" 10 | // utility/ 11 | #include "LaserPoint2D.h" 12 | #include "Pose2D.h" 13 | #include "Scan2D.h" 14 | 15 | 16 | namespace sample_slam { 17 | 18 | class MapDrawer { 19 | public: 20 | MapDrawer() 21 | : min_x_(-10.0), max_x_(10.0), min_y_(-10.0), max_y_(10.0), aspect_ratio_(-0.9), 22 | num_skip_laser_point_(1), num_skip_pose_(10), 23 | // num_skip_laser_point_(5), num_skip_pose_(10), 24 | axis_length_(0.5) { 25 | pipe_gnuplot_ptr_ = popen("gnuplot", "w"); 26 | fprintf(pipe_gnuplot_ptr_, "set size ratio %lf \n", aspect_ratio_); 27 | } 28 | 29 | ~MapDrawer() { 30 | if (pipe_gnuplot_ptr_ != nullptr) { 31 | pclose(pipe_gnuplot_ptr_); 32 | } 33 | } 34 | 35 | void SetRange(double range) { 36 | min_x_ = -range; 37 | max_x_ = range; 38 | fprintf(pipe_gnuplot_ptr_, "set xrange [%lf:%lf] \n", min_x_, max_x_); 39 | 40 | min_y_ = -range; 41 | max_y_ = range; 42 | fprintf(pipe_gnuplot_ptr_, "set yrange [%lf:%lf] \n", min_y_, max_y_); 43 | } 44 | 45 | void DrawMapGnuplot(const PointCloudMap& point_cloud_map); 46 | void DrawScanGnuplot(const Scan2D& scan); 47 | void DrawGnuplot(const std::vector& laser_points, const std::vector& poses); 48 | 49 | 50 | private: 51 | double min_x_, max_x_, min_y_, max_y_; 52 | double aspect_ratio_; 53 | 54 | int num_skip_laser_point_; 55 | int num_skip_pose_; 56 | 57 | double axis_length_; 58 | 59 | FILE* pipe_gnuplot_ptr_; 60 | }; 61 | 62 | } // namespace sample_slam 63 | 64 | 65 | #endif // LAUNCHER_MAPDRAWER_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/SensorDataReader/SensorDataReader.cpp: -------------------------------------------------------------------------------- 1 | #include "SensorDataReader.h" 2 | 3 | #include 4 | #include 5 | 6 | // utility/ 7 | #include "LaserPoint2D.h" 8 | 9 | 10 | namespace sample_slam { 11 | 12 | #define LASERSCAN "LASERSCAN" 13 | 14 | 15 | bool SensorDataReader::OpenSensorDataFile(std::string sensor_data_filename) { 16 | sensor_data_filestream_.open(sensor_data_filename); 17 | if (!sensor_data_filestream_.is_open()) { 18 | std::cerr << "[SensorDataReader::OpenSensorDataFile()] "; 19 | std::cerr << "Error: cannot open file " << sensor_data_filename << "\n\n"; 20 | return false; 21 | } 22 | 23 | return true; 24 | } 25 | 26 | 27 | void SensorDataReader::CloseSensorDataFile() { 28 | sensor_data_filestream_.close(); 29 | return; 30 | } 31 | 32 | 33 | bool SensorDataReader::LoadScan(std::size_t count, Scan2D& scan) { 34 | while(!sensor_data_filestream_.eof()) { 35 | std::string data_type; 36 | sensor_data_filestream_ >> data_type; 37 | 38 | // Loads and decodes the line if the data type is "LASERSCAN" 39 | if (data_type == LASERSCAN) { 40 | scan.id = count; 41 | 42 | /*********************************************************************************************************************************************************** 43 | * LASERSCAN (scan_id) (second) (nano_second) (yaw_1 [deg]) (range_1 [m]) ... (yaw_n [deg]) (range_n [m]) (odom_x [m]) (odom_y [m]) (odom_yaw [rad]) 44 | ***********************************************************************************************************************************************************/ 45 | 46 | int scan_id, second, nano_second; 47 | sensor_data_filestream_ >> scan_id >> second >> nano_second; // These are not used 48 | 49 | int num_laser_point; 50 | sensor_data_filestream_ >> num_laser_point; 51 | 52 | scan.laser_points.reserve(num_laser_point); 53 | for (std::size_t i = 0; i < num_laser_point; ++i) { 54 | double yaw, range; 55 | sensor_data_filestream_ >> yaw >> range; // [deg], [m] 56 | 57 | if (range <= Scan2D::MIN_SCAN_RANGE || Scan2D::MAX_SCAN_RANGE <= range) { 58 | continue; 59 | } 60 | 61 | yaw = DEG2RAD(yaw) + lidar_yaw_offset_; // [rad] 62 | 63 | LaserPoint2D laser_point; 64 | laser_point.id = count; 65 | laser_point.RangeYaw2XY(range, yaw); 66 | scan.laser_points.emplace_back(laser_point); 67 | } 68 | 69 | // Odometry data corresponding with the laser scan 70 | sensor_data_filestream_ >> scan.pose.x >> scan.pose.y >> scan.pose.yaw; // [m], [m], [rad] 71 | scan.pose.CalcRotationMatrix(); 72 | 73 | break; 74 | } 75 | 76 | // Skips loading the line if the data type is NOT "LASERSCAN" 77 | std::string tmp; 78 | std::getline(sensor_data_filestream_, tmp); 79 | } 80 | 81 | return sensor_data_filestream_.eof(); 82 | } 83 | 84 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/SensorDataReader/SensorDataReader.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_SENSORDATAREADER_H_ 2 | #define LAUNCHER_SENSORDATAREADER_H_ 3 | 4 | 5 | #include 6 | #include 7 | 8 | // utility/ 9 | #include "MyUtility.h" 10 | #include "Scan2D.h" 11 | 12 | 13 | namespace sample_slam { 14 | 15 | class SensorDataReader { 16 | public: 17 | SensorDataReader() : lidar_yaw_offset_(DEG2RAD(180.0)) {} 18 | 19 | ~SensorDataReader() {} 20 | 21 | bool OpenSensorDataFile(std::string sensor_data_filename); 22 | 23 | void CloseSensorDataFile(); 24 | 25 | bool LoadScan(std::size_t count, Scan2D& scan); 26 | 27 | 28 | private: 29 | std::ifstream sensor_data_filestream_; 30 | double lidar_yaw_offset_; // [rad] 31 | }; 32 | 33 | } // namespace sample_slam 34 | 35 | 36 | #endif // LAUNCHER_SENSORDATAREADER_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/SlamLauncher/SlamLauncher.cpp: -------------------------------------------------------------------------------- 1 | #include "SlamLauncher.h" 2 | 3 | #include 4 | 5 | #include "ScanPreprocessor.h" 6 | 7 | 8 | namespace sample_slam { 9 | 10 | bool SlamLauncher::OpenSensorDataFile(std::string sensor_data_filename) { 11 | return sensor_data_reader_.OpenSensorDataFile(sensor_data_filename); 12 | } 13 | 14 | 15 | void SlamLauncher::VisualizeScan() { 16 | map_drawer_.SetRange(Scan2D::MAX_SCAN_RANGE); 17 | 18 | std::size_t count = 0; 19 | bool is_eof = false; 20 | 21 | while (!is_eof) { 22 | std::cout << "[SlamLauncher::VisualizeScan()] count = " << count << "\n"; 23 | 24 | Scan2D scan; 25 | is_eof = sensor_data_reader_.LoadScan(count, scan); 26 | 27 | if (is_scan_preprocess_) { 28 | ScanPreprocessor scan_preprocessor; 29 | scan_preprocessor.ResampleLaserPoints(scan.laser_points); 30 | } 31 | 32 | map_drawer_.DrawScanGnuplot(scan); 33 | usleep(0.1 * 1000 * 1000); // [us] 34 | 35 | ++count; 36 | } 37 | 38 | sensor_data_reader_.CloseSensorDataFile(); 39 | 40 | return; 41 | } 42 | 43 | 44 | void SlamLauncher::Run(std::string viz_option) { 45 | std::size_t count = 0; 46 | bool is_eof = false; 47 | 48 | while (!is_eof) { 49 | std::cout << "[SlamLauncher::Run()] count = " << count << "\n"; 50 | 51 | Scan2D scan; 52 | is_eof = sensor_data_reader_.LoadScan(count, scan); 53 | 54 | if (count == 0) { 55 | init_pose_ = scan.pose; 56 | init_pose_.CalcRotationMatrix(); 57 | } 58 | 59 | if (viz_option == VIZ_MODE_ODOMETRY) { 60 | RunOdometry(scan); 61 | } else if (viz_option == VIZ_MODE_SLAM) { 62 | RunSlam(scan, count); 63 | } else { 64 | std::cout << "[SlamLauncher::Run()] "; 65 | std::cerr << "Error: viz_option = " << viz_option << "\n"; 66 | } 67 | 68 | if (count % num_skip_draw_ == 0) { 69 | map_drawer_.DrawMapGnuplot(*point_cloud_map_ptr_); 70 | } 71 | 72 | ++count; 73 | } 74 | 75 | sensor_data_reader_.CloseSensorDataFile(); 76 | 77 | // Endless loop for remaining the map drawing after processing. 78 | while (true) { 79 | usleep(0.1 * 1000 * 1000); // [us] 80 | std::cout << "[SlamLauncher::VisualizeSlam()] EOF: Ctrl-c to end... \n"; 81 | } 82 | } 83 | 84 | 85 | void SlamLauncher::RunOdometry(const Scan2D& scan) { 86 | Pose2D odometry_pose = scan.pose - init_pose_; 87 | 88 | std::vector global_laser_points; 89 | for (std::size_t i = 0; i < scan.laser_points.size(); ++i) { 90 | LaserPoint2D global_laser_point; 91 | odometry_pose.LocalCoord2GlobalCoord(scan.laser_points[i], global_laser_point); 92 | global_laser_points.emplace_back(global_laser_point); 93 | } 94 | 95 | point_cloud_map_ptr_->AddPose(odometry_pose); 96 | point_cloud_map_ptr_->AddLaserPoints(global_laser_points); 97 | point_cloud_map_ptr_->MakeGlobalMap(); 98 | 99 | std::cout << "[SlamLauncher::RunOdometry()] " 100 | << "odometry_pose.x = " << odometry_pose.x << ", odometry_pose.y = " << odometry_pose.y 101 | << ", odometry_pose.yaw = " << odometry_pose.yaw <<"\n"; 102 | 103 | return; 104 | } 105 | 106 | 107 | void SlamLauncher::RunSlam(const Scan2D& scan, int count) { 108 | slam_frontend_.RunSlam(scan, count); 109 | 110 | return; 111 | } 112 | 113 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/launcher/SlamLauncher/SlamLauncher.h: -------------------------------------------------------------------------------- 1 | #ifndef LAUNCHER_SLAMLAUNCHER_H_ 2 | #define LAUNCHER_SLAMLAUNCHER_H_ 3 | 4 | 5 | #include 6 | #include 7 | 8 | // launcher/ 9 | #include "MapDrawer.h" 10 | #include "SensorDataReader.h" 11 | // slam/ 12 | #include "PointCloudMap.h" 13 | #include "SlamFrontend.h" 14 | // utility/ 15 | #include "Pose2D.h" 16 | #include "Scan2D.h" 17 | 18 | 19 | #define VIZ_MODE_SCAN "scan" 20 | #define VIZ_MODE_ODOMETRY "odom" 21 | #define VIZ_MODE_SLAM "slam" 22 | 23 | 24 | namespace sample_slam { 25 | 26 | class SlamLauncher { 27 | public: 28 | SlamLauncher() 29 | : num_skip_draw_(10) {} 30 | 31 | ~SlamLauncher() {} 32 | 33 | void SetPointCloudMapPtr(PointCloudMap* point_cloud_map_ptr) { 34 | point_cloud_map_ptr_ = point_cloud_map_ptr; 35 | } 36 | 37 | void SetIsScanPreprocess(bool is_scan_preprocess) { 38 | is_scan_preprocess_ = is_scan_preprocess; 39 | } 40 | 41 | SlamFrontend* GetSlamFrontendPtr() { 42 | return &slam_frontend_; 43 | } 44 | 45 | bool OpenSensorDataFile(std::string sensor_data_filename); 46 | void VisualizeScan(); 47 | void Run(std::string viz_option); 48 | 49 | 50 | private: 51 | void RunOdometry(const Scan2D& scan); 52 | void RunSlam(const Scan2D& scan, int count); 53 | 54 | PointCloudMap* point_cloud_map_ptr_; // Pointer to abstract class, which is customizable 55 | bool is_scan_preprocess_; 56 | 57 | SensorDataReader sensor_data_reader_; 58 | Pose2D init_pose_; 59 | SlamFrontend slam_frontend_; 60 | MapDrawer map_drawer_; 61 | int num_skip_draw_; 62 | }; 63 | 64 | } // namespace sample_slam 65 | 66 | 67 | #endif // LAUNCHER_SLAMLAUNCHER_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "cxxopts.hpp" 5 | #include "FrameworkFactory.h" 6 | #include "SlamLauncher.h" 7 | 8 | 9 | int main(int argc, char* argv []) { 10 | // Decomposes argv[0] into directory path and file name 11 | std::string argv0(argv[0]); 12 | std::size_t last_slash_pos = argv0.find_last_of('/'); 13 | std::string executable_directory = argv0.substr(0, last_slash_pos + 1); 14 | std::string executable_filename = argv0.substr(last_slash_pos + 1); 15 | 16 | std::string help_string = "Sample SLAM: Scan matching as a front-end, Graph based SLAM as a back-end"; 17 | cxxopts::Options option_parser(executable_filename, help_string); 18 | 19 | std::string sensor_data_filename; 20 | std::string viz_mode; 21 | 22 | // Parses program options 23 | // https://tadaoyamaoka.hatenablog.com/entry/2019/01/30/235251 (Japanese) 24 | try { 25 | option_parser.add_options() 26 | ("data-file", "String: Path to sensor data file (.lsc)", cxxopts::value()) 27 | ("viz-mode", "String: Visualization mode ... scan | odom | slam", cxxopts::value()) 28 | ("h,help", "Print usage"); 29 | 30 | option_parser.parse_positional({"data-file", "viz-mode"}); 31 | auto options = option_parser.parse(argc, argv); 32 | 33 | if (options.count("help")) { 34 | std::cout << option_parser.help({}) << std::endl; 35 | return EXIT_SUCCESS; 36 | } 37 | 38 | sensor_data_filename = options["data-file"].as(); 39 | viz_mode = options["viz-mode"].as(); 40 | 41 | std::cout << "\n"; 42 | std::cout << "sensor_data_filename = " << sensor_data_filename << "\n"; 43 | std::cout << "viz_mode = " << viz_mode << "\n"; 44 | std::cout << "\n"; 45 | } 46 | catch (cxxopts::OptionException &e) { 47 | std::cout << option_parser.usage() << "\n"; 48 | std::cerr << e.what() << "\n"; 49 | std::exit(EXIT_FAILURE); 50 | } 51 | 52 | std::string framework_config_filename = executable_directory + "../../framework.cfg"; 53 | sample_slam::FrameworkFactory framework_factory; 54 | framework_factory.CustomizeFramework(framework_config_filename); 55 | sample_slam::SlamLauncher* slam_launcher_ptr = framework_factory.GetSlamLauncherPtr(); 56 | 57 | if(!slam_launcher_ptr->OpenSensorDataFile(sensor_data_filename)) { 58 | return EXIT_FAILURE; 59 | } 60 | 61 | if (viz_mode == VIZ_MODE_SCAN) { 62 | slam_launcher_ptr->VisualizeScan(); 63 | } else if (viz_mode == VIZ_MODE_ODOMETRY || viz_mode == VIZ_MODE_SLAM) { 64 | slam_launcher_ptr->Run(viz_mode); 65 | } else { 66 | std::cerr << "Error: invalid option... \n"; 67 | std::cerr << "The command should be like: ./main /path/to/data " 68 | << VIZ_MODE_SCAN << " | " << VIZ_MODE_ODOMETRY << " | " << VIZ_MODE_SLAM << "\n\n"; 69 | return EXIT_FAILURE; 70 | } 71 | 72 | return EXIT_SUCCESS; 73 | } -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | project(slam_lib 4 | VERSION 1.0.0 5 | DESCRIPTION "SLAM library" 6 | LANGUAGES CXX 7 | ) 8 | 9 | find_package(Boost REQUIRED) 10 | message(STATUS " Boost_INCLUDE_DIR = ${Boost_INCLUDE_DIR}") 11 | 12 | add_library(slam STATIC 13 | ${PROJECT_SOURCE_DIR}/CostFunction/CostFunction_EuclideanDistance.cpp 14 | ${PROJECT_SOURCE_DIR}/CostFunction/CostFunction_PerpendicularDistance.cpp 15 | ######## 16 | ${PROJECT_SOURCE_DIR}/DataAssociator/DataAssociator_LinearSearch.cpp 17 | ${PROJECT_SOURCE_DIR}/DataAssociator/DataAssociator_GridTable.cpp 18 | ######## 19 | ${PROJECT_SOURCE_DIR}/PointCloudMap/PointCloudMap_EntirePoint.cpp 20 | ${PROJECT_SOURCE_DIR}/PointCloudMap/PointCloudMap_GridTable.cpp 21 | ${PROJECT_SOURCE_DIR}/PointCloudMap/PointCloudMap_SubMap.cpp 22 | ######## 23 | ${PROJECT_SOURCE_DIR}/PoseEstimator/PoseEstimator_ICP.cpp 24 | # ${PROJECT_SOURCE_DIR}/PoseEstimator/PoseEstimator_NDT.cpp 25 | ######## 26 | ${PROJECT_SOURCE_DIR}/PoseOptimizer/PoseOptimizer_GradientDescent.cpp 27 | ${PROJECT_SOURCE_DIR}/PoseOptimizer/PoseOptimizer_LineSearch.cpp 28 | ######## 29 | ${PROJECT_SOURCE_DIR}/ReferenceScanMaker/ReferenceScanMaker_LastScan.cpp 30 | ${PROJECT_SOURCE_DIR}/ReferenceScanMaker/ReferenceScanMaker_LocalMap.cpp 31 | ################ 32 | ${PROJECT_SOURCE_DIR}/CovarianceCalculator/CovarianceCalculator.cpp 33 | ${PROJECT_SOURCE_DIR}/LoopDetector/LoopDetector.cpp 34 | ${PROJECT_SOURCE_DIR}/PoseGraph/PoseGraph.cpp 35 | ${PROJECT_SOURCE_DIR}/ScanMatcher/ScanMatcher.cpp 36 | ${PROJECT_SOURCE_DIR}/ScanPreprocessor/ScanPreprocessor.cpp 37 | ${PROJECT_SOURCE_DIR}/SlamBackend/SlamBackend.cpp 38 | ${PROJECT_SOURCE_DIR}/SlamFrontend/SlamFrontend.cpp 39 | ) 40 | target_compile_features(slam PRIVATE cxx_std_11) 41 | 42 | target_include_directories(slam PUBLIC 43 | ${Boost_INCLUDE_DIR}/ 44 | ${PROJECT_SOURCE_DIR}/CostFunction/ 45 | ${PROJECT_SOURCE_DIR}/DataAssociator/ 46 | ${PROJECT_SOURCE_DIR}/PointCloudMap/ 47 | ${PROJECT_SOURCE_DIR}/PoseEstimator/ 48 | ${PROJECT_SOURCE_DIR}/PoseOptimizer/ 49 | ${PROJECT_SOURCE_DIR}/ReferenceScanMaker/ 50 | ################ 51 | ${PROJECT_SOURCE_DIR}/CovarianceCalculator/ 52 | ${PROJECT_SOURCE_DIR}/LoopDetector/ 53 | ${PROJECT_SOURCE_DIR}/PoseGraph/ 54 | ${PROJECT_SOURCE_DIR}/ScanMatcher/ 55 | ${PROJECT_SOURCE_DIR}/ScanPreprocessor/ 56 | ${PROJECT_SOURCE_DIR}/SlamBackend/ 57 | ${PROJECT_SOURCE_DIR}/SlamFrontend/ 58 | ################ 59 | ${CMAKE_SOURCE_DIR}/solver/p2o/ 60 | ) 61 | 62 | target_link_libraries(slam 63 | utility 64 | ) -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CostFunction/CostFunction.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_COSTFUNCTION_H_ 2 | #define SLAM_COSTFUNCTION_H_ 3 | 4 | 5 | #include 6 | #include 7 | 8 | // utility/ 9 | #include "LaserPoint2D.h" 10 | #include "Pose2D.h" 11 | 12 | 13 | namespace sample_slam { 14 | 15 | class CostFunction { 16 | public: 17 | CostFunction() 18 | : error_distance_threshold_(0.2), 19 | match_cost_scale_(100.0) {} 20 | 21 | virtual ~CostFunction() {} 22 | 23 | void SetAssociatedLaserPoints(const std::vector& associated_reference_laser_points, 24 | const std::vector& associated_current_laser_points) { 25 | associated_reference_laser_points_ = associated_reference_laser_points; 26 | associated_current_laser_points_ = associated_current_laser_points; 27 | } 28 | 29 | double GetMatchedPointRatio() const { 30 | return matched_point_ratio_; 31 | } 32 | 33 | double CalcPartialDerivativeX(double x, double delta_distance, double y, double yaw) { 34 | return (CalcMatchCost(x + delta_distance, y, yaw) - CalcMatchCost(x, y, yaw)) / delta_distance; 35 | } 36 | 37 | double CalcPartialDerivativeY(double x, double y, double delta_distance, double yaw) { 38 | return (CalcMatchCost(x, y + delta_distance, yaw) - CalcMatchCost(x, y, yaw)) / delta_distance; 39 | } 40 | 41 | double CalcPartialDerivativeYaw(double x, double y, double yaw, double delta_angle) { 42 | return (CalcMatchCost(x, y, yaw + delta_angle) - CalcMatchCost(x, y, yaw)) / delta_angle; 43 | } 44 | 45 | double CalcMatchCost(double x, double y, double yaw) { 46 | int num_matched_point = 0; 47 | int num_all_point = 0; 48 | double sum_square_error_distance = 0.0; // [m^2] 49 | 50 | for (std::size_t i = 0; i < associated_current_laser_points_.size(); ++i) { 51 | const LaserPoint2D& global_associated_reference_laser_point_i = associated_reference_laser_points_[i]; // on global (map) coordinate 52 | const LaserPoint2D& local_associated_current_laser_point_i = associated_current_laser_points_[i]; // on local (robot) coordinate 53 | 54 | if (global_associated_reference_laser_point_i.point_type != PointType::LINE) { 55 | continue; 56 | } 57 | 58 | Pose2D pose_tmp(x, y, yaw); 59 | LaserPoint2D global_associated_current_laser_point_i; 60 | pose_tmp.LocalCoord2GlobalCoord(local_associated_current_laser_point_i, 61 | global_associated_current_laser_point_i); 62 | 63 | double error_distance = CalcErrorDistance(global_associated_reference_laser_point_i, 64 | global_associated_current_laser_point_i); 65 | 66 | if (error_distance < error_distance_threshold_) { 67 | ++num_matched_point; 68 | } 69 | 70 | ++num_all_point; 71 | sum_square_error_distance += error_distance * error_distance; // [m^2] 72 | } 73 | 74 | double num_all_point_double = static_cast(num_all_point) + __DBL_EPSILON__; 75 | 76 | double mean_square_error_distance = sum_square_error_distance / num_all_point_double; 77 | matched_point_ratio_ = static_cast(num_matched_point) / num_all_point_double; 78 | 79 | // Scale up the MSE in order to avoid being too small value 80 | double match_cost = match_cost_scale_ * mean_square_error_distance; 81 | 82 | return match_cost; 83 | } 84 | 85 | virtual double CalcErrorDistance(const LaserPoint2D& associated_reference_laser_point, 86 | const LaserPoint2D& associated_current_laser_point) = 0; 87 | 88 | 89 | protected: 90 | double error_distance_threshold_; 91 | double match_cost_scale_; 92 | double matched_point_ratio_; 93 | 94 | std::vector associated_reference_laser_points_; // on global (map) coordinate 95 | std::vector associated_current_laser_points_; // on local (robot) coordinate 96 | }; 97 | 98 | } // namespace sample_slam 99 | 100 | 101 | #endif // SLAM_COSTFUNCTION_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CostFunction/CostFunction_EuclideanDistance.cpp: -------------------------------------------------------------------------------- 1 | #include "CostFunction_EuclideanDistance.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | double CostFunction_EuclideanDistance::CalcErrorDistance(const LaserPoint2D& associated_reference_laser_point, 7 | const LaserPoint2D& associated_current_laser_point) { 8 | double dx = associated_current_laser_point.x - associated_reference_laser_point.x; 9 | double dy = associated_current_laser_point.y - associated_reference_laser_point.y; 10 | 11 | double euclidean_error_distance = hypot(dx, dy); // [m] 12 | 13 | return euclidean_error_distance; 14 | } 15 | 16 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CostFunction/CostFunction_EuclideanDistance.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_COSTFUNCTION_EUCLIDEANDISTANCE_H_ 2 | #define SLAM_COSTFUNCTION_EUCLIDEANDISTANCE_H_ 3 | 4 | 5 | // slam/ 6 | #include "CostFunction.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | class CostFunction_EuclideanDistance : public CostFunction { 12 | public: 13 | CostFunction_EuclideanDistance() {} 14 | 15 | ~CostFunction_EuclideanDistance() {} 16 | 17 | double CalcErrorDistance(const LaserPoint2D& associated_reference_laser_point, 18 | const LaserPoint2D& associated_current_laser_point) override; 19 | }; 20 | 21 | } // namespace sample_slam 22 | 23 | 24 | #endif // SLAM_COSTFUNCTION_EUCLIDEANDISTANCE_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CostFunction/CostFunction_PerpendicularDistance.cpp: -------------------------------------------------------------------------------- 1 | #include "CostFunction_PerpendicularDistance.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | double CostFunction_PerpendicularDistance::CalcErrorDistance(const LaserPoint2D& associated_reference_laser_point, 7 | const LaserPoint2D& associated_current_laser_point) { 8 | double dx = associated_current_laser_point.x - associated_reference_laser_point.x; 9 | double dy = associated_current_laser_point.y - associated_reference_laser_point.y; 10 | 11 | double perpendicular_error_distance = dx * associated_reference_laser_point.normal_x 12 | + dy * associated_reference_laser_point.normal_y; // [m] 13 | 14 | return perpendicular_error_distance; 15 | } 16 | 17 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CostFunction/CostFunction_PerpendicularDistance.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_COSTFUNCTION_PERPENDICULARDISTANCE_H_ 2 | #define SLAM_COSTFUNCTION_PERPENDICULARDISTANCE_H_ 3 | 4 | 5 | // slam/ 6 | #include "CostFunction.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | class CostFunction_PerpendicularDistance : public CostFunction { 12 | public: 13 | CostFunction_PerpendicularDistance() {} 14 | 15 | ~CostFunction_PerpendicularDistance() {} 16 | 17 | double CalcErrorDistance(const LaserPoint2D& associated_reference_laser_point, 18 | const LaserPoint2D& associated_current_laser_point) override; 19 | }; 20 | 21 | } // namespace sample_slam 22 | 23 | 24 | #endif // SLAM_COSTFUNCTION_PERPENDICULARDISTANCE_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CovarianceCalculator/CovarianceCalculator.cpp: -------------------------------------------------------------------------------- 1 | #include "CovarianceCalculator.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | void CovarianceCalculator::RotateRelative2Absolute(const Pose2D& base_pose, const Eigen::Matrix3f& covariance_relative, 9 | Eigen::Matrix3f& covariance) { 10 | double cos_yaw = cos(base_pose.yaw); 11 | double sin_yaw = sin(base_pose.yaw); 12 | 13 | Eigen::Matrix3f rotation_rel2abs; 14 | rotation_rel2abs << cos_yaw, -sin_yaw, 0.0, 15 | sin_yaw, cos_yaw, 0.0, 16 | 0.0, 0.0, 1.0; 17 | 18 | covariance = rotation_rel2abs * covariance_relative * rotation_rel2abs.transpose(); 19 | 20 | return; 21 | } 22 | 23 | 24 | void CovarianceCalculator::RotateAbsolute2Relative(const Pose2D& base_pose, const Eigen::Matrix3f& covariance, 25 | Eigen::Matrix3f& covariance_relative) { 26 | double cos_yaw = cos(base_pose.yaw); 27 | double sin_yaw = sin(base_pose.yaw); 28 | 29 | Eigen::Matrix3f rotation_abs2rel; 30 | rotation_abs2rel << cos_yaw, sin_yaw, 0.0, 31 | -sin_yaw, cos_yaw, 0.0, 32 | 0.0, 0.0, 1.0; 33 | 34 | covariance_relative = rotation_abs2rel * covariance * rotation_abs2rel.transpose(); 35 | 36 | return; 37 | } 38 | 39 | 40 | void CovarianceCalculator::CalcOdometryCovarianceRelative(const Pose2D& odometry_pose_relative, double delta_time, 41 | Eigen::Matrix3f& odometry_covariance_relative) const { 42 | double v_t = hypot(odometry_pose_relative.x, odometry_pose_relative.y) / delta_time; // [m/s] 43 | double w_t = odometry_pose_relative.yaw / delta_time; // [rad/s] 44 | 45 | v_t = std::max(v_t, min_v_t_); 46 | w_t = std::max(w_t, min_w_t_); 47 | 48 | odometry_covariance_relative = Eigen::Matrix3f::Zero(3, 3); 49 | odometry_covariance_relative(0,0) = odometry_covariance_coefficient_.x * v_t * v_t; 50 | odometry_covariance_relative(1,1) = odometry_covariance_coefficient_.y * v_t * v_t; 51 | odometry_covariance_relative(2,2) = odometry_covariance_coefficient_.yaw * w_t * w_t; 52 | 53 | // Adjusts scale depending on the scale of scan match covariance 54 | odometry_covariance_relative *= odometry_covariance_scale_; 55 | 56 | return; 57 | } 58 | 59 | 60 | void CovarianceCalculator::CalcScanMatchCovarianceICP(const Pose2D& scan_match_pose, 61 | const std::vector& associated_reference_laser_points, 62 | const std::vector& associated_current_laser_points, 63 | CostFunction* cost_function_ptr, 64 | Eigen::Matrix3f& scan_match_covariance) { 65 | double x = scan_match_pose.x; // [m] 66 | double y = scan_match_pose.y; // [m] 67 | double yaw = scan_match_pose.yaw; // [yaw] 68 | 69 | std::vector jacobi_x; // column vector w.r.t. x 70 | std::vector jacobi_y; // column vector w.r.t. y 71 | std::vector jacobi_yaw; // column vector w.r.t. yaw 72 | 73 | for (std::size_t i = 0; i < associated_current_laser_points.size(); ++i) { 74 | const LaserPoint2D& global_associated_reference_laser_point_i = associated_reference_laser_points[i]; // on global (map) coordinate 75 | const LaserPoint2D& local_associated_current_laser_point_i = associated_current_laser_points[i]; // on local (robot) coordinate 76 | 77 | if (global_associated_reference_laser_point_i.point_type == PointType::ISOLATE) { 78 | continue; 79 | } 80 | 81 | double error_distance = CalcErrorDistance(x, y, yaw, 82 | global_associated_reference_laser_point_i, 83 | local_associated_current_laser_point_i, 84 | cost_function_ptr); 85 | 86 | double error_distance_x = CalcErrorDistance(x + delta_distance_, y, yaw, 87 | global_associated_reference_laser_point_i, 88 | local_associated_current_laser_point_i, 89 | cost_function_ptr); 90 | 91 | double error_distance_y = CalcErrorDistance(x, y + delta_distance_, yaw, 92 | global_associated_reference_laser_point_i, 93 | local_associated_current_laser_point_i, 94 | cost_function_ptr); 95 | 96 | double error_distance_yaw = CalcErrorDistance(x, y, yaw + delta_angle_, 97 | global_associated_reference_laser_point_i, 98 | local_associated_current_laser_point_i, 99 | cost_function_ptr); 100 | 101 | float partial_derivative_x = static_cast((error_distance_x - error_distance) / delta_distance_); 102 | float partial_derivative_y = static_cast((error_distance_y - error_distance) / delta_distance_); 103 | float partial_derivative_yaw = static_cast((error_distance_yaw - error_distance) / delta_angle_); 104 | 105 | jacobi_x.push_back(partial_derivative_x); 106 | jacobi_y.push_back(partial_derivative_y); 107 | jacobi_yaw.push_back(partial_derivative_yaw); 108 | } 109 | 110 | // H = J^T * J 111 | Eigen::Matrix3f hess = Eigen::Matrix3f::Zero(3, 3); 112 | for (std::size_t i = 0; i < jacobi_x.size(); ++i) { 113 | hess(0,0) += jacobi_x[i]*jacobi_x[i]; 114 | hess(1,0) += jacobi_y[i]*jacobi_x[i]; hess(1,1) += jacobi_y[i]*jacobi_y[i]; 115 | hess(2,0) += jacobi_yaw[i]*jacobi_x[i]; hess(2,1) += jacobi_yaw[i]*jacobi_y[i]; hess(2,2) += jacobi_yaw[i]*jacobi_yaw[i]; 116 | } 117 | 118 | // H^T = H 119 | hess(0,1) = hess(1,0); hess(0,2) = hess(2,0); 120 | hess(1,2) = hess(2,1); 121 | 122 | // S = H^{-1} 123 | MyUtility::InverseMatrixSVD(hess, scan_match_covariance); 124 | 125 | // Adjusts scale depending on the scale of odometry covariance 126 | scan_match_covariance *= scan_match_covariance_scale_; 127 | 128 | return; 129 | } 130 | 131 | 132 | double CovarianceCalculator::CalcErrorDistance(double x, double y, double yaw, 133 | const LaserPoint2D& global_associated_reference_laser_point, 134 | const LaserPoint2D& local_associated_current_laser_point, 135 | CostFunction* cost_function_ptr) { 136 | Pose2D pose_tmp(x, y, yaw); 137 | LaserPoint2D global_associated_current_laser_point; 138 | pose_tmp.LocalCoord2GlobalCoord(local_associated_current_laser_point, 139 | global_associated_current_laser_point); 140 | 141 | double error_distance = cost_function_ptr->CalcErrorDistance(global_associated_reference_laser_point, 142 | global_associated_current_laser_point); 143 | 144 | return error_distance; 145 | } 146 | 147 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/CovarianceCalculator/CovarianceCalculator.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_COVARIANCECALCULATOR_H_ 2 | #define SLAM_COVARIANCECALCULATOR_H_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | 9 | // slam/ 10 | #include "CostFunction.h" 11 | // utility/ 12 | #include "LaserPoint2D.h" 13 | #include "MyUtility.h" 14 | #include "Pose2D.h" 15 | 16 | 17 | namespace sample_slam { 18 | 19 | class CovarianceCalculator { 20 | public: 21 | CovarianceCalculator() 22 | : delta_distance_(0.00001), 23 | delta_angle_(0.00001), 24 | min_v_t_(0.02), 25 | min_w_t_(0.05), 26 | odometry_covariance_coefficient_(0.001, 0.005, 0.05), 27 | odometry_covariance_scale_(1.0), 28 | scan_match_covariance_scale_(0.1) {} 29 | 30 | ~CovarianceCalculator() {} 31 | 32 | static void RotateRelative2Absolute(const Pose2D& base_pose, const Eigen::Matrix3f& covariance_relative, 33 | Eigen::Matrix3f& covariance); 34 | 35 | static void RotateAbsolute2Relative(const Pose2D& base_pose, const Eigen::Matrix3f& covariance, 36 | Eigen::Matrix3f& covariance_relative); 37 | 38 | void CalcOdometryCovarianceRelative(const Pose2D& odometry_pose_relative, double delta_time, 39 | Eigen::Matrix3f& odometry_covariance_relative) const; 40 | 41 | void CalcScanMatchCovarianceICP(const Pose2D& scan_match_pose, 42 | const std::vector& associated_reference_laser_points, 43 | const std::vector& associated_current_laser_points, 44 | CostFunction* cost_function_ptr, 45 | Eigen::Matrix3f& scan_match_covariance); 46 | 47 | /* 48 | void CalcScanMatchCovarianceNDT(const Pose2D& scan_match_pose, 49 | const std::vector& associated_reference_laser_points, 50 | const std::vector& associated_current_laser_points, 51 | CostFunction* cost_function_ptr, 52 | Eigen::Matrix3f& scan_match_covariance); 53 | */ 54 | 55 | 56 | private: 57 | double CalcErrorDistance(double x, double y, double yaw, 58 | const LaserPoint2D& global_associated_reference_laser_point, 59 | const LaserPoint2D& local_associated_current_laser_point, 60 | CostFunction* cost_function_ptr); 61 | 62 | double delta_distance_; // [m] 63 | double delta_angle_; // [rad] 64 | double min_v_t_; // [m/s] 65 | double min_w_t_; // [rad/s] 66 | Pose2D odometry_covariance_coefficient_; 67 | double odometry_covariance_scale_; 68 | double scan_match_covariance_scale_; 69 | }; 70 | 71 | } // namespace sample_slam 72 | 73 | 74 | #endif // SLAM_COVARIANCECALCULATOR_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/DataAssociator/DataAssociator.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_DATAASSOCIATOR_H_ 2 | #define SLAM_DATAASSOCIATOR_H_ 3 | 4 | 5 | #include 6 | #include 7 | 8 | // utility/ 9 | #include "LaserPoint2D.h" 10 | #include "Pose2D.h" 11 | 12 | 13 | namespace sample_slam { 14 | 15 | class DataAssociator { 16 | public: 17 | DataAssociator() {} 18 | 19 | virtual ~DataAssociator() {} 20 | 21 | void SetCurrentLaserPoints(const std::vector& curr_laser_points) { 22 | curr_laser_points_ = curr_laser_points; // on local (robot) coordinate 23 | } 24 | 25 | const std::vector& GetAssociatedReferenceLaserPoints() const { 26 | return associated_reference_laser_points_; 27 | } 28 | 29 | const std::vector& GetAssociatedCurrentLaserPoints() const { 30 | return associated_current_laser_points_; 31 | } 32 | 33 | virtual void SetReferenceLaserPoints(const std::vector& reference_laser_points) = 0; 34 | 35 | virtual double AssociateCorrespondingLaserPoints(const Pose2D& pre_optimize_pose) = 0; 36 | 37 | 38 | protected: 39 | std::vector curr_laser_points_; // on local (robot) coordinate 40 | 41 | std::vector associated_reference_laser_points_; // on global (map) coordinate 42 | std::vector associated_current_laser_points_; // on local (robot) coordinate 43 | }; 44 | 45 | } // namespace sample_slam 46 | 47 | 48 | #endif // SLAM_DATAASSOCIATOR_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/DataAssociator/DataAssociator_GridTable.cpp: -------------------------------------------------------------------------------- 1 | #include "DataAssociator_GridTable.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | void DataAssociator_GridTable::SetReferenceLaserPoints(const std::vector& reference_laser_points) { 7 | grid_table_.clear(); // on global (map) coordinate 8 | for (std::size_t i = 0; i < reference_laser_points.size(); ++i) { 9 | grid_table_.AddLaserPoint(&(reference_laser_points[i])); 10 | } 11 | 12 | return; 13 | } 14 | 15 | 16 | double DataAssociator_GridTable::AssociateCorrespondingLaserPoints(const Pose2D& pre_optimize_pose) { 17 | associated_reference_laser_points_.clear(); // on global (map) coordinate 18 | associated_current_laser_points_.clear(); // on local (robot) coordinate 19 | 20 | for (std::size_t i = 0; i < curr_laser_points_.size(); ++i) { 21 | LaserPoint2D& local_curr_laser_point = curr_laser_points_[i]; 22 | 23 | LaserPoint2D global_curr_laser_point; 24 | pre_optimize_pose.LocalCoord2GlobalCoord(local_curr_laser_point, global_curr_laser_point); 25 | 26 | const LaserPoint2D* reference_laser_point_ptr = grid_table_.FindNearestLaserPoint(global_curr_laser_point); 27 | 28 | if (reference_laser_point_ptr != nullptr) { 29 | associated_reference_laser_points_.emplace_back(*reference_laser_point_ptr); // on global (map) coordinate 30 | associated_current_laser_points_.emplace_back(local_curr_laser_point); // on local (robot) coordinate 31 | } 32 | } 33 | 34 | double association_ratio = static_cast(associated_current_laser_points_.size()) 35 | / static_cast(curr_laser_points_.size()); 36 | 37 | return association_ratio; 38 | } 39 | 40 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/DataAssociator/DataAssociator_GridTable.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_DATAASSOCIATOR_GRIDTABLE_H_ 2 | #define SLAM_DATAASSOCIATOR_GRIDTABLE_H_ 3 | 4 | 5 | // slam/ 6 | #include "DataAssociator.h" 7 | // utility/ 8 | #include "GridTable2D.h" 9 | 10 | 11 | namespace sample_slam { 12 | 13 | class DataAssociator_GridTable : public DataAssociator { 14 | public: 15 | DataAssociator_GridTable() {} 16 | 17 | ~DataAssociator_GridTable() {} 18 | 19 | void SetReferenceLaserPoints(const std::vector& reference_laser_points) override; 20 | 21 | double AssociateCorrespondingLaserPoints(const Pose2D& pre_optimize_pose) override; 22 | 23 | private: 24 | GridTable2D grid_table_; // on global (map) coordinate 25 | }; 26 | 27 | } // namespace sample_slam 28 | 29 | 30 | #endif // SLAM_DATAASSOCIATOR_GRIDTABLE_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/DataAssociator/DataAssociator_LinearSearch.cpp: -------------------------------------------------------------------------------- 1 | #include "DataAssociator_LinearSearch.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | void DataAssociator_LinearSearch::SetReferenceLaserPoints(const std::vector& reference_laser_points) { 9 | reference_laser_points_ = reference_laser_points; // on global (map) coordinate 10 | 11 | return; 12 | } 13 | 14 | 15 | double DataAssociator_LinearSearch::AssociateCorrespondingLaserPoints(const Pose2D& pre_optimize_pose) { 16 | associated_reference_laser_points_.clear(); // on global (map) coordinate 17 | associated_current_laser_points_.clear(); // on local (robot) coordinate 18 | 19 | for (std::size_t i = 0; i < curr_laser_points_.size(); ++i) { 20 | LaserPoint2D& local_curr_laser_point_i = curr_laser_points_[i]; 21 | 22 | LaserPoint2D global_curr_laser_point; 23 | pre_optimize_pose.LocalCoord2GlobalCoord(local_curr_laser_point_i, global_curr_laser_point); 24 | 25 | double min_distance = __DBL_MAX__; 26 | LaserPoint2D* reference_laser_point_j_ptr = nullptr; 27 | 28 | for (std::size_t j = 0; j < reference_laser_points_.size(); ++j) { 29 | double dx = global_curr_laser_point.x - reference_laser_points_[j].x; 30 | double dy = global_curr_laser_point.y - reference_laser_points_[j].y; 31 | double distance = hypot(dx, dy); 32 | 33 | if (distance < distance_threshold_ && distance < min_distance) { 34 | min_distance = distance; 35 | reference_laser_point_j_ptr = &(reference_laser_points_[j]); 36 | } 37 | } 38 | 39 | if (reference_laser_point_j_ptr != nullptr) { 40 | associated_reference_laser_points_.emplace_back(*reference_laser_point_j_ptr); // on global (map) coordinate 41 | associated_current_laser_points_.emplace_back(local_curr_laser_point_i); // on local (robot) coordinate 42 | } 43 | } 44 | 45 | double association_ratio = static_cast(associated_current_laser_points_.size()) 46 | / static_cast(curr_laser_points_.size()); 47 | 48 | return association_ratio; 49 | } 50 | 51 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/DataAssociator/DataAssociator_LinearSearch.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_DATAASSOCIATOR_LINEARSEARCH_H_ 2 | #define SLAM_DATAASSOCIATOR_LINEARSEARCH_H_ 3 | 4 | 5 | // slam/ 6 | #include "DataAssociator.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | class DataAssociator_LinearSearch : public DataAssociator { 12 | public: 13 | DataAssociator_LinearSearch() 14 | : distance_threshold_(0.2) {} 15 | 16 | ~DataAssociator_LinearSearch() {} 17 | 18 | void SetReferenceLaserPoints(const std::vector& reference_laser_points) override; 19 | 20 | double AssociateCorrespondingLaserPoints(const Pose2D& pre_optimize_pose) override; 21 | 22 | private: 23 | std::vector reference_laser_points_; // on global (map) coordinate 24 | double distance_threshold_; 25 | }; 26 | 27 | } // namespace sample_slam 28 | 29 | 30 | #endif // SLAM_DATAASSOCIATOR_LINEARSEARCH_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/LoopDetector/LoopDetector.cpp: -------------------------------------------------------------------------------- 1 | #include "LoopDetector.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | bool LoopDetector::DetectLoop(const Scan2D& curr_scan, const Pose2D& estimated_pose, int count, 9 | LoopInfo& loop_info) { 10 | double cumulative_distance = 0.0; // [m] 11 | Pose2D prev_pose(0.0, 0.0, 0.0); // [m], [m], [rad] 12 | 13 | double min_distance = __DBL_MAX__; // [m] 14 | int min_sub_map_idx; 15 | int min_revisit_candidate_idx; 16 | 17 | const std::vector& sub_maps = point_cloud_map_ptr_->GetSubMaps(); 18 | const std::vector& map_poses = point_cloud_map_ptr_->GetPoses(); 19 | double map_cumulative_distance = point_cloud_map_ptr_->GetCumulativeDistance(); 20 | 21 | for (std::size_t i = 0; i < sub_maps.size() - 1; ++i) { 22 | for (std::size_t j = sub_maps[i].scan_id_start; j <= sub_maps[i].scan_id_end; ++j) { 23 | cumulative_distance += hypot(map_poses[j].x - prev_pose.x, map_poses[j].y - prev_pose.y); 24 | if ((map_cumulative_distance - cumulative_distance) < cumulative_distance_threshold_) { 25 | i = sub_maps.size(); // in order to break i's loop 26 | break; // in order to break j's loop 27 | } 28 | 29 | double distance = hypot(estimated_pose.x - map_poses[j].x, estimated_pose.y - map_poses[j].y); 30 | if (distance < min_distance) { 31 | min_distance = distance; 32 | min_sub_map_idx = i; 33 | min_revisit_candidate_idx = j; 34 | } 35 | 36 | prev_pose = map_poses[j]; 37 | } 38 | } 39 | 40 | std::cout << "[LoopDetector::DetectLoop()] " 41 | << "min_distance = " << min_distance << ", min_distance_threshold_ = " << min_distance_threshold_ 42 | << ", min_sub_map_idx = " << min_sub_map_idx 43 | << ", min_revisit_candidate_idx = " << min_revisit_candidate_idx << "\n"; 44 | 45 | if (min_distance_threshold_ < min_distance) { 46 | return false; 47 | } 48 | 49 | Scan2D reference_scan; 50 | reference_scan.laser_points = sub_maps[min_sub_map_idx].map_laser_points; 51 | 52 | Pose2D revisit_pose; 53 | bool is_loop_detected = SearchRevisitPose(reference_scan, curr_scan, estimated_pose, 54 | revisit_pose); 55 | 56 | if (is_loop_detected) { 57 | Eigen::Matrix3f scan_match_covariance; 58 | pose_estimator_ptr_->CalcScanMatchCovariance(reference_scan, curr_scan, revisit_pose, 59 | scan_match_covariance); 60 | 61 | loop_info.reference_scan_id = min_revisit_candidate_idx; 62 | loop_info.curr_scan_id = count; 63 | loop_info.revisit_pose = revisit_pose; 64 | loop_info.scan_match_covariance = scan_match_covariance; // on absolute (map) coordinate 65 | } 66 | 67 | return is_loop_detected; 68 | } 69 | 70 | 71 | bool LoopDetector::SearchRevisitPose(const Scan2D& reference_scan, const Scan2D& curr_scan, const Pose2D& estimated_pose, 72 | Pose2D& revisit_pose) { 73 | data_associator_ptr_->SetReferenceLaserPoints(reference_scan.laser_points); 74 | data_associator_ptr_->SetCurrentLaserPoints(curr_scan.laser_points); 75 | 76 | std::vector candidate_poses; 77 | for (double dx = -revisit_max_distance_; dx <= revisit_max_distance_; dx += revisit_delta_distance_) { 78 | double search_x = estimated_pose.x + dx; 79 | 80 | for (double dy = -revisit_max_distance_; dy <= revisit_max_distance_; dy += revisit_delta_distance_) { 81 | double search_y = estimated_pose.y + dy; 82 | 83 | for (double dyaw = -revisit_max_angle_; dyaw <= revisit_max_angle_; dyaw += revisit_delta_angle_) { 84 | double search_yaw = MyUtility::AddRadian(estimated_pose.yaw, dyaw); 85 | 86 | Pose2D search_pose(search_x, search_y, search_yaw); 87 | double association_ratio = data_associator_ptr_->AssociateCorrespondingLaserPoints(search_pose); 88 | int num_used_point = data_associator_ptr_->GetAssociatedCurrentLaserPoints().size(); 89 | 90 | if (association_ratio < association_ratio_threshold_ || num_used_point < num_used_point_threshold_) { 91 | continue; 92 | } 93 | 94 | cost_function_ptr_->SetAssociatedLaserPoints(data_associator_ptr_->GetAssociatedReferenceLaserPoints(), 95 | data_associator_ptr_->GetAssociatedCurrentLaserPoints()); 96 | 97 | double match_cost_tmp = cost_function_ptr_->CalcMatchCost(search_x, search_y, search_yaw); 98 | double matched_point_ratio = cost_function_ptr_->GetMatchedPointRatio(); 99 | 100 | if (matched_point_ratio < matched_point_ratio_threshold_) { 101 | continue; 102 | } 103 | 104 | candidate_poses.emplace_back(search_pose); 105 | } 106 | } 107 | } 108 | 109 | std::cout << "[LoopDetector::SearchRevisitPose()] " 110 | << "candidate_poses.size() = " << candidate_poses.size() << "\n"; 111 | 112 | if (candidate_poses.size() == 0) { 113 | return false; 114 | } 115 | 116 | pose_estimator_ptr_->SetReferenceScan(reference_scan); 117 | pose_estimator_ptr_->SetCurrentScan(curr_scan); 118 | 119 | double best_match_cost = __DBL_MAX__; 120 | Pose2D best_scan_match_pose; 121 | for (std::size_t i = 0; i < candidate_poses.size(); ++i) { 122 | pose_estimator_ptr_->EstimatePose(candidate_poses[i]); 123 | 124 | double matched_point_ratio = cost_function_ptr_->GetMatchedPointRatio(); 125 | int num_used_point = pose_estimator_ptr_->GetNumUsedPoint(); 126 | if (matched_point_ratio < 1.1*matched_point_ratio_threshold_ && num_used_point < num_used_point_threshold_) { 127 | continue; 128 | } 129 | 130 | double min_match_cost = pose_estimator_ptr_->GetMinMatchCost(); 131 | if (min_match_cost < best_match_cost) { 132 | best_match_cost = min_match_cost; 133 | best_scan_match_pose = pose_estimator_ptr_->GetEstimatedPose(); 134 | 135 | std::cout << "[LoopDetector::SearchRevisitPose()] " 136 | << "best_match_cost = " << best_match_cost 137 | << ", matched_point_ratio = " << matched_point_ratio 138 | << ", num_used_point = " << num_used_point << "\n"; 139 | } 140 | } 141 | 142 | if (best_match_cost < best_match_cost_threshold_) { 143 | revisit_pose = best_scan_match_pose; 144 | return true; 145 | } 146 | 147 | return false; 148 | } 149 | 150 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/LoopDetector/LoopDetector.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_LOOPDETECTOR_H_ 2 | #define SLAM_LOOPDETECTOR_H_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | 9 | // slam/ 10 | #include "CostFunction_PerpendicularDistance.h" 11 | #include "DataAssociator_GridTable.h" 12 | #include "PointCloudMap_SubMap.h" 13 | #include "PoseEstimator.h" 14 | #include "PoseGraph.h" 15 | // utility/ 16 | #include "MyUtility.h" 17 | #include "Pose2D.h" 18 | #include "Scan2D.h" 19 | 20 | 21 | namespace sample_slam { 22 | 23 | struct LoopInfo { 24 | bool is_arc; 25 | int reference_scan_id; 26 | int curr_scan_id; 27 | Pose2D revisit_pose; 28 | Eigen::Matrix3f scan_match_covariance; 29 | }; 30 | 31 | 32 | class LoopDetector { 33 | public: 34 | LoopDetector() 35 | : min_distance_threshold_(4.0), 36 | cumulative_distance_threshold_(10.0), 37 | best_match_cost_threshold_(0.2), 38 | revisit_max_distance_(1.0), 39 | revisit_delta_distance_(0.2), 40 | revisit_max_angle_(DEG2RAD(45.0)), 41 | revisit_delta_angle_(DEG2RAD(2.0)), 42 | num_used_point_threshold_(50), 43 | association_ratio_threshold_(0.9), 44 | matched_point_ratio_threshold_(0.8) {} 45 | 46 | ~LoopDetector() {} 47 | 48 | void SetPointCloudMapPtr(PointCloudMap_SubMap* point_cloud_map_ptr) { 49 | point_cloud_map_ptr_ = point_cloud_map_ptr; 50 | } 51 | 52 | void SetDataAssociatorPtr(DataAssociator_GridTable* data_associator_ptr) { 53 | data_associator_ptr_ = data_associator_ptr; 54 | } 55 | 56 | void SetCostFunctionPtr(CostFunction_PerpendicularDistance* cost_function_ptr) { 57 | cost_function_ptr_ = cost_function_ptr; 58 | } 59 | 60 | void SetPoseEstimatorPtr(PoseEstimator* pose_estimator_ptr) { 61 | pose_estimator_ptr_ = pose_estimator_ptr; 62 | } 63 | 64 | void SetPoseGraphPtr(PoseGraph* pose_graph_ptr) { 65 | pose_graph_ptr_ = pose_graph_ptr; 66 | } 67 | 68 | bool DetectLoop(const Scan2D& curr_scan, const Pose2D& estimated_pose, int count, 69 | LoopInfo& loop_info); 70 | 71 | 72 | private: 73 | bool SearchRevisitPose(const Scan2D& reference_scan, const Scan2D& curr_scan, const Pose2D& estimated_pose, 74 | Pose2D& revisit_pose); 75 | 76 | PoseEstimator* pose_estimator_ptr_; // Pointer to abstract class, which is customizable 77 | 78 | PointCloudMap_SubMap* point_cloud_map_ptr_; 79 | DataAssociator_GridTable* data_associator_ptr_; 80 | CostFunction_PerpendicularDistance* cost_function_ptr_; 81 | PoseGraph* pose_graph_ptr_; 82 | 83 | double min_distance_threshold_; // [m] 84 | double cumulative_distance_threshold_; // [m] 85 | double best_match_cost_threshold_; 86 | double revisit_max_distance_; // [m] 87 | double revisit_delta_distance_; // [m] 88 | double revisit_max_angle_; // [rad] 89 | double revisit_delta_angle_; // [rad] 90 | int num_used_point_threshold_; 91 | double association_ratio_threshold_; 92 | double matched_point_ratio_threshold_; 93 | }; 94 | 95 | } // namespace sample_slam 96 | 97 | 98 | #endif // SLAM_LOOPDETECTOR_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PointCloudMap/PointCloudMap.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POINTCLOUDMAP_H_ 2 | #define SLAM_POINTCLOUDMAP_H_ 3 | 4 | 5 | #include 6 | 7 | // utility/ 8 | #include "LaserPoint2D.h" 9 | #include "Pose2D.h" 10 | #include "Scan2D.h" 11 | 12 | 13 | namespace sample_slam { 14 | 15 | // Abstract class of `PointCloudMap` 16 | class PointCloudMap { 17 | public: 18 | PointCloudMap() : max_num_point_(1000000), num_point_threshold_(1) { 19 | global_map_.reserve(max_num_point_); 20 | } 21 | 22 | virtual ~PointCloudMap() {} 23 | 24 | void SetNumPointThreshold(int num_point_threshold) { 25 | num_point_threshold_ = num_point_threshold; 26 | } 27 | 28 | void SetLastPose(const Pose2D& last_pose) { 29 | last_pose_ = last_pose; 30 | } 31 | 32 | void SetLastScan(const Scan2D& last_scan) { 33 | last_scan_ = last_scan; 34 | } 35 | 36 | const std::vector& GetGlobalMap() const { 37 | return global_map_; 38 | } 39 | 40 | const std::vector& GetLocalMap() const { 41 | return local_map_; 42 | } 43 | 44 | const std::vector& GetPoses() const { 45 | return poses_; 46 | } 47 | 48 | const Pose2D& GetLastPose() const { 49 | return last_pose_; 50 | } 51 | 52 | const Scan2D& GetLastScan() const { 53 | return last_scan_; 54 | } 55 | 56 | virtual void AddPose(const Pose2D& pose) = 0; 57 | 58 | virtual void AddLaserPoints(const std::vector& laser_points) = 0; 59 | 60 | virtual void MakeGlobalMap() = 0; 61 | 62 | virtual void MakeLocalMap() = 0; 63 | 64 | virtual void RemakeMap(const std::vector& adjusted_poses) = 0; 65 | 66 | 67 | protected: 68 | std::vector global_map_; 69 | std::vector local_map_; 70 | 71 | std::vector poses_; 72 | Pose2D last_pose_; 73 | Scan2D last_scan_; 74 | 75 | int max_num_point_; 76 | int num_point_threshold_; // for `PointCloudMap_GridTable` and `PointCloudMap_SubMap` 77 | }; 78 | 79 | } // namespace sample_slam 80 | 81 | 82 | #endif // SLAM_POINTCLOUDMAP_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PointCloudMap/PointCloudMap_EntirePoint.cpp: -------------------------------------------------------------------------------- 1 | #include "PointCloudMap_EntirePoint.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | void PointCloudMap_EntirePoint::AddPose(const Pose2D& pose) { 7 | poses_.emplace_back(pose); 8 | 9 | return; 10 | } 11 | 12 | 13 | void PointCloudMap_EntirePoint::AddLaserPoints(const std::vector& laser_points) { 14 | for(std::size_t i = 0; i < laser_points.size(); i += num_skip_laser_point_) { 15 | global_map_.emplace_back(laser_points[i]); 16 | } 17 | 18 | return; 19 | } 20 | 21 | 22 | void PointCloudMap_EntirePoint::MakeGlobalMap() { 23 | // Dummy 24 | } 25 | 26 | 27 | void PointCloudMap_EntirePoint::MakeLocalMap() { 28 | // Dummy 29 | } 30 | 31 | 32 | void PointCloudMap_EntirePoint::RemakeMap(const std::vector& adjusted_poses) { 33 | // Dummy 34 | } 35 | 36 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PointCloudMap/PointCloudMap_EntirePoint.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POINTCLOUDMAP_ENTIREPOINT_H_ 2 | #define SLAM_POINTCLOUDMAP_ENTIREPOINT_H_ 3 | 4 | 5 | // slam/ 6 | #include "PointCloudMap.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | // Concrete class of `PointCloudMap` that stores entire laser point 12 | class PointCloudMap_EntirePoint : public PointCloudMap { 13 | public: 14 | PointCloudMap_EntirePoint() : num_skip_laser_point_(5) {} 15 | 16 | ~PointCloudMap_EntirePoint() {} 17 | 18 | void AddPose(const Pose2D& pose) override; 19 | 20 | void AddLaserPoints(const std::vector& laser_points) override; 21 | 22 | void MakeGlobalMap() override; 23 | 24 | void MakeLocalMap() override; 25 | 26 | void RemakeMap(const std::vector& adjusted_poses) override; 27 | 28 | 29 | private: 30 | int num_skip_laser_point_; 31 | }; 32 | 33 | } // namespace sample_slam 34 | 35 | 36 | #endif // SLAM_POINTCLOUDMAP_ENTIREPOINT_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PointCloudMap/PointCloudMap_GridTable.cpp: -------------------------------------------------------------------------------- 1 | #include "PointCloudMap_GridTable.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | void PointCloudMap_GridTable::AddPose(const Pose2D& pose) { 9 | poses_.emplace_back(pose); 10 | 11 | return; 12 | } 13 | 14 | 15 | void PointCloudMap_GridTable::AddLaserPoints(const std::vector& laser_points) { 16 | for(std::size_t i = 0; i < laser_points.size(); ++i) { 17 | all_laser_points_.emplace_back(laser_points[i]); 18 | } 19 | 20 | return; 21 | } 22 | 23 | 24 | void PointCloudMap_GridTable::MakeGlobalMap() { 25 | global_map_.clear(); 26 | SubsampleMapPoints(global_map_); 27 | 28 | std::cout << "[PointCloudMap_GridTable::MakeGlobalMap()] " 29 | << "global_map_.size() = " << global_map_.size() << "\n"; 30 | } 31 | 32 | 33 | void PointCloudMap_GridTable::MakeLocalMap() { 34 | local_map_ = global_map_; 35 | 36 | std::cout << "[PointCloudMap_GridTable::MakeLocalMap()] " 37 | << "local_map_.size() = " << local_map_.size() << "\n"; 38 | } 39 | 40 | 41 | void PointCloudMap_GridTable::RemakeMap(const std::vector& adjusted_poses) { 42 | // Dummy 43 | } 44 | 45 | 46 | void PointCloudMap_GridTable::SubsampleMapPoints(std::vector& map) { 47 | // auto start_clear = std::chrono::high_resolution_clock::now(); 48 | grid_table_.clear(); 49 | // auto end_clear = std::chrono::high_resolution_clock::now(); 50 | // auto duration_clear = std::chrono::duration_cast(end_clear - start_clear); 51 | // std::cout << "duration_clear.count() = " << duration_clear.count() << " [ms] \n"; 52 | 53 | // auto start_AddLaserPoints = std::chrono::high_resolution_clock::now(); 54 | for (std::size_t i = 0; i < all_laser_points_.size(); ++i) { 55 | grid_table_.AddLaserPoint(all_laser_points_[i]); 56 | } 57 | // auto end_AddLaserPoints = std::chrono::high_resolution_clock::now(); 58 | // auto duration_AddLaserPoints = std::chrono::duration_cast(end_AddLaserPoints - start_AddLaserPoints); 59 | // std::cout << "duration_AddLaserPoints.count() = " << duration_AddLaserPoints.count() << " [ms] \n"; 60 | 61 | // auto start_MakeCellPoints = std::chrono::high_resolution_clock::now(); 62 | grid_table_.MakeCellPoints(num_point_threshold_, map); 63 | // auto end_MakeCellPoints = std::chrono::high_resolution_clock::now(); 64 | // auto duration_MakeCellPoints = std::chrono::duration_cast(end_MakeCellPoints - start_MakeCellPoints); 65 | // std::cout << "duration_MakeCellPoints.count() = " << duration_MakeCellPoints.count() << " [ms] \n"; 66 | 67 | std::cout << "[PointCloudMap_GridTable::SubsampleMapPoints()] " 68 | << "all_laser_points_.size() = " << all_laser_points_.size() 69 | << ", map.size() = " << map.size() << "\n"; 70 | 71 | return; 72 | } 73 | 74 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PointCloudMap/PointCloudMap_GridTable.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POINTCLOUDMAP_GRIDTABLE_H_ 2 | #define SLAM_POINTCLOUDMAP_GRIDTABLE_H_ 3 | 4 | 5 | #include 6 | 7 | // slam/ 8 | #include "PointCloudMap.h" 9 | // utility/ 10 | #include "GridTable2D.h" 11 | #include "LaserPoint2D.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | // Concrete class of `PointCloudMap` that stores laser points in a grid table 17 | class PointCloudMap_GridTable : public PointCloudMap { 18 | public: 19 | PointCloudMap_GridTable() { 20 | all_laser_points_.reserve(max_num_point_); 21 | } 22 | 23 | ~PointCloudMap_GridTable() {} 24 | 25 | void AddPose(const Pose2D& pose) override; 26 | 27 | void AddLaserPoints(const std::vector& laser_points) override; 28 | 29 | void MakeGlobalMap() override; 30 | 31 | void MakeLocalMap() override; 32 | 33 | void RemakeMap(const std::vector& adjusted_poses) override; 34 | 35 | 36 | private: 37 | void SubsampleMapPoints(std::vector& map); 38 | 39 | std::vector all_laser_points_; 40 | GridTable2D grid_table_; 41 | }; 42 | 43 | } // namespace sample_slam 44 | 45 | 46 | #endif // SLAM_POINTCLOUDMAP_GRIDTABLE_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PointCloudMap/PointCloudMap_SubMap.cpp: -------------------------------------------------------------------------------- 1 | #include "PointCloudMap_SubMap.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | void PointCloudMap_SubMap::AddPose(const Pose2D& pose) { 7 | if (poses_.size() == 0) { 8 | cumulative_distance_ += hypot(pose.x, pose.y); 9 | } else if (0 < poses_.size()) { 10 | Pose2D last_pose = poses_.back(); 11 | double dx = pose.x - last_pose.x; 12 | double dy = pose.y - last_pose.y; 13 | cumulative_distance_ += hypot(dx, dy); 14 | } 15 | 16 | poses_.emplace_back(pose); 17 | 18 | return; 19 | } 20 | 21 | 22 | void PointCloudMap_SubMap::AddLaserPoints(const std::vector& laser_points) { 23 | SubMap* curr_sub_map_ptr = &(sub_maps_.back()); 24 | 25 | double delta_cumulative_distance = cumulative_distance_ - curr_sub_map_ptr->cumulative_distance_start; 26 | 27 | if (cumulative_distance_threshold_ <= delta_cumulative_distance) { 28 | curr_sub_map_ptr->scan_id_end = poses_.size() - 1; 29 | curr_sub_map_ptr->map_laser_points = curr_sub_map_ptr->SubsampleMapPoints(num_point_threshold_); 30 | 31 | SubMap new_sub_map; 32 | new_sub_map.cumulative_distance_start = cumulative_distance_; 33 | new_sub_map.scan_id_start = poses_.size(); 34 | new_sub_map.AddLaserPoints(laser_points); 35 | 36 | sub_maps_.emplace_back(new_sub_map); 37 | } else { 38 | curr_sub_map_ptr->AddLaserPoints(laser_points); 39 | } 40 | 41 | return; 42 | } 43 | 44 | 45 | void PointCloudMap_SubMap::MakeGlobalMap() { 46 | global_map_.clear(); 47 | local_map_.clear(); 48 | 49 | for (std::size_t i = 0; i < (sub_maps_.size() - 1); ++i) { 50 | std::vector& sub_map_i_map_laser_points = sub_maps_[i].map_laser_points; 51 | 52 | for (std::size_t j = 0; j < sub_map_i_map_laser_points.size(); ++j) { 53 | global_map_.emplace_back(sub_map_i_map_laser_points[j]); 54 | } 55 | 56 | // Make local map as well using the last sub map 57 | if (i == (sub_maps_.size() - 2)) { 58 | for (std::size_t j = 0; j < sub_map_i_map_laser_points.size(); ++j) { 59 | local_map_.emplace_back(sub_map_i_map_laser_points[j]); 60 | } 61 | } 62 | } 63 | 64 | SubMap* curr_sub_map_ptr = &(sub_maps_.back()); 65 | std::vector subsampled_map_laser_points = curr_sub_map_ptr->SubsampleMapPoints(num_point_threshold_); 66 | for (std::size_t i = 0; i < subsampled_map_laser_points.size(); ++i) { 67 | global_map_.emplace_back(subsampled_map_laser_points[i]); 68 | local_map_.emplace_back(subsampled_map_laser_points[i]); 69 | } 70 | 71 | std::cout << "[PointCloudMap_SubMap::MakeGlobalMap()] \n" 72 | << "curr_sub_map_ptr->cumulative_distance_start = " << curr_sub_map_ptr->cumulative_distance_start 73 | << ",cumulative_distance_ = " << cumulative_distance_ 74 | << ", subsampled_map_laser_points.size() = " << subsampled_map_laser_points.size() << "\n" 75 | << "sub_maps_.size() = " << sub_maps_.size() << ", global_map_.size() = " << global_map_.size() << "\n"; 76 | 77 | return; 78 | } 79 | 80 | 81 | void PointCloudMap_SubMap::MakeLocalMap() { 82 | local_map_.clear(); 83 | 84 | if (2 <= sub_maps_.size()) { 85 | SubMap* last_sub_map_ptr = &(sub_maps_[sub_maps_.size() - 2]); 86 | for (std::size_t i = 0; i < last_sub_map_ptr->map_laser_points.size(); ++i) { 87 | local_map_.emplace_back(last_sub_map_ptr->map_laser_points[i]); 88 | } 89 | } 90 | 91 | SubMap* curr_sub_map_ptr = &(sub_maps_.back()); 92 | std::vector subsampled_map_laser_points = curr_sub_map_ptr->SubsampleMapPoints(num_point_threshold_); 93 | for (std::size_t i = 0; i < subsampled_map_laser_points.size(); ++i) { 94 | local_map_.emplace_back(subsampled_map_laser_points[i]); 95 | } 96 | 97 | std::cout << "[PointCloudMap_SubMap::MakeLocalMap()] " 98 | << "local_map_.size() = " << local_map_.size() << "\n"; 99 | 100 | return; 101 | } 102 | 103 | 104 | void PointCloudMap_SubMap::RemakeMap(const std::vector& adjusted_poses) { 105 | for (std::size_t i = 0; i < sub_maps_.size(); ++i) { 106 | std::vector& sub_map_i_map_laser_points = sub_maps_[i].map_laser_points; 107 | for (std::size_t j = 0; j < sub_map_i_map_laser_points.size(); ++j) { 108 | LaserPoint2D& laser_point_j = sub_map_i_map_laser_points[j]; 109 | if (poses_.size() <= laser_point_j.id) { 110 | continue; 111 | } 112 | 113 | const Pose2D& old_pose = poses_[laser_point_j.id]; 114 | const Pose2D& new_pose = adjusted_poses[laser_point_j.id]; 115 | 116 | LaserPoint2D old_local_laser_point_j; 117 | old_pose.GlobalCoord2LocalCoord(laser_point_j, old_local_laser_point_j); 118 | old_pose.InverseRotateNormalVector(laser_point_j, old_local_laser_point_j); 119 | 120 | LaserPoint2D new_global_laser_point_j; 121 | new_pose.LocalCoord2GlobalCoord(old_local_laser_point_j, new_global_laser_point_j); 122 | new_pose.RotateNormalVector(old_local_laser_point_j, new_global_laser_point_j); 123 | 124 | laser_point_j.x = new_global_laser_point_j.x; 125 | laser_point_j.y = new_global_laser_point_j.y; 126 | laser_point_j.normal_x = new_global_laser_point_j.normal_x; 127 | laser_point_j.normal_y = new_global_laser_point_j.normal_y; 128 | } 129 | } 130 | 131 | MakeGlobalMap(); 132 | 133 | for (std::size_t i = 0; i < poses_.size(); ++i) { 134 | poses_[i] = adjusted_poses[i]; 135 | } 136 | last_pose_ = adjusted_poses.back(); 137 | 138 | return; 139 | } 140 | 141 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PointCloudMap/PointCloudMap_SubMap.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POINTCLOUDMAP_SUBMAP_H_ 2 | #define SLAM_POINTCLOUDMAP_SUBMAP_H_ 3 | 4 | 5 | #include 6 | 7 | // slam/ 8 | #include "PointCloudMap.h" 9 | // utility/ 10 | #include "GridTable2D.h" 11 | #include "LaserPoint2D.h" 12 | #include "Pose2D.h" 13 | 14 | 15 | namespace sample_slam { 16 | 17 | struct SubMap { 18 | double cumulative_distance_start; 19 | int scan_id_start; 20 | int scan_id_end; 21 | std::vector map_laser_points; 22 | 23 | void AddLaserPoints(const std::vector& laser_points) { 24 | for (std::size_t i = 0; i < laser_points.size(); ++i) { 25 | map_laser_points.emplace_back(laser_points[i]); 26 | } 27 | 28 | return; 29 | } 30 | 31 | std::vector SubsampleMapPoints(int num_point_threshold) { 32 | GridTable2D grid_table; 33 | for (std::size_t i = 0; i < map_laser_points.size(); ++i) { 34 | grid_table.AddLaserPoint(map_laser_points[i]); 35 | } 36 | 37 | std::vector subsampled_map_laser_points; 38 | grid_table.MakeCellPoints(num_point_threshold, subsampled_map_laser_points); 39 | 40 | std::cout << "[SubMap::SubsampleMapPoints()] " 41 | << "map_laser_points.size() = " << map_laser_points.size() 42 | << ", subsampled_map_laser_points.size() = " << subsampled_map_laser_points.size() << "\n"; 43 | 44 | return subsampled_map_laser_points; 45 | } 46 | }; 47 | 48 | 49 | // Concrete class of `PointCloudMap` that stores laser points in a grid table 50 | class PointCloudMap_SubMap : public PointCloudMap { 51 | public: 52 | PointCloudMap_SubMap() 53 | : cumulative_distance_threshold_(10.0), 54 | cumulative_distance_(0.0) { 55 | SubMap sub_map; 56 | sub_maps_.emplace_back(sub_map); 57 | } 58 | 59 | ~PointCloudMap_SubMap() {} 60 | 61 | const std::vector& GetSubMaps() const { 62 | return sub_maps_; 63 | } 64 | 65 | double GetCumulativeDistance() const { 66 | return cumulative_distance_; 67 | } 68 | 69 | void AddPose(const Pose2D& pose) override; 70 | 71 | void AddLaserPoints(const std::vector& laser_points) override; 72 | 73 | void MakeGlobalMap() override; 74 | 75 | void MakeLocalMap() override; 76 | 77 | void RemakeMap(const std::vector& adjusted_poses) override; 78 | 79 | 80 | private: 81 | double cumulative_distance_threshold_; 82 | double cumulative_distance_; 83 | std::vector sub_maps_; 84 | }; 85 | 86 | } // namespace sample_slam 87 | 88 | 89 | #endif // SLAM_POINTCLOUDMAP_SUBMAP_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseEstimator/PoseEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POSEESTIMATOR_H_ 2 | #define SLAM_POSEESTIMATOR_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | // slam/ 12 | #include "CovarianceCalculator.h" 13 | #include "DataAssociator.h" 14 | #include "PoseOptimizer.h" 15 | // utility/ 16 | #include "MyUtility.h" 17 | #include "Pose2D.h" 18 | #include "Scan2D.h" 19 | 20 | 21 | namespace sample_slam { 22 | 23 | class PoseEstimator { 24 | public: 25 | PoseEstimator() 26 | : data_associator_ptr_(nullptr), 27 | pose_optimizer_ptr_(nullptr), 28 | min_match_cost_(__DBL_MAX__), 29 | delta_match_cost_threshold_(0.000001), 30 | num_iteration_threshold_(100), 31 | delta_time_(0.1) {} 32 | 33 | virtual ~PoseEstimator() {} 34 | 35 | void SetDataAssociatorPtr(DataAssociator* data_associator_ptr) { 36 | data_associator_ptr_ = data_associator_ptr; 37 | } 38 | 39 | void SetPoseOptimizerPtr(PoseOptimizer* pose_optimizer_ptr) { 40 | pose_optimizer_ptr_ = pose_optimizer_ptr; 41 | } 42 | 43 | PoseOptimizer* GetPoseOptimizerPtr() { 44 | return pose_optimizer_ptr_; 45 | } 46 | 47 | void SetReferenceScan(const Scan2D& reference_scan) { 48 | data_associator_ptr_->SetReferenceLaserPoints(reference_scan.laser_points); 49 | } 50 | 51 | void SetCurrentScan(const Scan2D& curr_scan) { 52 | data_associator_ptr_->SetCurrentLaserPoints(curr_scan.laser_points); 53 | } 54 | 55 | const Pose2D& GetEstimatedPose() const { 56 | return estimated_pose_; 57 | } 58 | 59 | double GetMinMatchCost() const { 60 | return min_match_cost_; 61 | } 62 | 63 | int GetNumUsedPoint() const { 64 | return num_used_point_; 65 | } 66 | 67 | double GetMatchedPointRatio() const { 68 | return matched_point_ratio_; 69 | } 70 | 71 | void OdometryFusion(const Scan2D& reference_scan, const Scan2D& curr_scan, const Pose2D& scan_match_pose, 72 | const Pose2D& last_pose, const Pose2D& odometry_pose_relative, const Pose2D& odometry_pose, 73 | Pose2D& fused_pose, Eigen::Matrix3f& fused_covariance) { 74 | Eigen::Vector3f scan_match_mean(scan_match_pose.x, scan_match_pose.y, scan_match_pose.yaw); 75 | 76 | Eigen::Matrix3f scan_match_covariance; 77 | CalcScanMatchCovariance(reference_scan, curr_scan, scan_match_pose, 78 | scan_match_covariance); 79 | 80 | Eigen::Vector3f odometry_mean(odometry_pose.x, odometry_pose.y, odometry_pose.yaw); 81 | 82 | Eigen::Matrix3f odometry_covariance; 83 | CalcOdometryCovariance(last_pose, odometry_pose_relative, 84 | odometry_covariance); 85 | /* 86 | CalcOdometryCovariance(scan_match_pose, odometry_pose_relative, 87 | odometry_covariance); 88 | */ 89 | 90 | Eigen::Vector3f fused_mean; 91 | FuseNormalDistributions(scan_match_mean, scan_match_covariance, 92 | odometry_mean, odometry_covariance, 93 | fused_mean, fused_covariance); 94 | 95 | fused_pose.x = fused_mean(0); 96 | fused_pose.y = fused_mean(1); 97 | fused_pose.yaw = fused_mean(2); 98 | fused_pose.CalcRotationMatrix(); 99 | 100 | std::cout << "[PoseEstimator::OdometryFusion()] \n" 101 | << "odometry_pose.x = " << odometry_pose.x << ", odometry_pose.y = " << odometry_pose.y 102 | << ", odometry_pose.yaw = " << odometry_pose.yaw << "\n" 103 | << "scan_match_pose.x = " << scan_match_pose.x << ", scan_match_pose.y = " << scan_match_pose.y 104 | << ", scan_match_pose.yaw = " << scan_match_pose.yaw << "\n" 105 | << "fused_pose.x = " << fused_pose.x << ", fused_pose.y = " << fused_pose.y 106 | << ", fused_pose.yaw = " << fused_pose.yaw << "\n"; 107 | 108 | return; 109 | } 110 | 111 | void CalcOdometryCovariance(const Pose2D& last_pose, const Pose2D& odometry_pose_relative, 112 | Eigen::Matrix3f& odometry_covariance) { 113 | Eigen::Matrix3f odometry_covariance_relative; 114 | covariance_calculator_.CalcOdometryCovarianceRelative(odometry_pose_relative, delta_time_, 115 | odometry_covariance_relative); 116 | 117 | CovarianceCalculator::RotateRelative2Absolute(last_pose, odometry_covariance_relative, 118 | odometry_covariance); 119 | 120 | return; 121 | } 122 | 123 | virtual void CalcScanMatchCovariance(const Scan2D& reference_scan, const Scan2D& curr_scan, const Pose2D& scan_match_pose, 124 | Eigen::Matrix3f& scan_match_covariance) = 0; 125 | 126 | virtual void EstimatePose(const Pose2D& init_pose) = 0; 127 | 128 | 129 | protected: 130 | void FuseNormalDistributions(const Eigen::Vector3f& mean1, const Eigen::Matrix3f& covariance1, 131 | const Eigen::Vector3f& mean2, const Eigen::Matrix3f& covariance2, 132 | Eigen::Vector3f& fused_mean, Eigen::Matrix3f& fused_covariance) { 133 | Eigen::Matrix3f inverse_covariance1; 134 | MyUtility::InverseMatrixSVD(covariance1, inverse_covariance1); 135 | 136 | Eigen::Matrix3f inverse_covariance2; 137 | MyUtility::InverseMatrixSVD(covariance2, inverse_covariance2); 138 | 139 | // S' = (S_1^{-1} + S_2^{-1})^{-1} 140 | Eigen::Matrix3f inverse_fused_covariance = inverse_covariance1 + inverse_covariance2; 141 | MyUtility::InverseMatrixSVD(inverse_fused_covariance, fused_covariance); 142 | 143 | // Angle correction 144 | Eigen::Vector3f mean1_correct = mean1; 145 | double delta_angle = mean2(2) - mean1(2); // [rad] 146 | if (delta_angle < -M_PI) { 147 | mean1_correct(2) -= 2.0*M_PI; 148 | } else if (M_PI < delta_angle) { 149 | mean1_correct(2) += 2.0*M_PI; 150 | } 151 | 152 | // x' = S'*(S_1^{-1}*x_1 + S_2^{-1}*x_2) 153 | fused_mean = fused_covariance*(inverse_covariance1*mean1_correct + inverse_covariance2*mean2); 154 | fused_mean(2) = MyUtility::CastRadian(fused_mean(2)); 155 | 156 | return; 157 | } 158 | 159 | DataAssociator* data_associator_ptr_; // Pointer to abstract class, which is customizable 160 | PoseOptimizer* pose_optimizer_ptr_; // Pointer to abstract class, which is customizable 161 | 162 | double min_match_cost_; 163 | double delta_match_cost_threshold_; 164 | int num_iteration_threshold_; 165 | double delta_time_; 166 | 167 | int num_used_point_; 168 | double matched_point_ratio_; 169 | Pose2D estimated_pose_; 170 | CovarianceCalculator covariance_calculator_; 171 | }; 172 | 173 | } // namespace sample_slam 174 | 175 | 176 | #endif // SLAM_POSEESTIMATOR_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseEstimator/PoseEstimator_ICP.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseEstimator_ICP.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | void PoseEstimator_ICP::EstimatePose(const Pose2D& init_pose) { 7 | min_match_cost_ = __DBL_MAX__; 8 | 9 | Pose2D pre_optimize_pose = init_pose; 10 | double delta_match_cost = __DBL_MAX__; 11 | double prev_match_cost = __DBL_MAX__; 12 | int i = 0; 13 | 14 | while (delta_match_cost_threshold_ < delta_match_cost) { 15 | data_associator_ptr_->AssociateCorrespondingLaserPoints(pre_optimize_pose); 16 | 17 | pose_optimizer_ptr_->SetAssociatedLaserPoints(data_associator_ptr_->GetAssociatedReferenceLaserPoints(), 18 | data_associator_ptr_->GetAssociatedCurrentLaserPoints()); 19 | 20 | Pose2D post_optimize_pose; 21 | double curr_match_cost = pose_optimizer_ptr_->OptimizePose(pre_optimize_pose, post_optimize_pose); 22 | 23 | if (curr_match_cost < min_match_cost_) { 24 | min_match_cost_ = curr_match_cost; 25 | estimated_pose_ = post_optimize_pose; 26 | } 27 | 28 | pre_optimize_pose = post_optimize_pose; 29 | delta_match_cost = std::fabs(prev_match_cost - curr_match_cost); 30 | prev_match_cost = curr_match_cost; 31 | ++i; 32 | 33 | if (num_iteration_threshold_ < i) { 34 | break; 35 | } 36 | } 37 | 38 | num_used_point_ = data_associator_ptr_->GetAssociatedCurrentLaserPoints().size(); 39 | matched_point_ratio_ = pose_optimizer_ptr_->GetMatchedPointRatio(); 40 | 41 | return; 42 | } 43 | 44 | 45 | void PoseEstimator_ICP::CalcScanMatchCovariance(const Scan2D& reference_scan, const Scan2D& curr_scan, const Pose2D& scan_match_pose, 46 | Eigen::Matrix3f& scan_match_covariance) { 47 | data_associator_ptr_->SetReferenceLaserPoints(reference_scan.laser_points); 48 | data_associator_ptr_->SetCurrentLaserPoints(curr_scan.laser_points); 49 | data_associator_ptr_->AssociateCorrespondingLaserPoints(scan_match_pose); 50 | 51 | covariance_calculator_.CalcScanMatchCovarianceICP(scan_match_pose, 52 | data_associator_ptr_->GetAssociatedReferenceLaserPoints(), 53 | data_associator_ptr_->GetAssociatedCurrentLaserPoints(), 54 | pose_optimizer_ptr_->GetCostFunctionPtr(), 55 | scan_match_covariance); 56 | 57 | return; 58 | } 59 | 60 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseEstimator/PoseEstimator_ICP.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POSEESTIMATOR_ICP_H_ 2 | #define SLAM_POSEESTIMATOR_ICP_H_ 3 | 4 | 5 | // slam/ 6 | #include "PoseEstimator.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | class PoseEstimator_ICP : public PoseEstimator { 12 | public: 13 | PoseEstimator_ICP() {} 14 | 15 | ~PoseEstimator_ICP() {} 16 | 17 | void EstimatePose(const Pose2D& init_pose) override; 18 | 19 | void CalcScanMatchCovariance(const Scan2D& reference_scan, const Scan2D& curr_scan, const Pose2D& scan_match_pose, 20 | Eigen::Matrix3f& scan_match_covariance) override; 21 | }; 22 | 23 | } // namespace sample_slam 24 | 25 | 26 | #endif // SLAM_POSEESTIMATOR_ICP_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseEstimator/PoseEstimator_NDT.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_cpp/src/slam/PoseEstimator/PoseEstimator_NDT.cpp -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseEstimator/PoseEstimator_NDT.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_cpp/src/slam/PoseEstimator/PoseEstimator_NDT.h -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseGraph/PoseGraph.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseGraph.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | PoseNode* PoseGraph::AddNode(const Pose2D& pose) { 9 | PoseNode* node_ptr = AllocNode(); 10 | 11 | node_ptr->id = static_cast(node_ptrs_.size()); 12 | node_ptr->pose = pose; 13 | node_ptrs_.emplace_back(node_ptr); 14 | 15 | return node_ptr; 16 | } 17 | 18 | 19 | PoseArc* PoseGraph::AddArc(int src_node_id, int dst_node_id, 20 | const Pose2D& pose_relative, const Eigen::Matrix3f& covariance_relative) { 21 | // Omega = Sigma^{-1} 22 | Eigen::Matrix3f information_relative; 23 | MyUtility::InverseMatrixSVD(covariance_relative, information_relative); 24 | 25 | PoseArc* arc_ptr = AllocArc(); 26 | arc_ptr->src_node_ptr = node_ptrs_[src_node_id]; 27 | arc_ptr->dst_node_ptr = node_ptrs_[dst_node_id]; 28 | arc_ptr->pose_relative = pose_relative; 29 | arc_ptr->information_relative = information_relative; 30 | 31 | arc_ptrs_.emplace_back(arc_ptr); 32 | 33 | return arc_ptr; 34 | } 35 | 36 | 37 | PoseNode* PoseGraph::AllocNode() { 38 | if (max_pool_size_ <= nodes_pool_.size()) { 39 | std::cerr << "[PoseGraph::AllocNode()] Error: nodes_pool_ exceeds its capacity \n"; 40 | return nullptr; 41 | } 42 | 43 | PoseNode node; 44 | nodes_pool_.emplace_back(node); 45 | PoseNode* alloced_node_ptr = &(nodes_pool_.back()); 46 | 47 | return alloced_node_ptr; 48 | } 49 | 50 | 51 | PoseArc* PoseGraph::AllocArc() { 52 | if (max_pool_size_ <= arcs_pool_.size()) { 53 | std::cerr << "[PoseGraph::AllocArc()] Error: arcs_pool_ exceeds its capacity \n"; 54 | return nullptr; 55 | } 56 | 57 | PoseArc arc; 58 | arcs_pool_.emplace_back(arc); 59 | PoseArc* alloced_arc_ptr = &(arcs_pool_.back()); 60 | 61 | return alloced_arc_ptr; 62 | } 63 | 64 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseGraph/PoseGraph.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POSEGRAPH_H_ 2 | #define SLAM_POSEGRAPH_H_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | 9 | // utility/ 10 | #include "Pose2D.h" 11 | 12 | 13 | namespace sample_slam { 14 | 15 | struct PoseNode { 16 | int id; 17 | Pose2D pose; 18 | }; 19 | 20 | 21 | struct PoseArc { 22 | PoseNode* src_node_ptr; 23 | PoseNode* dst_node_ptr; 24 | Pose2D pose_relative; 25 | Eigen::Matrix3f information_relative; // Inverse covariance matrix 26 | }; 27 | 28 | 29 | class PoseGraph { 30 | public: 31 | PoseGraph() : max_pool_size_(100000) { 32 | nodes_pool_.reserve(max_pool_size_); 33 | arcs_pool_.reserve(max_pool_size_); 34 | } 35 | 36 | ~PoseGraph() {} 37 | 38 | int GetNodePtrsSize() const { 39 | return node_ptrs_.size(); 40 | } 41 | 42 | PoseNode* GetNodePtrsBack() const { 43 | return node_ptrs_.back(); 44 | } 45 | 46 | std::vector& GetNodePtrs() { 47 | return node_ptrs_; 48 | } 49 | 50 | std::vector& GetArcPtrs() { 51 | return arc_ptrs_; 52 | } 53 | 54 | PoseNode* AddNode(const Pose2D& pose); 55 | 56 | PoseArc* AddArc(int src_node_id, int dst_node_id, 57 | const Pose2D& pose_relative, const Eigen::Matrix3f& covariance_relative); 58 | 59 | 60 | private: 61 | PoseNode* AllocNode(); 62 | 63 | PoseArc* AllocArc(); 64 | 65 | int max_pool_size_; 66 | std::vector nodes_pool_; 67 | std::vector arcs_pool_; 68 | std::vector node_ptrs_; 69 | std::vector arc_ptrs_; 70 | }; 71 | 72 | } // namespace sample_slam 73 | 74 | 75 | #endif // SLAM_POSEGRAPH_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseOptimizer/PoseOptimizer.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POSEOPTIMIZER_H_ 2 | #define SLAM_POSEOPTIMIZER_H_ 3 | 4 | 5 | #include 6 | #include 7 | 8 | // slam/ 9 | #include "CostFunction.h" 10 | // utility/ 11 | #include "LaserPoint2D.h" 12 | #include "Pose2D.h" 13 | 14 | 15 | namespace sample_slam { 16 | 17 | class PoseOptimizer { 18 | public: 19 | PoseOptimizer() 20 | : cost_function_ptr_(nullptr), 21 | delta_match_cost_threshold_(0.000001), 22 | delta_distance_(0.00001), 23 | delta_angle_(0.0000001) {} 24 | 25 | virtual ~PoseOptimizer() {} 26 | 27 | void SetCostFunctionPtr(CostFunction* cost_function_ptr) { 28 | cost_function_ptr_ = cost_function_ptr; 29 | } 30 | 31 | CostFunction* GetCostFunctionPtr() { 32 | return cost_function_ptr_; 33 | } 34 | 35 | void SetAssociatedLaserPoints(const std::vector& associated_reference_laser_points, 36 | const std::vector& associated_current_laser_points) { 37 | cost_function_ptr_->SetAssociatedLaserPoints(associated_reference_laser_points, 38 | associated_current_laser_points); 39 | } 40 | 41 | double GetMatchedPointRatio() const { 42 | return cost_function_ptr_->GetMatchedPointRatio(); 43 | } 44 | 45 | virtual double OptimizePose(const Pose2D& pre_optimize_pose, Pose2D& post_optimize_pose) = 0; 46 | 47 | 48 | protected: 49 | CostFunction* cost_function_ptr_; // Pointer to abstract class, which is customizable 50 | 51 | double delta_match_cost_threshold_; 52 | double delta_distance_; // [m] 53 | double delta_angle_; // [rad] 54 | }; 55 | 56 | } // namespace sample_slam 57 | 58 | #endif // SLAM_POSEOPTIMIZER_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseOptimizer/PoseOptimizer_GradientDescent.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseOptimizer_GradientDescent.h" 2 | 3 | #include 4 | #include 5 | 6 | 7 | namespace sample_slam { 8 | 9 | double PoseOptimizer_GradientDescent::OptimizePose(const Pose2D& pre_optimize_pose, Pose2D& post_optimize_pose) { 10 | double x = pre_optimize_pose.x; 11 | double y = pre_optimize_pose.y; 12 | double yaw = pre_optimize_pose.yaw; 13 | 14 | double min_match_cost = __DBL_MAX__; 15 | double min_x = x; 16 | double min_y = y; 17 | double min_yaw = yaw; 18 | 19 | double delta_match_cost = __DBL_MAX__; 20 | double prev_match_cost = __DBL_MAX__; 21 | int i = 0; 22 | 23 | while (delta_match_cost_threshold_ < delta_match_cost) { 24 | double partial_derivative_x = cost_function_ptr_->CalcPartialDerivativeX(x, delta_distance_, y, yaw); 25 | double partial_derivative_y = cost_function_ptr_->CalcPartialDerivativeY(x, y, delta_distance_, yaw); 26 | double partial_derivative_yaw = cost_function_ptr_->CalcPartialDerivativeYaw(x, y, yaw, delta_angle_); 27 | 28 | x += -step_rate_ * partial_derivative_x; 29 | y += -step_rate_ * partial_derivative_y; 30 | yaw += -step_rate_ * partial_derivative_yaw; 31 | 32 | double curr_match_cost = cost_function_ptr_->CalcMatchCost(x, y, yaw); 33 | 34 | if (curr_match_cost < min_match_cost) { 35 | min_match_cost = curr_match_cost; 36 | min_x = x; 37 | min_y = y; 38 | min_yaw = yaw; 39 | } 40 | 41 | delta_match_cost = std::fabs(prev_match_cost - curr_match_cost); 42 | prev_match_cost = curr_match_cost; 43 | ++i; 44 | } 45 | 46 | post_optimize_pose.x = min_x; 47 | post_optimize_pose.y = min_y; 48 | post_optimize_pose.yaw = min_yaw; 49 | post_optimize_pose.CalcRotationMatrix(); 50 | 51 | return min_match_cost; 52 | } 53 | 54 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseOptimizer/PoseOptimizer_GradientDescent.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POSEOPTIMIZER_GRADIENTDESCENT_H_ 2 | #define SLAM_POSEOPTIMIZER_GRADIENTDESCENT_H_ 3 | 4 | 5 | // slam/ 6 | #include "PoseOptimizer.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | class PoseOptimizer_GradientDescent : public PoseOptimizer { 12 | public: 13 | PoseOptimizer_GradientDescent() 14 | : step_rate_(0.00001) {} 15 | 16 | ~PoseOptimizer_GradientDescent() {} 17 | 18 | double OptimizePose(const Pose2D& pre_optimize_pose, Pose2D& post_optimize_pose) override; 19 | 20 | 21 | private: 22 | double step_rate_; 23 | }; 24 | 25 | } // namespace sample_slam 26 | 27 | 28 | #endif // SLAM_POSEOPTIMIZER_GRADIENTDESCENT_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseOptimizer/PoseOptimizer_LineSearch.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseOptimizer_LineSearch.h" 2 | 3 | #include 4 | 5 | // utility/ 6 | #include "MyUtility.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | double PoseOptimizer_LineSearch::OptimizePose(const Pose2D& pre_optimize_pose, Pose2D& post_optimize_pose) { 12 | double x = pre_optimize_pose.x; 13 | double y = pre_optimize_pose.y; 14 | double yaw = pre_optimize_pose.yaw; 15 | 16 | double min_match_cost = __DBL_MAX__; 17 | double min_x = x; 18 | double min_y = y; 19 | double min_yaw = yaw; 20 | 21 | double delta_match_cost = __DBL_MAX__; 22 | double prev_match_cost = __DBL_MAX__; 23 | int i = 0; 24 | 25 | while (delta_match_cost_threshold_ < delta_match_cost) { 26 | double partial_derivative_x = cost_function_ptr_->CalcPartialDerivativeX(x, delta_distance_, y, yaw); 27 | double partial_derivative_y = cost_function_ptr_->CalcPartialDerivativeY(x, y, delta_distance_, yaw); 28 | double partial_derivative_yaw = cost_function_ptr_->CalcPartialDerivativeYaw(x, y, yaw, delta_angle_); 29 | 30 | x += partial_derivative_x; 31 | y += partial_derivative_y; 32 | yaw += partial_derivative_yaw; 33 | 34 | Pose2D search_pose(x, y, yaw); 35 | Pose2D search_direction(partial_derivative_x, partial_derivative_y, partial_derivative_yaw); 36 | LineSearchBrent(search_pose, search_direction); 37 | 38 | x = search_pose.x; 39 | y = search_pose.y; 40 | yaw = search_pose.yaw; 41 | 42 | double curr_match_cost = cost_function_ptr_->CalcMatchCost(x, y, yaw); 43 | 44 | if (curr_match_cost < min_match_cost) { 45 | min_match_cost = curr_match_cost; 46 | min_x = x; 47 | min_y = y; 48 | min_yaw = yaw; 49 | } 50 | 51 | delta_match_cost = std::fabs(prev_match_cost - curr_match_cost); 52 | prev_match_cost = curr_match_cost; 53 | ++i; 54 | } 55 | 56 | post_optimize_pose.x = min_x; 57 | post_optimize_pose.y = min_y; 58 | post_optimize_pose.yaw = min_yaw; 59 | post_optimize_pose.CalcRotationMatrix(); 60 | 61 | return min_match_cost; 62 | } 63 | 64 | 65 | void PoseOptimizer_LineSearch::LineSearchBrent(Pose2D& search_pose, Pose2D& search_direction) const { 66 | boost::uintmax_t max_iteration = max_iteration_; 67 | 68 | std::pair result = boost::math::tools::brent_find_minima( 69 | [this, &search_pose, &search_direction](double step_rate_tmp) { 70 | double x = search_pose.x + step_rate_tmp * search_direction.x; 71 | double y = search_pose.y + step_rate_tmp * search_direction.y; 72 | double yaw = MyUtility::AddRadian(search_pose.yaw, step_rate_tmp * search_direction.yaw); 73 | double match_cost = cost_function_ptr_->CalcMatchCost(x, y, yaw); 74 | return match_cost; 75 | }, 76 | -search_range_, search_range_, std::numeric_limits::digits, max_iteration 77 | ); 78 | 79 | double step_rate = result.first; 80 | // double minima = result.second; 81 | 82 | search_pose.x += step_rate * search_direction.x; 83 | search_pose.y += step_rate * search_direction.y; 84 | search_pose.yaw = MyUtility::AddRadian(search_pose.yaw, step_rate * search_direction.yaw); 85 | 86 | return; 87 | } 88 | 89 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/PoseOptimizer/PoseOptimizer_LineSearch.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_POSEOPTIMIZER_LINESEARCH_H_ 2 | #define SLAM_POSEOPTIMIZER_LINESEARCH_H_ 3 | 4 | 5 | // slam/ 6 | #include "PoseOptimizer.h" 7 | 8 | 9 | namespace sample_slam { 10 | 11 | class PoseOptimizer_LineSearch : public PoseOptimizer { 12 | public: 13 | PoseOptimizer_LineSearch() : search_range_(2.0), max_iteration_(100) {} 14 | 15 | ~PoseOptimizer_LineSearch() {} 16 | 17 | double OptimizePose(const Pose2D& pre_optimize_pose, Pose2D& post_optimize_pose) override; 18 | 19 | 20 | private: 21 | void LineSearchBrent(Pose2D& search_pose, Pose2D& search_direction) const; 22 | 23 | double search_range_; 24 | int max_iteration_; 25 | }; 26 | 27 | } // namespace sample_slam 28 | 29 | 30 | #endif // SLAM_POSEOPTIMIZER_LINESEARCH_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ReferenceScanMaker/ReferenceScanMaker.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_REFERENCESCANMAKER_H_ 2 | #define SLAM_REFERENCESCANMAKER_H_ 3 | 4 | 5 | // slam/ 6 | #include "PointCloudMap.h" 7 | // utility/ 8 | #include "Scan2D.h" 9 | 10 | 11 | namespace sample_slam { 12 | 13 | class ReferenceScanMaker { 14 | public: 15 | ReferenceScanMaker() 16 | : point_cloud_map_ptr_(nullptr) {} 17 | 18 | virtual ~ReferenceScanMaker() {} 19 | 20 | void SetPointCloudMapPtr(PointCloudMap* point_cloud_map_ptr) { 21 | point_cloud_map_ptr_ = point_cloud_map_ptr; 22 | } 23 | 24 | const Scan2D& GetReferenceScan() const { 25 | return reference_scan_; 26 | } 27 | 28 | virtual void MakeReferenceScan() = 0; 29 | 30 | 31 | protected: 32 | PointCloudMap* point_cloud_map_ptr_; // Pointer to abstract class, which is customizable 33 | 34 | Scan2D reference_scan_; 35 | }; 36 | 37 | } // namespace sample_slam 38 | 39 | 40 | #endif // SLAM_REFERENCESCANMAKER_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ReferenceScanMaker/ReferenceScanMaker_LastScan.cpp: -------------------------------------------------------------------------------- 1 | #include "ReferenceScanMaker_LastScan.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | void ReferenceScanMaker_LastScan::MakeReferenceScan() { 7 | const Pose2D& last_pose = point_cloud_map_ptr_->GetLastPose(); 8 | const Scan2D& last_scan = point_cloud_map_ptr_->GetLastScan(); 9 | 10 | reference_scan_.laser_points.clear(); 11 | for (std::size_t i = 0; i < last_scan.laser_points.size(); ++i) { 12 | const LaserPoint2D& laser_point = last_scan.laser_points[i]; 13 | 14 | LaserPoint2D reference_laser_point; 15 | reference_laser_point.id = laser_point.id; 16 | reference_laser_point.point_type = laser_point.point_type; 17 | 18 | last_pose.LocalCoord2GlobalCoord(laser_point, reference_laser_point); // x' = R*x + t 19 | last_pose.RotateNormalVector(laser_point, reference_laser_point); // n' = R*n 20 | 21 | reference_scan_.laser_points.emplace_back(reference_laser_point); 22 | } 23 | 24 | return; 25 | } 26 | 27 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ReferenceScanMaker/ReferenceScanMaker_LastScan.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_REFERENCESCANMAKER_LASTSCAN_H_ 2 | #define SLAM_REFERENCESCANMAKER_LASTSCAN_H_ 3 | 4 | 5 | #include "ReferenceScanMaker.h" 6 | 7 | 8 | namespace sample_slam { 9 | 10 | // Concrete class of `ReferenceScanMaker` that uses the last scan as a reference scan 11 | class ReferenceScanMaker_LastScan : public ReferenceScanMaker { 12 | public: 13 | ReferenceScanMaker_LastScan() {} 14 | 15 | ~ReferenceScanMaker_LastScan() {} 16 | 17 | void MakeReferenceScan() override; 18 | }; 19 | 20 | } // namespace sample_slam 21 | 22 | 23 | #endif // SLAM_REFERENCESCANMAKER_LASTSCAN_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ReferenceScanMaker/ReferenceScanMaker_LocalMap.cpp: -------------------------------------------------------------------------------- 1 | #include "ReferenceScanMaker_LocalMap.h" 2 | 3 | 4 | namespace sample_slam { 5 | 6 | void ReferenceScanMaker_LocalMap::MakeReferenceScan() { 7 | reference_scan_.laser_points.clear(); 8 | 9 | const std::vector& local_map = point_cloud_map_ptr_->GetLocalMap(); 10 | for (std::size_t i = 0; i < local_map.size(); ++i) { 11 | reference_scan_.laser_points.emplace_back(local_map[i]); 12 | } 13 | 14 | return; 15 | } 16 | 17 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ReferenceScanMaker/ReferenceScanMaker_LocalMap.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_REFERENCESCANMAKER_LOCALMAP_H_ 2 | #define SLAM_REFERENCESCANMAKER_LOCALMAP_H_ 3 | 4 | 5 | #include "ReferenceScanMaker.h" 6 | 7 | 8 | namespace sample_slam { 9 | 10 | // Concrete class of `ReferenceScanMaker` that uses the local map as a reference scan 11 | class ReferenceScanMaker_LocalMap : public ReferenceScanMaker { 12 | public: 13 | ReferenceScanMaker_LocalMap() {} 14 | 15 | ~ReferenceScanMaker_LocalMap() {} 16 | 17 | void MakeReferenceScan() override; 18 | }; 19 | 20 | } // namespace sample_slam 21 | 22 | 23 | #endif // SLAM_REFERENCESCANMAKER_LOCALMAP_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ScanMatcher/ScanMatcher.cpp: -------------------------------------------------------------------------------- 1 | #include "ScanMatcher.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | // slam/ 9 | #include "ScanPreprocessor.h" 10 | // utility/ 11 | #include "LaserPoint2D.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | bool ScanMatcher::MatchScan(const Scan2D& scan, int count) { 17 | Scan2D curr_scan(scan); 18 | 19 | if (is_scan_preprocess_) { 20 | ScanPreprocessor scan_preprocessor; 21 | scan_preprocessor.ResampleLaserPoints(curr_scan.laser_points); 22 | scan_preprocessor.CalcNormalVectors(curr_scan.laser_points); 23 | } 24 | 25 | if (count == 0) { 26 | GrowMap(curr_scan, Pose2D(0.0, 0.0, 0.0), count); 27 | prev_scan_ = curr_scan; 28 | 29 | return true; 30 | } 31 | 32 | Pose2D curr_scan_pose(curr_scan.pose); 33 | Pose2D prev_scan_pose(prev_scan_.pose); 34 | Pose2D odometry_pose_relative = curr_scan_pose - prev_scan_pose; // Inverse compounding operator 35 | 36 | const Pose2D& last_pose = point_cloud_map_ptr_->GetLastPose(); 37 | Pose2D odometry_pose = last_pose + odometry_pose_relative; // Compounding operator 38 | 39 | reference_scan_maker_ptr_->MakeReferenceScan(); 40 | const Scan2D& reference_scan = reference_scan_maker_ptr_->GetReferenceScan(); 41 | 42 | pose_estimator_ptr_->SetReferenceScan(reference_scan); 43 | pose_estimator_ptr_->SetCurrentScan(curr_scan); 44 | 45 | auto start_EstimatePose = std::chrono::high_resolution_clock::now(); 46 | pose_estimator_ptr_->EstimatePose(odometry_pose); 47 | auto end_EstimatePose = std::chrono::high_resolution_clock::now(); 48 | auto duration_EstimatePose = std::chrono::duration_cast(end_EstimatePose - start_EstimatePose); 49 | std::cout << "duration_EstimatePose.count() = " << duration_EstimatePose.count() << " [ms] \n"; 50 | 51 | Pose2D scan_match_pose(pose_estimator_ptr_->GetEstimatedPose()); 52 | double min_match_cost = pose_estimator_ptr_->GetMinMatchCost(); 53 | int num_used_point = pose_estimator_ptr_->GetNumUsedPoint(); 54 | 55 | bool is_scan_match_success; 56 | Pose2D estimated_pose; 57 | if (min_match_cost < match_cost_threshold_ && num_used_point_threshold_ < num_used_point) { 58 | is_scan_match_success = true; 59 | estimated_pose = scan_match_pose; // Adopts the scan-matched pose 60 | } else { 61 | is_scan_match_success = false; 62 | estimated_pose = odometry_pose; // Adopts the odometry-only pose 63 | } 64 | 65 | std::cout << "[ScanMatcher::MatchScan()] is_scan_match_success = " << is_scan_match_success << " : " 66 | << "min_match_cost = " << min_match_cost << ", num_used_point = " << num_used_point << "\n"; 67 | 68 | if (is_odometry_fusion_) { 69 | if (is_scan_match_success) { 70 | Pose2D fused_pose; 71 | Eigen::Matrix3f fused_covariance; 72 | pose_estimator_ptr_->OdometryFusion(reference_scan, curr_scan, scan_match_pose, 73 | last_pose, odometry_pose_relative, odometry_pose, 74 | fused_pose, fused_covariance); 75 | 76 | estimated_pose = fused_pose; // Adopts the scan-odometry-fused pose 77 | move_covariance_ = fused_covariance; 78 | } else { 79 | Eigen::Matrix3f odometry_covariance; 80 | pose_estimator_ptr_->CalcOdometryCovariance(last_pose, odometry_pose_relative, 81 | odometry_covariance); 82 | 83 | estimated_pose = odometry_pose; // Adopts the odometry-only pose 84 | move_covariance_ = odometry_covariance; 85 | } 86 | } 87 | 88 | GrowMap(curr_scan, estimated_pose, count); 89 | prev_scan_ = curr_scan; 90 | 91 | return is_scan_match_success; 92 | } 93 | 94 | 95 | void ScanMatcher::GrowMap(const Scan2D& curr_scan, const Pose2D& estimated_pose, int count) { 96 | std::vector global_laser_points; 97 | for (std::size_t i = 0; i < curr_scan.laser_points.size(); ++i) { 98 | const LaserPoint2D& laser_point = curr_scan.laser_points[i]; 99 | if (laser_point.point_type == ISOLATE) { 100 | continue; 101 | } 102 | 103 | LaserPoint2D map_laser_point; 104 | map_laser_point.id = count; 105 | map_laser_point.point_type = laser_point.point_type; 106 | 107 | estimated_pose.LocalCoord2GlobalCoord(laser_point, map_laser_point); // x' = R*x + t 108 | estimated_pose.RotateNormalVector(laser_point, map_laser_point); // n' = R*n 109 | 110 | global_laser_points.emplace_back(map_laser_point); 111 | } 112 | 113 | point_cloud_map_ptr_->AddPose(estimated_pose); 114 | point_cloud_map_ptr_->AddLaserPoints(global_laser_points); 115 | point_cloud_map_ptr_->SetLastPose(estimated_pose); 116 | point_cloud_map_ptr_->SetLastScan(curr_scan); 117 | point_cloud_map_ptr_->MakeLocalMap(); 118 | 119 | return; 120 | } 121 | 122 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ScanMatcher/ScanMatcher.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_SCANMATCHER_H_ 2 | #define SLAM_SCANMATCHER_H_ 3 | 4 | 5 | // slam/ 6 | #include "PointCloudMap.h" 7 | #include "PoseEstimator.h" 8 | #include "ReferenceScanMaker.h" 9 | // utility/ 10 | #include "Pose2D.h" 11 | #include "Scan2D.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | class ScanMatcher { 17 | public: 18 | ScanMatcher() 19 | : point_cloud_map_ptr_(nullptr), 20 | reference_scan_maker_ptr_(nullptr), 21 | pose_estimator_ptr_(nullptr), 22 | is_odometry_fusion_(false), 23 | match_cost_threshold_(1.0), num_used_point_threshold_(50) {} 24 | 25 | ~ScanMatcher() {} 26 | 27 | void SetPointCloudMapPtr(PointCloudMap* point_cloud_map_ptr) { 28 | point_cloud_map_ptr_ = point_cloud_map_ptr; 29 | } 30 | 31 | void SetReferenceScanMakerPtr(ReferenceScanMaker* reference_scan_maker_ptr) { 32 | reference_scan_maker_ptr_ = reference_scan_maker_ptr; 33 | } 34 | 35 | void SetPoseEstimatorPtr(PoseEstimator* pose_estimator_ptr) { 36 | pose_estimator_ptr_ = pose_estimator_ptr; 37 | } 38 | 39 | void SetIsScanPreprocess(bool is_scan_preprocess) { 40 | is_scan_preprocess_ = is_scan_preprocess; 41 | } 42 | 43 | void SetIsOdometryFusion(bool is_odometry_fusion) { 44 | is_odometry_fusion_ = is_odometry_fusion; 45 | } 46 | 47 | ReferenceScanMaker* GetReferenceScanMakerPtr() { 48 | return reference_scan_maker_ptr_; 49 | } 50 | 51 | PoseEstimator* GetPoseEstimatorPtr() { 52 | return pose_estimator_ptr_; 53 | } 54 | 55 | const Eigen::Matrix3f& GetMoveCovariance() { 56 | return move_covariance_; 57 | } 58 | 59 | bool MatchScan(const Scan2D& scan, int count); 60 | 61 | void GrowMap(const Scan2D& curr_scan, const Pose2D& estimated_pose, int count); 62 | 63 | 64 | private: 65 | PointCloudMap* point_cloud_map_ptr_; // Pointer to abstract class, which is customizable 66 | ReferenceScanMaker* reference_scan_maker_ptr_; // Pointer to abstract class, which is customizable 67 | PoseEstimator* pose_estimator_ptr_; // Pointer to abstract class, which is customizable 68 | bool is_scan_preprocess_; 69 | bool is_odometry_fusion_; 70 | 71 | double match_cost_threshold_; 72 | int num_used_point_threshold_; 73 | 74 | Scan2D prev_scan_; 75 | Eigen::Matrix3f move_covariance_; // Covariance of the relative movement in one frame 76 | }; 77 | 78 | } // namespace sample_slam 79 | 80 | #endif // SLAM_SCANMATCHER_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ScanPreprocessor/ScanPreprocessor.cpp: -------------------------------------------------------------------------------- 1 | #include "ScanPreprocessor.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | void ScanPreprocessor::ResampleLaserPoints(std::vector& laser_points) { 9 | if (laser_points.size() == 0) { 10 | return; 11 | } 12 | 13 | std::vector resampled_laser_points = {laser_points[0]}; 14 | LaserPoint2D prev_laser_point(laser_points[0]); 15 | 16 | for (std::size_t i = 1; i < laser_points.size(); ++i) { 17 | LaserPoint2D curr_laser_point(laser_points[i]); 18 | 19 | LaserPoint2D resampled_laser_point; 20 | bool is_resampled; 21 | bool is_interpolated; 22 | ResamplePoint(prev_laser_point, curr_laser_point, 23 | resampled_laser_point, is_resampled, is_interpolated); 24 | 25 | if (is_resampled) { 26 | resampled_laser_points.emplace_back(resampled_laser_point); 27 | cumulative_distance_ = 0.0; 28 | if (is_interpolated) { 29 | // Retry with the current laser point again 30 | // because a resampled point was interpolated before the current laser point 31 | --i; 32 | } 33 | prev_laser_point = resampled_laser_point; 34 | } else { 35 | prev_laser_point = curr_laser_point; 36 | } 37 | } 38 | 39 | std::cout << "[ScanPreprocessor::ResampleLaserPoints()] " 40 | << "laser_points.size() = " << laser_points.size() << ", " 41 | << "resampled_laser_points.size() = " << resampled_laser_points.size() << "\n"; 42 | 43 | laser_points = resampled_laser_points; 44 | 45 | return; 46 | } 47 | 48 | 49 | void ScanPreprocessor::ResamplePoint(const LaserPoint2D& prev_laser_point, const LaserPoint2D& curr_laser_point, 50 | LaserPoint2D& resampled_laser_point, bool& is_resampled, bool& is_interpolated) { 51 | double dx = curr_laser_point.x - prev_laser_point.x; 52 | double dy = curr_laser_point.y - prev_laser_point.y; 53 | double distance = hypot(dx, dy); 54 | double total_distance = cumulative_distance_ + distance; 55 | 56 | if (total_distance < min_interpolation_threshold_) { 57 | cumulative_distance_ = total_distance; 58 | 59 | resampled_laser_point = LaserPoint2D(); 60 | is_resampled = false; 61 | is_interpolated = false; 62 | return; 63 | } 64 | 65 | if (min_interpolation_threshold_ <= total_distance && total_distance < max_interpolation_threshold_) { 66 | double interpolation_ratio = (min_interpolation_threshold_ - cumulative_distance_) / distance; 67 | double resampled_x = prev_laser_point.x + interpolation_ratio * dx; 68 | double resampled_y = prev_laser_point.y + interpolation_ratio * dy; 69 | 70 | resampled_laser_point = LaserPoint2D(curr_laser_point.id, resampled_x, resampled_y); 71 | is_resampled = true; 72 | is_interpolated = true; 73 | return; 74 | } 75 | 76 | if (max_interpolation_threshold_ <= total_distance) { 77 | resampled_laser_point = curr_laser_point; 78 | is_resampled = true; 79 | is_interpolated = false; 80 | return; 81 | } 82 | } 83 | 84 | 85 | void ScanPreprocessor::CalcNormalVectors(std::vector& laser_points) const { 86 | for (std::size_t i = 0; i < laser_points.size(); ++i) { 87 | double left_normal_x, left_normal_y; 88 | bool is_left_normal = CalcLeftNormalVector(i, laser_points, left_normal_x, left_normal_y); 89 | 90 | double right_normal_x, right_normal_y; 91 | bool is_right_normal = CalcRightNormalVector(i, laser_points, right_normal_x, right_normal_y); 92 | 93 | PointType point_type; 94 | double normal_x, normal_y; 95 | 96 | if (is_left_normal && is_right_normal) { 97 | double inner_product = left_normal_x*right_normal_x + left_normal_y*right_normal_y; 98 | if (cos(corner_angle_threshold_) <= fabs(inner_product)) { 99 | point_type = PointType::LINE; 100 | } else { 101 | point_type = PointType::CORNER; 102 | } 103 | normal_x = left_normal_x + right_normal_x; 104 | normal_y = left_normal_y + right_normal_y; 105 | normal_x /= hypot(normal_x, normal_y); 106 | normal_y /= hypot(normal_x, normal_y); 107 | } else if (is_left_normal && !is_right_normal) { 108 | point_type = PointType::LINE; 109 | normal_x = left_normal_x; 110 | normal_y = left_normal_y; 111 | } else if (!is_left_normal && is_right_normal) { 112 | point_type = PointType::LINE; 113 | normal_x = right_normal_x; 114 | normal_y = right_normal_y; 115 | } else if (!is_left_normal && !is_right_normal) { 116 | point_type = PointType::ISOLATE; 117 | normal_x = 0.0; 118 | normal_y = 0.0; 119 | } 120 | 121 | laser_points[i].point_type = point_type; 122 | laser_points[i].normal_x = normal_x; 123 | laser_points[i].normal_y = normal_y; 124 | } 125 | 126 | return; 127 | } 128 | 129 | 130 | bool ScanPreprocessor::CalcLeftNormalVector(int curr_idx, const std::vector& laser_points, 131 | double& left_normal_x, double& left_normal_y) const { 132 | for (std::size_t i = curr_idx - 1; 0 <= i && i < laser_points.size(); --i) { 133 | double dx = laser_points[i].x - laser_points[curr_idx].x; 134 | double dy = laser_points[i].y - laser_points[curr_idx].y; 135 | double distance = hypot(dx, dy); 136 | if (distance < min_adjacent_threshold_) { 137 | continue; 138 | } 139 | 140 | if (min_adjacent_threshold_ <= distance && distance < max_adjacent_threshold_) { 141 | left_normal_x = dy / distance; 142 | left_normal_y = -dx / distance; 143 | return true; 144 | } 145 | 146 | if (max_adjacent_threshold_ <= distance ) { 147 | left_normal_x = 0.0; 148 | left_normal_y = 0.0; 149 | return false; 150 | } 151 | } 152 | 153 | left_normal_x = 0.0; 154 | left_normal_y = 0.0; 155 | return false; 156 | } 157 | 158 | 159 | bool ScanPreprocessor::CalcRightNormalVector(int curr_idx, const std::vector& laser_points, 160 | double& right_normal_x, double& right_normal_y) const { 161 | for (std::size_t i = curr_idx + 1; 0 <= i && i < laser_points.size(); ++i) { 162 | double dx = laser_points[i].x - laser_points[curr_idx].x; 163 | double dy = laser_points[i].y - laser_points[curr_idx].y; 164 | double distance = hypot(dx, dy); 165 | if (distance < min_adjacent_threshold_) { 166 | continue; 167 | } 168 | 169 | if (min_adjacent_threshold_ <= distance && distance < max_adjacent_threshold_) { 170 | right_normal_x = -dy / distance; 171 | right_normal_y = dx / distance; 172 | return true; 173 | } 174 | 175 | if (max_adjacent_threshold_ <= distance ) { 176 | right_normal_x = 0.0; 177 | right_normal_y = 0.0; 178 | return false; 179 | } 180 | } 181 | 182 | right_normal_x = 0.0; 183 | right_normal_y = 0.0; 184 | return false; 185 | } 186 | 187 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/ScanPreprocessor/ScanPreprocessor.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_SCANPREPROCESSOR_H_ 2 | #define SLAM_SCANPREPROCESSOR_H_ 3 | 4 | 5 | #include 6 | #include 7 | 8 | // utility/ 9 | #include "LaserPoint2D.h" 10 | #include "MyUtility.h" 11 | 12 | 13 | namespace sample_slam { 14 | 15 | class ScanPreprocessor { 16 | public: 17 | ScanPreprocessor() 18 | : min_interpolation_threshold_(0.05), max_interpolation_threshold_(0.25), 19 | cumulative_distance_(0.0), 20 | min_adjacent_threshold_(0.06), max_adjacent_threshold_(1.0), 21 | corner_angle_threshold_(DEG2RAD(45.0)) {} 22 | 23 | ~ScanPreprocessor() {} 24 | 25 | void ResampleLaserPoints(std::vector& laser_points); 26 | 27 | void CalcNormalVectors(std::vector& laser_points) const; 28 | 29 | 30 | private: 31 | void ResamplePoint(const LaserPoint2D& prev_laser_point, const LaserPoint2D& curr_laser_point, 32 | LaserPoint2D& resampled_laser_point, bool& is_resampled, bool& is_interpolated); 33 | 34 | bool CalcLeftNormalVector(int curr_idx, const std::vector& laser_points, 35 | double& left_normal_x, double& left_normal_y) const; 36 | 37 | bool CalcRightNormalVector(int curr_idx, const std::vector& laser_points, 38 | double& right_normal_x, double& right_normal_y) const; 39 | 40 | double min_interpolation_threshold_; // [m] 41 | double max_interpolation_threshold_; // [m] 42 | double cumulative_distance_; // [m] 43 | double min_adjacent_threshold_; // [m] 44 | double max_adjacent_threshold_; // [m] 45 | double corner_angle_threshold_; // [rad] 46 | }; 47 | 48 | } // namespace sample_slam 49 | 50 | 51 | #endif // SLAM_SCANPREPROCESSOR_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/SlamBackend/SlamBackend.cpp: -------------------------------------------------------------------------------- 1 | #include "SlamBackend.h" 2 | 3 | #include // It is needed to use p2o.h 4 | 5 | #include "p2o.h" 6 | 7 | 8 | namespace sample_slam { 9 | 10 | void SlamBackend::AdjustPoses(int pose_adjustment_iteration) { 11 | const std::vector& node_ptrs = pose_graph_ptr_->GetNodePtrs(); 12 | std::vector p2o_poses; 13 | for (std::size_t i = 0; i < node_ptrs.size(); ++i) { 14 | const Pose2D& node_pose = node_ptrs[i]->pose; 15 | p2o_poses.emplace_back(p2o::Pose2D(node_pose.x, node_pose.y, node_pose.yaw)); 16 | } 17 | 18 | const std::vector& arc_ptrs = pose_graph_ptr_->GetArcPtrs(); 19 | p2o::Con2DVec p2o_cons; 20 | for (std::size_t i = 0; i < arc_ptrs.size(); ++i) { 21 | p2o::Con2D p2o_con; 22 | p2o_con.id1 = arc_ptrs[i]->src_node_ptr->id; 23 | p2o_con.id2 = arc_ptrs[i]->dst_node_ptr->id; 24 | 25 | const Pose2D& arc_pose_relative = arc_ptrs[i]->pose_relative; 26 | p2o_con.t = p2o::Pose2D(arc_pose_relative.x, arc_pose_relative.y, arc_pose_relative.yaw); 27 | 28 | const Eigen::Matrix3f& arc_information_relative = arc_ptrs[i]->information_relative; 29 | int dimension = 3; 30 | for (std::size_t h = 0; h < dimension; ++h) { 31 | for (std::size_t w = 0; w < dimension; ++w) { 32 | p2o_con.info(h, w) = arc_information_relative(h, w); 33 | } 34 | } 35 | 36 | p2o_cons.emplace_back(p2o_con); 37 | } 38 | 39 | p2o::Optimizer2D p2o_optimizer; 40 | std::vector p2o_adjusted_poses = p2o_optimizer.optimizePath(p2o_poses, 41 | p2o_cons, 42 | pose_adjustment_iteration); 43 | 44 | adjusted_poses_.clear(); 45 | for (std::size_t i = 0; i < p2o_adjusted_poses.size(); ++i) { 46 | Pose2D adjusted_pose(p2o_adjusted_poses[i].x, p2o_adjusted_poses[i].y, p2o_adjusted_poses[i].th); 47 | adjusted_poses_.emplace_back(adjusted_pose); 48 | } 49 | 50 | return; 51 | } 52 | 53 | 54 | void SlamBackend::RemakeMap() { 55 | std::vector& node_ptrs = pose_graph_ptr_->GetNodePtrs(); 56 | for (std::size_t i = 0; i < adjusted_poses_.size(); ++i) { 57 | node_ptrs[i]->pose = adjusted_poses_[i]; 58 | } 59 | 60 | point_cloud_map_ptr_->RemakeMap(adjusted_poses_); 61 | 62 | return; 63 | } 64 | 65 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/SlamBackend/SlamBackend.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_SLAMBACKEND_H_ 2 | #define SLAM_SLAMBACKEND_H_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | 9 | //slam/ 10 | #include "PointCloudMap_SubMap.h" 11 | #include "PoseGraph.h" 12 | // utility/ 13 | #include "Pose2D.h" 14 | 15 | 16 | namespace sample_slam { 17 | 18 | class SlamBackend { 19 | public: 20 | SlamBackend() {} 21 | 22 | ~SlamBackend() {} 23 | 24 | void SetPointCloudMapPtr(PointCloudMap_SubMap* point_cloud_map_ptr) { 25 | point_cloud_map_ptr_ = point_cloud_map_ptr; 26 | } 27 | 28 | void SetPoseGraphPtr(PoseGraph* pose_graph_ptr) { 29 | pose_graph_ptr_ = pose_graph_ptr; 30 | } 31 | 32 | void AdjustPoses(int pose_adjustment_iteration); 33 | 34 | void RemakeMap(); 35 | 36 | 37 | private: 38 | PointCloudMap_SubMap* point_cloud_map_ptr_; 39 | PoseGraph* pose_graph_ptr_; 40 | 41 | std::vector adjusted_poses_; 42 | }; 43 | 44 | } // namespace sample_slam 45 | 46 | 47 | #endif // SLAM_SLAMBACKEND_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/SlamFrontend/SlamFrontend.cpp: -------------------------------------------------------------------------------- 1 | #include "SlamFrontend.h" 2 | 3 | // slam/ 4 | #include "CovarianceCalculator.h" 5 | 6 | 7 | namespace sample_slam { 8 | 9 | void SlamFrontend::RunSlam(const Scan2D& scan, int count) { 10 | // Scan matching 11 | scan_matcher_.MatchScan(scan, count); 12 | 13 | // Pose graph 14 | const Pose2D& estimated_pose = point_cloud_map_ptr_->GetLastPose(); 15 | if (count == 0) { 16 | pose_graph_.AddNode(estimated_pose); 17 | } else { 18 | AddMoveArc(estimated_pose, scan_matcher_.GetMoveCovariance()); 19 | } 20 | 21 | // Mapping 22 | if (count % num_skip_mapping_ == 0) { 23 | if (count == 0) { 24 | point_cloud_map_ptr_->SetNumPointThreshold(1); 25 | } else { 26 | point_cloud_map_ptr_->SetNumPointThreshold(num_point_threshold_); 27 | } 28 | 29 | point_cloud_map_ptr_->MakeGlobalMap(); 30 | } 31 | 32 | // Loop closure 33 | if (is_loop_closure_ && count % num_skip_mapping_ == 0 && num_skip_mapping_ < count) { 34 | LoopInfo loop_info; 35 | bool is_loop_detected = loop_detector_.DetectLoop(scan, estimated_pose, count, 36 | loop_info); 37 | 38 | if (is_loop_detected) { 39 | AddLoopArc(loop_info); 40 | 41 | slam_backend_.AdjustPoses(pose_adjustment_iteration_); 42 | slam_backend_.RemakeMap(); 43 | } 44 | } 45 | 46 | return; 47 | } 48 | 49 | 50 | void SlamFrontend::AddMoveArc(const Pose2D& estimated_pose, const Eigen::Matrix3f& move_covariance) { 51 | if (pose_graph_.GetNodePtrsSize() == 0) { 52 | std::cerr << "[SlamFrontend::AddMoveArc()] Error: `PoseGraph::node_ptrs` are empty \n"; 53 | return; 54 | } 55 | 56 | PoseNode* last_node_ptr = pose_graph_.GetNodePtrsBack(); 57 | PoseNode* curr_node_ptr = pose_graph_.AddNode(estimated_pose); 58 | 59 | Pose2D last_node_pose = last_node_ptr->pose; 60 | Pose2D move_pose_relative = estimated_pose - last_node_pose; 61 | 62 | Eigen::Matrix3f move_covariance_relative; 63 | CovarianceCalculator::RotateAbsolute2Relative(last_node_pose, move_covariance, 64 | move_covariance_relative); 65 | 66 | PoseArc* arc_ptr = pose_graph_.AddArc(last_node_ptr->id, curr_node_ptr->id, 67 | move_pose_relative, move_covariance_relative); 68 | 69 | return; 70 | } 71 | 72 | 73 | void SlamFrontend::AddLoopArc(LoopInfo& loop_info) { 74 | if (loop_info.is_arc) { 75 | return; 76 | } 77 | 78 | loop_info.is_arc = true; 79 | 80 | const std::vector& map_poses = point_cloud_map_ptr_->GetPoses(); 81 | Pose2D src_pose(map_poses[loop_info.reference_scan_id]); 82 | 83 | Pose2D dst_pose(loop_info.revisit_pose); 84 | Pose2D dst_pose_relative = dst_pose - src_pose; 85 | 86 | Eigen::Matrix3f scan_match_covariance_relative; 87 | CovarianceCalculator::RotateAbsolute2Relative(src_pose, loop_info.scan_match_covariance, 88 | scan_match_covariance_relative); 89 | 90 | PoseArc* arc_ptr = pose_graph_.AddArc(loop_info.reference_scan_id, loop_info.curr_scan_id, 91 | dst_pose_relative, scan_match_covariance_relative); 92 | 93 | return; 94 | } 95 | 96 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/slam/SlamFrontend/SlamFrontend.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_SLAMFRONTEND_H_ 2 | #define SLAM_SLAMFRONTEND_H_ 3 | 4 | 5 | // slam/ 6 | #include "LoopDetector.h" 7 | #include "PointCloudMap.h" 8 | #include "PoseGraph.h" 9 | #include "ScanMatcher.h" 10 | #include "SlamBackend.h" 11 | // utility/ 12 | #include "Scan2D.h" 13 | 14 | 15 | namespace sample_slam { 16 | 17 | class SlamFrontend { 18 | public: 19 | SlamFrontend() 20 | : point_cloud_map_ptr_(nullptr), 21 | num_skip_mapping_(10), 22 | num_point_threshold_(5), 23 | is_loop_closure_(false), 24 | pose_adjustment_iteration_(5) { 25 | loop_detector_.SetPoseGraphPtr(&pose_graph_); 26 | slam_backend_.SetPoseGraphPtr(&pose_graph_); 27 | } 28 | 29 | ~SlamFrontend() {} 30 | 31 | void SetPointCloudMapPtr(PointCloudMap* point_cloud_map_ptr) { 32 | point_cloud_map_ptr_ = point_cloud_map_ptr; 33 | } 34 | 35 | void SetIsLoopClosure(bool is_loop_closure) { 36 | is_loop_closure_ = is_loop_closure; 37 | } 38 | 39 | ScanMatcher* GetScanMatcherPtr() { 40 | return &scan_matcher_; 41 | } 42 | 43 | LoopDetector* GetLoopDetectorPtr() { 44 | return &loop_detector_; 45 | } 46 | 47 | SlamBackend* GetSlamBackendPtr() { 48 | return &slam_backend_; 49 | } 50 | 51 | void RunSlam(const Scan2D& scan, int count); 52 | 53 | void AddMoveArc(const Pose2D& estimated_pose, const Eigen::Matrix3f& move_covariance); 54 | 55 | void AddLoopArc(LoopInfo& loop_info); 56 | 57 | 58 | private: 59 | PointCloudMap* point_cloud_map_ptr_; // Pointer to abstract class, which is customizable 60 | 61 | ScanMatcher scan_matcher_; 62 | PoseGraph pose_graph_; 63 | LoopDetector loop_detector_; 64 | SlamBackend slam_backend_; 65 | 66 | int num_skip_mapping_; 67 | int num_point_threshold_; 68 | bool is_loop_closure_; 69 | int pose_adjustment_iteration_; 70 | }; 71 | 72 | } // namespace sample_slam 73 | 74 | 75 | #endif // SLAM_SLAMFRONTEND_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/solver/p2o/.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /sample_code_cpp/src/solver/p2o/README.md: -------------------------------------------------------------------------------- 1 | # p2o: Petite Portable Pose-graph Optimizer 2 | 3 | p2o is a header-only 2D/3D pose-graph optimization library. 4 | 5 | Current status: BETA 6 | 7 | full features are available, APIs are subject to change. 8 | 9 | # Prerequisits 10 | p2o is dependent on Eigen3 and C++11. 11 | 12 | # How to run samples 13 | run samples/build_and_run_p2o.sh 14 | 15 | -------------------------------------------------------------------------------- /sample_code_cpp/src/solver/p2o/samples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | project(test_p2o) 4 | 5 | IF(NOT CMAKE_BUILD_TYPE) 6 | SET(CMAKE_BUILD_TYPE Release) 7 | ENDIF(NOT CMAKE_BUILD_TYPE) 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | set (CMAKE_CXX_STANDARD 11) 12 | include_directories( 13 | ${CMAKE_CURRENT_SOURCE_DIR}/../ 14 | ${EIGEN3_INCLUDE_DIR} 15 | ) 16 | 17 | link_directories( 18 | ) 19 | 20 | add_executable(test_p2o 21 | ${CMAKE_CURRENT_SOURCE_DIR}/../p2o.h 22 | test_p2o.cpp 23 | ) 24 | 25 | target_link_libraries(test_p2o 26 | ) 27 | 28 | -------------------------------------------------------------------------------- /sample_code_cpp/src/solver/p2o/samples/build_and_run_p2o.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # download data by Luca Carlone https://lucacarlone.mit.edu/datasets/ 4 | 5 | wget https://www.dropbox.com/s/vcz8cag7bo0zlaj/input_INTEL_g2o.g2o?dl=0 -O intel.g2o -nc 6 | wget https://www.dropbox.com/s/d8fcn1jg1mebx8f/input_MITb_g2o.g2o?dl=0 -O mit_killian.g2o -nc 7 | wget https://www.dropbox.com/s/gmdzo74b3tzvbrw/input_M3500_g2o.g2o?dl=0 -O manhattan3500.g2o -nc 8 | wget https://www.dropbox.com/s/zu23p8d522qccor/parking-garage.g2o?dl=0 -O parking-garage.g2o -nc 9 | 10 | # modified version of datasets 11 | 12 | wget http://www.furo.org/irie/datasets/sphere2200_guess.g2o -nc 13 | wget http://www.furo.org/irie/datasets/torus3d_guess.g2o -nc 14 | 15 | # build p2o sample 16 | 17 | cmake `dirname $0` 18 | make 19 | 20 | # run p2o sample 21 | 22 | ./test_p2o 23 | gnuplot `dirname $0`/plot_data.plt 24 | 25 | -------------------------------------------------------------------------------- /sample_code_cpp/src/solver/p2o/samples/plot_data.plt: -------------------------------------------------------------------------------- 1 | #!/usr/bin/gnuplot -persist 2 | # 3 | # 4 | # G N U P L O T 5 | # Version 5.0 patchlevel 3 last modified 2016-02-21 6 | # 7 | # Copyright (C) 1986-1993, 1998, 2004, 2007-2016 8 | # Thomas Williams, Colin Kelley and many others 9 | # 10 | # gnuplot home: http://www.gnuplot.info 11 | # faq, bugs, etc: type "help FAQ" 12 | # immediate help: type "help" (plot window: hit 'h') 13 | # set terminal qt 0 font "Sans,9" 14 | # set output 15 | unset clip points 16 | set clip one 17 | unset clip two 18 | set bar 1.000000 front 19 | set border 31 front lt black linewidth 1.000 dashtype solid 20 | set zdata 21 | set ydata 22 | set xdata 23 | set y2data 24 | set x2data 25 | set boxwidth 26 | set style fill empty border 27 | set style rectangle back fc bgnd fillstyle solid 1.00 border lt -1 28 | set style circle radius graph 0.02, first 0.00000, 0.00000 29 | set style ellipse size graph 0.05, 0.03, first 0.00000 angle 0 units xy 30 | set dummy x, y 31 | set format x "% h" 32 | set format y "% h" 33 | set format x2 "% h" 34 | set format y2 "% h" 35 | set format z "% h" 36 | set format cb "% h" 37 | set format r "% h" 38 | set timefmt "%d/%m/%y,%H:%M" 39 | set angles radians 40 | set tics back 41 | unset grid 42 | set raxis 43 | set style parallel front lt black linewidth 2.000 dashtype solid 44 | set key title "" center 45 | set key inside right top vertical Right noreverse enhanced autotitle nobox 46 | set key noinvert samplen 4 spacing 1 width 0 height 0 47 | set key maxcolumns 0 maxrows 0 48 | set key noopaque 49 | unset label 50 | unset arrow 51 | set style increment default 52 | unset style line 53 | unset style arrow 54 | set style histogram clustered gap 2 title textcolor lt -1 55 | unset object 56 | set style textbox transparent margins 1.0, 1.0 border 57 | unset logscale 58 | set offsets 0, 0, 0, 0 59 | set pointsize 1 60 | set pointintervalbox 1 61 | set encoding default 62 | unset polar 63 | unset parametric 64 | unset decimalsign 65 | set view 60, 30, 1, 1 66 | set samples 100, 100 67 | set isosamples 10, 10 68 | set surface 69 | unset contour 70 | set cntrlabel format '%8.3g' font '' start 5 interval 20 71 | set mapping cartesian 72 | set datafile separator whitespace 73 | unset hidden3d 74 | set cntrparam order 4 75 | set cntrparam linear 76 | set cntrparam levels auto 5 77 | set cntrparam points 5 78 | set size ratio -1 1,1 79 | set origin 0,0 80 | set style data points 81 | set style function lines 82 | unset xzeroaxis 83 | unset yzeroaxis 84 | unset zzeroaxis 85 | unset x2zeroaxis 86 | unset y2zeroaxis 87 | set xyplane relative 0.5 88 | set tics scale 1, 0.5, 1, 1, 1 89 | set mxtics default 90 | set mytics default 91 | set mztics default 92 | set mx2tics default 93 | set my2tics default 94 | set mcbtics default 95 | set mrtics default 96 | set xtics border in scale 1,0.5 mirror norotate autojustify 97 | set xtics norangelimit autofreq 98 | set ytics border in scale 1,0.5 mirror norotate autojustify 99 | set ytics norangelimit autofreq 100 | set ztics border in scale 1,0.5 nomirror norotate autojustify 101 | set ztics norangelimit autofreq 102 | unset x2tics 103 | unset y2tics 104 | set cbtics border in scale 1,0.5 mirror norotate autojustify 105 | set cbtics norangelimit autofreq 106 | set rtics axis in scale 1,0.5 nomirror norotate autojustify 107 | set rtics norangelimit autofreq 108 | unset paxis 1 tics 109 | unset paxis 2 tics 110 | unset paxis 3 tics 111 | unset paxis 4 tics 112 | unset paxis 5 tics 113 | unset paxis 6 tics 114 | unset paxis 7 tics 115 | set title "" 116 | set title font "" norotate 117 | set timestamp bottom 118 | set timestamp "" 119 | set timestamp font "" norotate 120 | set rrange [ * : * ] noreverse nowriteback 121 | set trange [ * : * ] noreverse nowriteback 122 | set urange [ * : * ] noreverse nowriteback 123 | set vrange [ * : * ] noreverse nowriteback 124 | set xlabel "" 125 | set xlabel font "" textcolor lt -1 norotate 126 | set x2label "" 127 | set x2label font "" textcolor lt -1 norotate 128 | set xrange [ * : * ] noreverse nowriteback 129 | set x2range [ * : * ] noreverse nowriteback 130 | set ylabel "" 131 | set ylabel font "" textcolor lt -1 rotate by -270 132 | set y2label "" 133 | set y2label font "" textcolor lt -1 rotate by -270 134 | set yrange [ * : * ] noreverse nowriteback 135 | set y2range [ * : * ] noreverse nowriteback 136 | set zlabel "" 137 | set zlabel font "" textcolor lt -1 norotate 138 | set zrange [ * : * ] noreverse nowriteback 139 | set cblabel "" 140 | set cblabel font "" textcolor lt -1 rotate by -270 141 | set cbrange [ * : * ] noreverse nowriteback 142 | set paxis 1 range [ * : * ] noreverse nowriteback 143 | set paxis 2 range [ * : * ] noreverse nowriteback 144 | set paxis 3 range [ * : * ] noreverse nowriteback 145 | set paxis 4 range [ * : * ] noreverse nowriteback 146 | set paxis 5 range [ * : * ] noreverse nowriteback 147 | set paxis 6 range [ * : * ] noreverse nowriteback 148 | set paxis 7 range [ * : * ] noreverse nowriteback 149 | set zero 1e-08 150 | set lmargin -1 151 | set bmargin -1 152 | set rmargin -1 153 | set tmargin -1 154 | set locale "ja_JP.UTF-8" 155 | set pm3d explicit at s 156 | set pm3d scansautomatic 157 | set pm3d interpolate 1,1 flush begin noftriangles noborder corners2color mean 158 | set palette positive nops_allcF maxcolors 0 gamma 1.5 color model RGB 159 | set palette rgbformulae 7, 5, 15 160 | set colorbox default 161 | set colorbox vertical origin screen 0.9, 0.2, 0 size screen 0.05, 0.6, 0 front bdefault 162 | set style boxplot candles range 1.50 outliers pt 7 separation 1 labels auto unsorted 163 | set loadpath 164 | set fontpath 165 | set psdir 166 | set fit brief errorvariables nocovariancevariables errorscaling prescale nowrap v5 167 | GNUTERM = "qt" 168 | x = 0.0 169 | ## Last datafile plotted: "manhattan3500.g2o_out.txt" 170 | set term x11 0 171 | plot 'intel.g2o_in.txt' w l, 'intel.g2o_out.txt' w l 172 | set term x11 1 173 | plot 'manhattan3500.g2o_in.txt' w l, 'manhattan3500.g2o_out.txt' w l 174 | set term x11 2 175 | plot 'mit_killian.g2o_in.txt' w l, 'mit_killian.g2o_out.txt' w l 176 | set term x11 3 177 | set view equal xyz 178 | splot 'parking-garage.g2o_in.txt' w l, 'parking-garage.g2o_out.txt' w l 179 | set term x11 4 180 | splot 'torus3d_guess.g2o_in.txt' w l, 'torus3d_guess.g2o_out.txt' w l 181 | set term x11 5 182 | splot 'sphere2200_guess.g2o_in.txt' w l, 'sphere2200_guess.g2o_out.txt' w l 183 | pause -1 184 | # EOF 185 | -------------------------------------------------------------------------------- /sample_code_cpp/src/solver/p2o/samples/test_p2o.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2017 Future Robotics Technology Center (fuRo), 3 | * Chiba Institute of Technology. 4 | * 5 | * This Source Code Form is subject to the terms of the Mozilla Public 6 | * License, v. 2.0. If a copy of the MPL was not distributed with this file, 7 | * You can obtain one at https://mozilla.org/MPL/2.0/. 8 | ****************************************************************************/ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | void sample_g2o_2d(const std::string &filename, int max_iter, int min_iter, double robust_thre) 15 | { 16 | std::string fname_in = filename + "_in.txt"; 17 | std::string fname_out = filename + "_out.txt"; 18 | std::ofstream ofs(fname_in); 19 | std::ofstream ofs2(fname_out); 20 | p2o::Pose2DVec nodes; 21 | p2o::Con2DVec con; 22 | p2o::Optimizer2D optimizer; 23 | optimizer.setLambda(1e-6); 24 | optimizer.setVerbose(true); 25 | if (!optimizer.loadFile(filename.c_str(), nodes, con)) { 26 | std::cout << "can't open file: " << filename << std::endl; 27 | return; 28 | } 29 | 30 | optimizer.setRobustThreshold(robust_thre); 31 | auto t0 = std::chrono::high_resolution_clock::now(); 32 | p2o::Pose2DVec result = optimizer.optimizePath(nodes, con, max_iter, min_iter); 33 | auto t1 = std::chrono::high_resolution_clock::now(); 34 | auto elapsed = std::chrono::duration_cast< std::chrono::microseconds> (t1-t0); 35 | std::cout << filename << ": " << elapsed.count()*1e-6 << "s" << std::endl; 36 | for(int i=1; i (t1-t0); 63 | std::cout << filename << ": " << elapsed.count()*1e-6 << "s" << std::endl; 64 | for(int i=1; i::max()); 77 | 78 | // robust 79 | sample_g2o_2d("mit_killian.g2o", 20, 20, 1); 80 | 81 | // 3D 82 | sample_g2o_3d("parking-garage.g2o", 10, 3, 1); 83 | 84 | sample_g2o_3d("torus3d_guess.g2o", 10, 3, 1); 85 | 86 | sample_g2o_3d("sphere2200_guess.g2o", 10, 3, 1); 87 | 88 | return 0; 89 | } 90 | 91 | -------------------------------------------------------------------------------- /sample_code_cpp/src/test/launcher/CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_cpp/src/test/launcher/CMakeLists.txt -------------------------------------------------------------------------------- /sample_code_cpp/src/test/slam/CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_cpp/src/test/slam/CMakeLists.txt -------------------------------------------------------------------------------- /sample_code_cpp/src/test/utility/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | include(${CMAKE_SOURCE_DIR}/cmake/DownloadProject/DownloadProject.cmake) 4 | download_project( 5 | PROJ googletest 6 | GIT_REPOSITORY https://github.com/google/googletest.git 7 | GIT_TAG master 8 | UPDATE_DISCONNECTED 1 9 | ) 10 | 11 | add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR}) 12 | 13 | add_executable(utility_test 14 | ./MyUtility_test.cpp 15 | ./LaserPoint2D_test.cpp 16 | ./Pose2D_test.cpp 17 | ) 18 | target_compile_features(utility_test PRIVATE cxx_std_11) 19 | 20 | target_link_libraries(utility_test 21 | utility 22 | gtest_main 23 | ) 24 | 25 | include(GoogleTest) 26 | gtest_add_tests(TARGET utility_test) -------------------------------------------------------------------------------- /sample_code_cpp/src/test/utility/LaserPoint2D_test.cpp: -------------------------------------------------------------------------------- 1 | #include "LaserPoint2D.h" 2 | 3 | #include 4 | #include 5 | // #include 6 | 7 | // #include 8 | // #include 9 | 10 | #include "gtest/gtest.h" 11 | #include "MyUtility.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | #define DEBUG_PRINT(var) std::cout << #var << " = " << var << "\n"; 17 | 18 | 19 | class LaserPoint2DTest : public ::testing::Test { 20 | protected: 21 | virtual void SetUp() {} 22 | virtual void TearDown() {} 23 | 24 | double max_range_ = 2.0; // [m] 25 | double delta_range_ = 1.0; // [m] 26 | }; 27 | 28 | 29 | TEST_F(LaserPoint2DTest, RangeYaw2XYTest) { 30 | LaserPoint2D laser_point; 31 | for (double range = -max_range_; range <= max_range_; range += delta_range_) { 32 | laser_point.RangeYaw2XY(range, DEG2RAD(-180.0)); 33 | EXPECT_DOUBLE_EQ(-range, laser_point.x); 34 | EXPECT_NEAR(0.0, laser_point.y, 1.0e-6); 35 | 36 | laser_point.RangeYaw2XY(range, DEG2RAD(-150.0)); 37 | EXPECT_DOUBLE_EQ(-range * (sqrt(3.0) / 2.0), laser_point.x); 38 | EXPECT_DOUBLE_EQ(-range * (1.0 / 2.0), laser_point.y); 39 | 40 | laser_point.RangeYaw2XY(range, DEG2RAD(-135.0)); 41 | EXPECT_DOUBLE_EQ(-range / sqrt(2.0), laser_point.x); 42 | EXPECT_DOUBLE_EQ(-range / sqrt(2.0), laser_point.y); 43 | 44 | laser_point.RangeYaw2XY(range, DEG2RAD(-120.0)); 45 | EXPECT_DOUBLE_EQ(-range * (1.0 / 2.0), laser_point.x); 46 | EXPECT_DOUBLE_EQ(-range * (sqrt(3.0) / 2.0), laser_point.y); 47 | 48 | laser_point.RangeYaw2XY(range, DEG2RAD(-90.0)); 49 | EXPECT_NEAR(0.0, laser_point.x, 1.0e-6); 50 | EXPECT_DOUBLE_EQ(-range, laser_point.y); 51 | 52 | laser_point.RangeYaw2XY(range, DEG2RAD(-60.0)); 53 | EXPECT_DOUBLE_EQ(range * (1.0 / 2.0), laser_point.x); 54 | EXPECT_DOUBLE_EQ(-range * (sqrt(3.0) / 2.0), laser_point.y); 55 | 56 | laser_point.RangeYaw2XY(range, DEG2RAD(-45.0)); 57 | EXPECT_DOUBLE_EQ(range / sqrt(2.0), laser_point.x); 58 | EXPECT_DOUBLE_EQ(-range / sqrt(2.0), laser_point.y); 59 | 60 | laser_point.RangeYaw2XY(range, DEG2RAD(-30.0)); 61 | EXPECT_DOUBLE_EQ(range * (sqrt(3.0) / 2.0), laser_point.x); 62 | EXPECT_DOUBLE_EQ(-range * (1.0 / 2.0), laser_point.y); 63 | 64 | laser_point.RangeYaw2XY(range, DEG2RAD(0.0)); 65 | EXPECT_DOUBLE_EQ(range, laser_point.x); 66 | EXPECT_NEAR(0.0, laser_point.y, 1.0e-6); 67 | 68 | laser_point.RangeYaw2XY(range, DEG2RAD(30.0)); 69 | EXPECT_DOUBLE_EQ(range * (sqrt(3.0) / 2.0), laser_point.x); 70 | EXPECT_DOUBLE_EQ(range * (1.0 / 2.0), laser_point.y); 71 | 72 | laser_point.RangeYaw2XY(range, DEG2RAD(45.0)); 73 | EXPECT_DOUBLE_EQ(range / sqrt(2.0), laser_point.x); 74 | EXPECT_DOUBLE_EQ(range / sqrt(2.0), laser_point.y); 75 | 76 | laser_point.RangeYaw2XY(range, DEG2RAD(60.0)); 77 | EXPECT_DOUBLE_EQ(range * (1.0 / 2.0), laser_point.x); 78 | EXPECT_DOUBLE_EQ(range * (sqrt(3.0) / 2.0), laser_point.y); 79 | 80 | laser_point.RangeYaw2XY(range, DEG2RAD(90.0)); 81 | EXPECT_NEAR(0.0, laser_point.x, 1.0e-6); 82 | EXPECT_DOUBLE_EQ(range, laser_point.y); 83 | 84 | laser_point.RangeYaw2XY(range, DEG2RAD(120.0)); 85 | EXPECT_DOUBLE_EQ(-range * (1.0 / 2.0), laser_point.x); 86 | EXPECT_DOUBLE_EQ(range * (sqrt(3.0) / 2.0), laser_point.y); 87 | 88 | laser_point.RangeYaw2XY(range, DEG2RAD(135.0)); 89 | EXPECT_DOUBLE_EQ(-range / sqrt(2.0), laser_point.x); 90 | EXPECT_DOUBLE_EQ(range / sqrt(2.0), laser_point.y); 91 | 92 | laser_point.RangeYaw2XY(range, DEG2RAD(150.0)); 93 | EXPECT_DOUBLE_EQ(-range * (sqrt(3.0) / 2.0), laser_point.x); 94 | EXPECT_DOUBLE_EQ(range * (1.0 / 2.0), laser_point.y); 95 | 96 | laser_point.RangeYaw2XY(range, DEG2RAD(180.0)); 97 | EXPECT_DOUBLE_EQ(-range, laser_point.x); 98 | EXPECT_NEAR(0.0, laser_point.y, 1.0e-6); 99 | } 100 | } 101 | 102 | 103 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/test/utility/MyUtility_test.cpp: -------------------------------------------------------------------------------- 1 | #include "MyUtility.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "gtest/gtest.h" 10 | 11 | 12 | namespace sample_slam { 13 | 14 | #define DEBUG_PRINT(var) std::cout << #var << " = " << var << "\n"; 15 | 16 | 17 | class MyUtilityTest : public ::testing::Test { 18 | protected: 19 | virtual void SetUp() {} 20 | virtual void TearDown() {} 21 | }; 22 | 23 | 24 | TEST_F(MyUtilityTest, CastRadianTest) { 25 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::CastRadian(-4.0*M_PI)); 26 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::CastRadian(-3.5*M_PI)); 27 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::CastRadian(-3.0*M_PI)); 28 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::CastRadian(-2.5*M_PI)); 29 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::CastRadian(-2.0*M_PI)); 30 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::CastRadian(-1.5*M_PI)); 31 | 32 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::CastRadian(-1.0*M_PI)); 33 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::CastRadian(-0.5*M_PI)); 34 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::CastRadian(0.0*M_PI)); 35 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::CastRadian(0.5*M_PI)); 36 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::CastRadian(1.0*M_PI)); 37 | 38 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::CastRadian(1.5*M_PI)); 39 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::CastRadian(2.0*M_PI)); 40 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::CastRadian(2.5*M_PI)); 41 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::CastRadian(3.0*M_PI)); 42 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::CastRadian(3.5*M_PI)); 43 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::CastRadian(4.0*M_PI)); 44 | } 45 | 46 | 47 | TEST_F(MyUtilityTest, AddRadianTest) { 48 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::AddRadian(-1.0*M_PI, -1.0*M_PI)); 49 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::AddRadian(-1.0*M_PI, -0.5*M_PI)); 50 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::AddRadian(-1.0*M_PI, 0.0*M_PI)); 51 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::AddRadian(-1.0*M_PI, 0.5*M_PI)); 52 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::AddRadian(-1.0*M_PI, 1.0*M_PI)); 53 | 54 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::AddRadian(-0.5*M_PI, -1.0*M_PI)); 55 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::AddRadian(-0.5*M_PI, -0.5*M_PI)); 56 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::AddRadian(-0.5*M_PI, 0.0*M_PI)); 57 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::AddRadian(-0.5*M_PI, 0.5*M_PI)); 58 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::AddRadian(-0.5*M_PI, 1.0*M_PI)); 59 | 60 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::AddRadian(0.0*M_PI, -1.0*M_PI)); 61 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::AddRadian(0.0*M_PI, -0.5*M_PI)); 62 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::AddRadian(0.0*M_PI, 0.0*M_PI)); 63 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::AddRadian(0.0*M_PI, 0.5*M_PI)); 64 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::AddRadian(0.0*M_PI, 1.0*M_PI)); 65 | 66 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::AddRadian(0.5*M_PI, -1.0*M_PI)); 67 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::AddRadian(0.5*M_PI, -0.5*M_PI)); 68 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::AddRadian(0.5*M_PI, 0.0*M_PI)); 69 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::AddRadian(0.5*M_PI, 0.5*M_PI)); 70 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::AddRadian(0.5*M_PI, 1.0*M_PI)); 71 | 72 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::AddRadian(1.0*M_PI, -1.0*M_PI)); 73 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::AddRadian(1.0*M_PI, -0.5*M_PI)); 74 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::AddRadian(1.0*M_PI, 0.0*M_PI)); 75 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::AddRadian(1.0*M_PI, 0.5*M_PI)); 76 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::AddRadian(1.0*M_PI, 1.0*M_PI)); 77 | } 78 | 79 | 80 | TEST_F(MyUtilityTest, SubtractRadianTest) { 81 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::SubtractRadian(-1.0*M_PI, -1.0*M_PI)); 82 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::SubtractRadian(-1.0*M_PI, -0.5*M_PI)); 83 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::SubtractRadian(-1.0*M_PI, 0.0*M_PI)); 84 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::SubtractRadian(-1.0*M_PI, 0.5*M_PI)); 85 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::SubtractRadian(-1.0*M_PI, 1.0*M_PI)); 86 | 87 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::SubtractRadian(-0.5*M_PI, -1.0*M_PI)); 88 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::SubtractRadian(-0.5*M_PI, -0.5*M_PI)); 89 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::SubtractRadian(-0.5*M_PI, 0.0*M_PI)); 90 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::SubtractRadian(-0.5*M_PI, 0.5*M_PI)); 91 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::SubtractRadian(-0.5*M_PI, 1.0*M_PI)); 92 | 93 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::SubtractRadian(0.0*M_PI, -1.0*M_PI)); 94 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::SubtractRadian(0.0*M_PI, -0.5*M_PI)); 95 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::SubtractRadian(0.0*M_PI, 0.0*M_PI)); 96 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::SubtractRadian(0.0*M_PI, 0.5*M_PI)); 97 | EXPECT_DOUBLE_EQ(-1.0*M_PI, MyUtility::SubtractRadian(0.0*M_PI, 1.0*M_PI)); 98 | 99 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::SubtractRadian(0.5*M_PI, -1.0*M_PI)); 100 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::SubtractRadian(0.5*M_PI, -0.5*M_PI)); 101 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::SubtractRadian(0.5*M_PI, 0.0*M_PI)); 102 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::SubtractRadian(0.5*M_PI, 0.5*M_PI)); 103 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::SubtractRadian(0.5*M_PI, 1.0*M_PI)); 104 | 105 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::SubtractRadian(1.0*M_PI, -1.0*M_PI)); 106 | EXPECT_DOUBLE_EQ(-0.5*M_PI, MyUtility::SubtractRadian(1.0*M_PI, -0.5*M_PI)); 107 | EXPECT_DOUBLE_EQ(1.0*M_PI, MyUtility::SubtractRadian(1.0*M_PI, 0.0*M_PI)); 108 | EXPECT_DOUBLE_EQ(0.5*M_PI, MyUtility::SubtractRadian(1.0*M_PI, 0.5*M_PI)); 109 | EXPECT_DOUBLE_EQ(0.0*M_PI, MyUtility::SubtractRadian(1.0*M_PI, 1.0*M_PI)); 110 | } 111 | 112 | 113 | TEST_F(MyUtilityTest, InverseMatrixSVDTest) { 114 | Eigen::Matrix3f indentity_matrix = Eigen::Matrix3f::Identity(); 115 | Eigen::Matrix3f expect_inverse_indentity_matrix = Eigen::Matrix3f::Identity(); 116 | Eigen::Matrix3f inverse_indentity_matrix; 117 | MyUtility::InverseMatrixSVD(indentity_matrix, inverse_indentity_matrix); 118 | EXPECT_TRUE(expect_inverse_indentity_matrix.isApprox(inverse_indentity_matrix)); 119 | 120 | double deg2rad_45 = DEG2RAD(45.0); 121 | Eigen::Matrix3f rotation_matrix_2d; 122 | rotation_matrix_2d << cos(deg2rad_45), -sin(deg2rad_45), 0.0, 123 | sin(deg2rad_45), cos(deg2rad_45), 0.0, 124 | 0.0, 0.0, 1.0; 125 | Eigen::Matrix3f expect_inverse_rotation_matrix_2d = rotation_matrix_2d.transpose(); 126 | Eigen::Matrix3f inverse_rotation_matrix_2d; 127 | MyUtility::InverseMatrixSVD(rotation_matrix_2d, inverse_rotation_matrix_2d); 128 | EXPECT_TRUE(expect_inverse_rotation_matrix_2d.isApprox(inverse_rotation_matrix_2d)); 129 | 130 | double deg2rad_30 = DEG2RAD(30.0); 131 | Eigen::Matrix3f rotation_matrix_yaw; 132 | rotation_matrix_yaw = Eigen::AngleAxisf(deg2rad_30, Eigen::Vector3f::UnitZ()); 133 | Eigen::Matrix3f rotation_matrix_pitch; 134 | rotation_matrix_pitch = Eigen::AngleAxisf(deg2rad_30, Eigen::Vector3f::UnitY()); 135 | Eigen::Matrix3f rotation_matrix_roll; 136 | rotation_matrix_roll = Eigen::AngleAxisf(deg2rad_30, Eigen::Vector3f::UnitX()); 137 | Eigen::Matrix3f rotation_matrix_3d = rotation_matrix_yaw 138 | * rotation_matrix_pitch 139 | * rotation_matrix_roll; 140 | Eigen::Matrix3f expect_inverse_rotation_matrix_3d = rotation_matrix_roll.transpose() 141 | * rotation_matrix_pitch.transpose() 142 | * rotation_matrix_yaw.transpose(); 143 | Eigen::Matrix3f inverse_rotation_matrix_3d; 144 | MyUtility::InverseMatrixSVD(rotation_matrix_3d, inverse_rotation_matrix_3d); 145 | EXPECT_TRUE(expect_inverse_rotation_matrix_3d.isApprox(inverse_rotation_matrix_3d)); 146 | } 147 | 148 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/test/utility/Pose2D_test.cpp: -------------------------------------------------------------------------------- 1 | #include "Pose2D.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "gtest/gtest.h" 11 | #include "LaserPoint2D.h" 12 | #include "MyUtility.h" 13 | 14 | 15 | namespace sample_slam { 16 | 17 | #define DEBUG_PRINT(var) std::cout << #var << " = " << var << "\n"; 18 | 19 | 20 | class Pose2DTest : public ::testing::Test { 21 | protected: 22 | virtual void SetUp() {} 23 | virtual void TearDown() {} 24 | 25 | double max_distance_ = 2.0; // [m] 26 | double delta_distance_ = 1.0; // [m] 27 | 28 | double max_angle_ = M_PI; // [rad] 29 | double delta_angle_ = 0.5*M_PI; // [rad] 30 | }; 31 | 32 | 33 | TEST_F(Pose2DTest, CalcRotationMatrixTest) { 34 | for (double yaw = -max_angle_; yaw <= max_angle_; yaw += delta_angle_) { 35 | Pose2D pose; 36 | pose.yaw = yaw; // [rad] 37 | pose.CalcRotationMatrix(); 38 | 39 | Eigen::Matrix2f eigen_rotation_matrix; 40 | eigen_rotation_matrix = Eigen::Rotation2Df(yaw); 41 | 42 | EXPECT_NEAR(eigen_rotation_matrix(0,0), pose.rotation_matrix[0][0], 1.0e-6); 43 | EXPECT_NEAR(eigen_rotation_matrix(0,1), pose.rotation_matrix[0][1], 1.0e-6); 44 | EXPECT_NEAR(eigen_rotation_matrix(1,0), pose.rotation_matrix[1][0], 1.0e-6); 45 | EXPECT_NEAR(eigen_rotation_matrix(1,1), pose.rotation_matrix[1][1], 1.0e-6); 46 | } 47 | } 48 | 49 | 50 | TEST_F(Pose2DTest, LocalCoord2GlobalCoordTest) { 51 | for (double pose_x = -max_distance_; pose_x <= max_distance_; pose_x += delta_distance_) { 52 | for (double pose_y = -max_distance_; pose_y <= max_distance_; pose_y += delta_distance_) { 53 | for (double pose_yaw = -max_angle_; pose_yaw <= max_angle_; pose_yaw += delta_angle_) { 54 | Pose2D pose(pose_x, pose_y, pose_yaw); 55 | 56 | Eigen::Affine2f eigen_affine_matrix = Eigen::Translation2f(pose_x, pose_y) 57 | * Eigen::Rotation2Df(pose_yaw); 58 | 59 | for (double local_laser_point_yaw = -max_angle_; local_laser_point_yaw <= max_angle_; local_laser_point_yaw += delta_angle_) { 60 | LaserPoint2D local_laser_point; 61 | local_laser_point.x = cos(local_laser_point_yaw); 62 | local_laser_point.y = sin(local_laser_point_yaw); 63 | LaserPoint2D global_laser_point; 64 | pose.LocalCoord2GlobalCoord(local_laser_point, global_laser_point); 65 | 66 | Eigen::Vector2f eigen_local_laser_point; 67 | eigen_local_laser_point << local_laser_point.x, 68 | local_laser_point.y; 69 | Eigen::Vector2f eigen_global_laser_point = eigen_affine_matrix * eigen_local_laser_point; 70 | 71 | EXPECT_NEAR(eigen_global_laser_point(0), global_laser_point.x, 1.0e-6); 72 | EXPECT_NEAR(eigen_global_laser_point(1), global_laser_point.y, 1.0e-6); 73 | } 74 | } 75 | } 76 | } 77 | } 78 | 79 | 80 | TEST_F(Pose2DTest, GlobalCoord2LocalCoordTest) { 81 | for (double pose_x = -max_distance_; pose_x <= max_distance_; pose_x += delta_distance_) { 82 | for (double pose_y = -max_distance_; pose_y <= max_distance_; pose_y += delta_distance_) { 83 | for (double pose_yaw = -max_angle_; pose_yaw <= max_angle_; pose_yaw += delta_angle_) { 84 | Pose2D pose(pose_x, pose_y, pose_yaw); 85 | 86 | Eigen::Affine2f eigen_affine_matrix = Eigen::Translation2f(pose_x, pose_y) 87 | * Eigen::Rotation2Df(pose_yaw); 88 | 89 | for (double local_laser_point_yaw = -max_angle_; local_laser_point_yaw <= max_angle_; local_laser_point_yaw += delta_angle_) { 90 | LaserPoint2D expect_local_laser_point; 91 | expect_local_laser_point.x = cos(local_laser_point_yaw); 92 | expect_local_laser_point.y = sin(local_laser_point_yaw); 93 | 94 | Eigen::Vector2f eigen_local_laser_point; 95 | eigen_local_laser_point << expect_local_laser_point.x, 96 | expect_local_laser_point.y; 97 | Eigen::Vector2f eigen_global_laser_point = eigen_affine_matrix * eigen_local_laser_point; 98 | 99 | LaserPoint2D global_laser_point; 100 | global_laser_point.x = eigen_global_laser_point(0); 101 | global_laser_point.y = eigen_global_laser_point(1); 102 | 103 | LaserPoint2D actual_local_laser_point; 104 | pose.GlobalCoord2LocalCoord(global_laser_point, actual_local_laser_point); 105 | 106 | EXPECT_NEAR(expect_local_laser_point.x, actual_local_laser_point.x, 1.0e-6); 107 | EXPECT_NEAR(expect_local_laser_point.y, actual_local_laser_point.y, 1.0e-6); 108 | } 109 | } 110 | } 111 | } 112 | } 113 | 114 | 115 | TEST_F(Pose2DTest, RotateNormalVectorTest) { 116 | for (double pose_x = -max_distance_; pose_x <= max_distance_; pose_x += delta_distance_) { 117 | for (double pose_y = -max_distance_; pose_y <= max_distance_; pose_y += delta_distance_) { 118 | for (double pose_yaw = -max_angle_; pose_yaw <= max_angle_; pose_yaw += delta_angle_) { 119 | Pose2D pose(pose_x, pose_y, pose_yaw); 120 | 121 | Eigen::Affine2f eigen_rotation_matrix; 122 | eigen_rotation_matrix = Eigen::Rotation2Df(pose_yaw); 123 | 124 | for (double local_laser_point_yaw = -max_angle_; local_laser_point_yaw <= max_angle_; local_laser_point_yaw += delta_angle_) { 125 | LaserPoint2D local_laser_point; 126 | local_laser_point.x = cos(local_laser_point_yaw); 127 | local_laser_point.y = sin(local_laser_point_yaw); 128 | local_laser_point.normal_x = cos(local_laser_point_yaw + 0.5*M_PI); 129 | local_laser_point.normal_y = sin(local_laser_point_yaw + 0.5*M_PI); 130 | 131 | LaserPoint2D global_laser_point; 132 | pose.RotateNormalVector(local_laser_point, global_laser_point); 133 | 134 | Eigen::Vector2f eigen_local_laser_point_normal_vector; 135 | eigen_local_laser_point_normal_vector << local_laser_point.normal_x, 136 | local_laser_point.normal_y; 137 | Eigen::Vector2f eigen_global_laser_point_normal_vector = eigen_rotation_matrix 138 | * eigen_local_laser_point_normal_vector; 139 | 140 | EXPECT_NEAR(eigen_global_laser_point_normal_vector(0), global_laser_point.normal_x, 1.0e-6); 141 | EXPECT_NEAR(eigen_global_laser_point_normal_vector(1), global_laser_point.normal_y, 1.0e-6); 142 | } 143 | } 144 | } 145 | } 146 | } 147 | 148 | 149 | TEST_F(Pose2DTest, InverseRotateNormalVectorTest) { 150 | for (double pose_x = -max_distance_; pose_x <= max_distance_; pose_x += delta_distance_) { 151 | for (double pose_y = -max_distance_; pose_y <= max_distance_; pose_y += delta_distance_) { 152 | for (double pose_yaw = -max_angle_; pose_yaw <= max_angle_; pose_yaw += delta_angle_) { 153 | Pose2D pose(pose_x, pose_y, pose_yaw); 154 | 155 | Eigen::Affine2f eigen_rotation_matrix; 156 | eigen_rotation_matrix = Eigen::Rotation2Df(pose_yaw); 157 | 158 | for (double local_laser_point_yaw = -max_angle_; local_laser_point_yaw <= max_angle_; local_laser_point_yaw += delta_angle_) { 159 | LaserPoint2D expect_local_laser_point; 160 | expect_local_laser_point.x = cos(local_laser_point_yaw); 161 | expect_local_laser_point.y = sin(local_laser_point_yaw); 162 | expect_local_laser_point.normal_x = cos(local_laser_point_yaw + 0.5*M_PI); 163 | expect_local_laser_point.normal_y = sin(local_laser_point_yaw + 0.5*M_PI); 164 | 165 | Eigen::Vector2f eigen_local_laser_point_normal_vector; 166 | eigen_local_laser_point_normal_vector << expect_local_laser_point.normal_x, 167 | expect_local_laser_point.normal_y; 168 | Eigen::Vector2f eigen_global_laser_point_normal_vector = eigen_rotation_matrix 169 | * eigen_local_laser_point_normal_vector; 170 | 171 | LaserPoint2D global_laser_point; 172 | global_laser_point.normal_x = eigen_global_laser_point_normal_vector(0); 173 | global_laser_point.normal_y = eigen_global_laser_point_normal_vector(1); 174 | 175 | LaserPoint2D actual_local_laser_point; 176 | pose.InverseRotateNormalVector(global_laser_point, actual_local_laser_point); 177 | 178 | EXPECT_NEAR(expect_local_laser_point.normal_x, actual_local_laser_point.normal_x, 1.0e-6); 179 | EXPECT_NEAR(expect_local_laser_point.normal_y, actual_local_laser_point.normal_y, 1.0e-6); 180 | } 181 | } 182 | } 183 | } 184 | } 185 | 186 | 187 | TEST_F(Pose2DTest, OperatorPlusTest) { 188 | std::vector absolute_poses; 189 | std::vector addition_relative_poses; 190 | for (double pose_x = -max_distance_; pose_x <= max_distance_; pose_x += delta_distance_) { 191 | for (double pose_y = -max_distance_; pose_y <= max_distance_; pose_y += delta_distance_) { 192 | for (double pose_yaw = -max_angle_; pose_yaw <= max_angle_; pose_yaw += delta_angle_) { 193 | Pose2D pose(pose_x, pose_y, pose_yaw); 194 | absolute_poses.emplace_back(pose); 195 | addition_relative_poses.emplace_back(pose); 196 | } 197 | } 198 | } 199 | 200 | for (std::size_t i = 0; i < absolute_poses.size(); ++i) { 201 | const Pose2D& absolute_pose_i = absolute_poses[i]; 202 | 203 | Eigen::Affine2f eigen_affine_matrix_i = Eigen::Translation2f(absolute_pose_i.x, absolute_pose_i.y) 204 | * Eigen::Rotation2Df(absolute_pose_i.yaw); 205 | 206 | for (std::size_t j = 0; j < addition_relative_poses.size(); ++j) { 207 | const Pose2D& addition_relative_pose_j = addition_relative_poses[j]; 208 | Pose2D absolute_pose_i_plus_j = absolute_pose_i + addition_relative_pose_j; 209 | 210 | Eigen::Vector2f eigen_addition_relative_pose_j; 211 | eigen_addition_relative_pose_j << addition_relative_pose_j.x, 212 | addition_relative_pose_j.y; 213 | Eigen::Vector2f eigen_absolute_pose_i_plus_j = eigen_affine_matrix_i * eigen_addition_relative_pose_j; 214 | 215 | EXPECT_NEAR(eigen_absolute_pose_i_plus_j(0), absolute_pose_i_plus_j.x, 1.0e-6); 216 | EXPECT_NEAR(eigen_absolute_pose_i_plus_j(1), absolute_pose_i_plus_j.y, 1.0e-6); 217 | } 218 | } 219 | } 220 | 221 | 222 | TEST_F(Pose2DTest, OperatorMinusTest) { 223 | std::vector absolute_poses; 224 | std::vector subtraction_absolute_poses; 225 | for (double pose_x = -max_distance_; pose_x <= max_distance_; pose_x += delta_distance_) { 226 | for (double pose_y = -max_distance_; pose_y <= max_distance_; pose_y += delta_distance_) { 227 | for (double pose_yaw = -max_angle_; pose_yaw <= max_angle_; pose_yaw += delta_angle_) { 228 | Pose2D pose(pose_x, pose_y, pose_yaw); 229 | absolute_poses.emplace_back(pose); 230 | subtraction_absolute_poses.emplace_back(pose); 231 | } 232 | } 233 | } 234 | 235 | for (std::size_t i = 0; i < absolute_poses.size(); ++i) { 236 | const Pose2D& absolute_pose_i = absolute_poses[i]; 237 | 238 | Eigen::Vector2f eigen_absolute_pose_i; 239 | eigen_absolute_pose_i << absolute_pose_i.x, 240 | absolute_pose_i.y; 241 | 242 | for (std::size_t j = 0; j < subtraction_absolute_poses.size(); ++j) { 243 | const Pose2D& subtraction_absolute_pose_j = subtraction_absolute_poses[j]; 244 | Pose2D relative_pose_i_minus_j = absolute_pose_i - subtraction_absolute_pose_j; 245 | 246 | Eigen::Affine2f eigen_inverse_translation_matrix_j; 247 | eigen_inverse_translation_matrix_j = Eigen::Translation2f(-subtraction_absolute_pose_j.x, 248 | -subtraction_absolute_pose_j.y); 249 | 250 | Eigen::Affine2f eigen_inverse_rotation_matrix_j; 251 | eigen_inverse_rotation_matrix_j = Eigen::Rotation2Df(-subtraction_absolute_pose_j.yaw); 252 | 253 | Eigen::Vector2f eigen_relative_pose_i_minus_j = 254 | eigen_inverse_rotation_matrix_j * (eigen_inverse_translation_matrix_j * eigen_absolute_pose_i); 255 | 256 | EXPECT_NEAR(eigen_relative_pose_i_minus_j(0), relative_pose_i_minus_j.x, 1.0e-6); 257 | EXPECT_NEAR(eigen_relative_pose_i_minus_j(1), relative_pose_i_minus_j.y, 1.0e-6); 258 | } 259 | } 260 | } 261 | 262 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/utility/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | project(utility_lib 4 | VERSION 1.0.0 5 | DESCRIPTION "Utility library" 6 | LANGUAGES CXX 7 | ) 8 | 9 | find_package(Eigen3) 10 | if(NOT EIGEN3_INCLUDE_DIR) 11 | set(EIGEN3_INCLUDE_DIR $ENV{EIGEN3_ROOT_DIR}) 12 | endif() 13 | message(STATUS " EIGEN3_INCLUDE_DIR = ${EIGEN3_INCLUDE_DIR}") 14 | 15 | add_library(utility STATIC 16 | ${PROJECT_SOURCE_DIR}/MyUtility/MyUtility.cpp 17 | ) 18 | target_compile_features(utility PRIVATE cxx_std_11) 19 | 20 | target_include_directories(utility PUBLIC 21 | ${EIGEN3_INCLUDE_DIR}/ 22 | ${PROJECT_SOURCE_DIR}/ 23 | ${PROJECT_SOURCE_DIR}/MyUtility/ 24 | ) -------------------------------------------------------------------------------- /sample_code_cpp/src/utility/GridTable2D.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILITY_GRIDTABLE2D_H_ 2 | #define UTILITY_GRIDTABLE2D_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | // utility/ 10 | #include "LaserPoint2D.h" 11 | #include "Pose2D.h" 12 | 13 | 14 | namespace sample_slam { 15 | 16 | struct GridTable2D { 17 | static constexpr double HALF_TABLE_LENGTH = 40.0; // [m] 18 | static constexpr double CELL_RESOLUTION = 0.05; // [m/px] 19 | static constexpr double NEAREST_RANGE = 0.2; // [m] 20 | 21 | int half_num_cell; // [px] 22 | int num_cell; // [px] 23 | std::vector> table; 24 | 25 | GridTable2D() { 26 | half_num_cell = static_cast(HALF_TABLE_LENGTH / CELL_RESOLUTION); 27 | num_cell = 2 * half_num_cell + 1; 28 | table.resize(num_cell * num_cell); 29 | clear(); 30 | } 31 | 32 | ~GridTable2D() {} 33 | 34 | void clear() { 35 | for (std::size_t idx = 0; idx < table.size(); ++idx) { 36 | if (0 < table[idx].size()) { 37 | table[idx].clear(); 38 | } 39 | } 40 | 41 | return; 42 | } 43 | 44 | void AddLaserPoint(const LaserPoint2D* laser_point_ptr) { 45 | int x_i = static_cast(laser_point_ptr->x / CELL_RESOLUTION) + half_num_cell; 46 | if (x_i < 0 || num_cell <= x_i) { 47 | return; 48 | } 49 | 50 | int y_i = static_cast(laser_point_ptr->y / CELL_RESOLUTION) + half_num_cell; 51 | if (y_i < 0 || num_cell <= y_i) { 52 | return; 53 | } 54 | 55 | int idx = y_i * num_cell + x_i; 56 | table[idx].emplace_back(laser_point_ptr); 57 | 58 | return; 59 | } 60 | 61 | void AddLaserPoint(const LaserPoint2D& laser_point) { 62 | int x_i = static_cast(laser_point.x / CELL_RESOLUTION) + half_num_cell; 63 | if (x_i < 0 || num_cell <= x_i) { 64 | return; 65 | } 66 | 67 | int y_i = static_cast(laser_point.y / CELL_RESOLUTION) + half_num_cell; 68 | if (y_i < 0 || num_cell <= y_i) { 69 | return; 70 | } 71 | 72 | int idx = y_i * num_cell + x_i; 73 | table[idx].emplace_back(&laser_point); 74 | 75 | return; 76 | } 77 | 78 | void MakeCellPoints(int num_point_threshold, std::vector& map) { 79 | for (std::size_t idx = 0; idx < table.size(); ++idx) { 80 | if (table[idx].size() < num_point_threshold) { 81 | continue; 82 | } 83 | 84 | int latest_id = -1; 85 | double centroid_x = 0.0; 86 | double centroid_y = 0.0; 87 | double mean_normal_x = 0.0; 88 | double mean_normal_y = 0.0; 89 | 90 | for (std::size_t i = 0; i < table[idx].size(); ++i) { 91 | if (latest_id < table[idx][i]->id) { 92 | latest_id = table[idx][i]->id; 93 | } 94 | centroid_x += table[idx][i]->x; 95 | centroid_y += table[idx][i]->y; 96 | mean_normal_x += table[idx][i]->normal_x; 97 | mean_normal_y += table[idx][i]->normal_y; 98 | } 99 | 100 | centroid_x /= table[idx].size(); 101 | centroid_y /= table[idx].size(); 102 | mean_normal_x /= hypot(mean_normal_x, mean_normal_y); 103 | mean_normal_y /= hypot(mean_normal_x, mean_normal_y); 104 | 105 | LaserPoint2D cell_laser_point; 106 | cell_laser_point.id = latest_id; 107 | cell_laser_point.x = centroid_x; 108 | cell_laser_point.y = centroid_y; 109 | cell_laser_point.normal_x = mean_normal_x; 110 | cell_laser_point.normal_y = mean_normal_y; 111 | cell_laser_point.point_type = PointType::LINE; 112 | 113 | map.emplace_back(cell_laser_point); 114 | } 115 | 116 | return; 117 | } 118 | 119 | const LaserPoint2D* FindNearestLaserPoint(const LaserPoint2D& curr_laser_point){ 120 | int curr_x_i = static_cast(curr_laser_point.x / CELL_RESOLUTION) + half_num_cell; 121 | if (curr_x_i < 0 || num_cell <= curr_x_i) { 122 | return nullptr; 123 | } 124 | 125 | int curr_y_i = static_cast(curr_laser_point.y / CELL_RESOLUTION) + half_num_cell; 126 | if (curr_y_i < 0 || num_cell <= curr_y_i) { 127 | return nullptr; 128 | } 129 | 130 | const LaserPoint2D* nearest_laser_point_ptr = nullptr; 131 | double nearest_distance = __DBL_MAX__; // [m] 132 | int range = static_cast(NEAREST_RANGE / CELL_RESOLUTION); // [px] 133 | 134 | for (int ny = -range; ny <= range; ++ny) { 135 | int y_i = curr_y_i + ny; 136 | if (y_i < 0 || num_cell <= y_i) { 137 | continue; 138 | } 139 | 140 | for (int nx = -range; nx <= range; ++nx) { 141 | int x_i = curr_x_i + nx; 142 | if (x_i < 0 || num_cell <= x_i) { 143 | continue; 144 | } 145 | 146 | int idx = y_i * num_cell + x_i; 147 | for(std::size_t i = 0; i < table[idx].size(); ++i){ 148 | const LaserPoint2D* reference_laser_point_ptr = table[idx][i]; 149 | 150 | double dx = reference_laser_point_ptr->x - curr_laser_point.x; 151 | double dy = reference_laser_point_ptr->y - curr_laser_point.y; 152 | double distance = hypot(dx, dy); 153 | 154 | if (distance < nearest_distance && distance < NEAREST_RANGE) { 155 | nearest_laser_point_ptr = reference_laser_point_ptr; 156 | nearest_distance = distance; 157 | } 158 | } 159 | } 160 | } 161 | 162 | return nearest_laser_point_ptr; 163 | } 164 | }; 165 | 166 | } // namespace sample_slam 167 | 168 | 169 | #endif // UTILITY_LASERPOINT2D_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/utility/LaserPoint2D.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILITY_LASERPOINT2D_H_ 2 | #define UTILITY_LASERPOINT2D_H_ 3 | 4 | 5 | #include 6 | 7 | // utility/ 8 | #include "MyUtility.h" 9 | 10 | 11 | namespace sample_slam { 12 | 13 | enum PointType { 14 | UNKNOWN = 0, 15 | LINE = 1, 16 | CORNER = 2, 17 | ISOLATE = 3 18 | }; 19 | 20 | 21 | struct LaserPoint2D { 22 | int id; 23 | double x, y; 24 | double normal_x, normal_y; // Normal vector of the laser point 25 | PointType point_type; 26 | 27 | LaserPoint2D() 28 | : id(-1), x(0.0), y(0.0), normal_x(0.0), normal_y(0.0), point_type(UNKNOWN) {} 29 | 30 | LaserPoint2D(int _id, double _x, double _y) 31 | : id(_id), x(_x), y(_y), normal_x(0.0), normal_y(0.0), point_type(UNKNOWN) {} 32 | 33 | LaserPoint2D(int _id, double _x, double _y, double _normal_x, double _normal_y) 34 | : id(_id), x(_x), y(_y), normal_x(_normal_x), normal_y(_normal_y), point_type(UNKNOWN) {} 35 | 36 | LaserPoint2D(int _id, double _x, double _y, double _normal_x, double _normal_y, PointType _point_type) 37 | : id(_id), x(_x), y(_y), normal_x(_normal_x), normal_y(_normal_y), point_type(_point_type) {} 38 | 39 | void RangeYaw2XY(double range, double yaw) { 40 | x = range * cos(yaw); 41 | y = range * sin(yaw); 42 | } 43 | }; 44 | 45 | } // namespace sample_slam 46 | 47 | 48 | #endif // UTILITY_LASERPOINT2D_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/utility/MyUtility/MyUtility.cpp: -------------------------------------------------------------------------------- 1 | #include "MyUtility.h" 2 | 3 | #include 4 | 5 | 6 | namespace sample_slam { 7 | 8 | double MyUtility::CastRadian(double radian) { 9 | while (radian < -M_PI) { 10 | radian += 2.0*M_PI; 11 | } 12 | while (M_PI < radian) { 13 | radian -= 2.0*M_PI; 14 | } 15 | 16 | return radian; 17 | } 18 | 19 | 20 | double MyUtility::AddRadian(double radian1, double radian2) { 21 | double radian = radian1 + radian2; 22 | radian = CastRadian(radian); 23 | 24 | return radian; 25 | } 26 | 27 | 28 | double MyUtility::SubtractRadian(double radian1, double radian2) { 29 | double radian = radian1 - radian2; 30 | radian = CastRadian(radian); 31 | 32 | return radian; 33 | } 34 | 35 | 36 | void MyUtility::InverseMatrixSVD(const Eigen::Matrix3f& matrix, Eigen::Matrix3f& inverse_matrix) { 37 | Eigen::JacobiSVD jacobi_svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV); 38 | Eigen::MatrixXf matrix_u = jacobi_svd.matrixU(); 39 | Eigen::MatrixXf matrix_v = jacobi_svd.matrixV(); 40 | Eigen::VectorXf singular_values = jacobi_svd.singularValues(); 41 | 42 | std::size_t dimension = 3; 43 | 44 | Eigen::Matrix3f M1; 45 | for (std::size_t i = 0; i < dimension; ++i) { 46 | for (std::size_t j = 0; j < dimension; ++j) { 47 | M1(i, j) = matrix_u(j, i) / singular_values(i); 48 | } 49 | } 50 | 51 | for (std::size_t i = 0; i < dimension; ++i) { 52 | for (std::size_t j = 0; j < dimension; ++j) { 53 | inverse_matrix(i, j) = 0.0; 54 | for (std::size_t k = 0; k < dimension; ++k) { 55 | inverse_matrix(i, j) += matrix_v(i, k) * M1(k, j); 56 | } 57 | } 58 | } 59 | 60 | return; 61 | } 62 | 63 | } // namespace sample_slam -------------------------------------------------------------------------------- /sample_code_cpp/src/utility/MyUtility/MyUtility.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILITY_MYUTILITY_H_ 2 | #define UTILITY_MYUTILITY_H_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | 9 | 10 | namespace sample_slam { 11 | 12 | #define DEG2RAD(theta) (theta * M_PI / 180.0) 13 | #define RAD2DEG(theta) (theta * 180.0 / M_PI) 14 | 15 | 16 | class MyUtility { 17 | public: 18 | MyUtility() {} 19 | 20 | ~MyUtility() {} 21 | 22 | static double CastRadian(double radian); 23 | 24 | static double AddRadian(double radian1, double radian2); 25 | 26 | static double SubtractRadian(double radian1, double radian2); 27 | 28 | static void InverseMatrixSVD(const Eigen::Matrix3f& matrix, Eigen::Matrix3f& inverse_matrix); 29 | }; 30 | 31 | } // namespace sample_slam 32 | 33 | 34 | #endif // UTILITY_MYUTILITY_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/utility/Pose2D.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILITY_POSE2D_H_ 2 | #define UTILITY_POSE2D_H_ 3 | 4 | 5 | #include 6 | 7 | // utility/ 8 | #include "LaserPoint2D.h" 9 | #include "MyUtility.h" 10 | 11 | 12 | namespace sample_slam { 13 | 14 | struct Pose2D { 15 | double x, y, yaw; // [m], [m], [rad] 16 | double rotation_matrix[2][2]; 17 | 18 | Pose2D() : x(0.0), y(0.0), yaw(0.0) { 19 | rotation_matrix[0][0] = 1.0; rotation_matrix[0][1] = 0.0; 20 | rotation_matrix[1][0] = 0.0; rotation_matrix[1][1] = 1.0; 21 | } 22 | 23 | Pose2D(double _x, double _y, double _yaw) : x(_x), y(_y), yaw(MyUtility::CastRadian(_yaw)) { 24 | CalcRotationMatrix(); 25 | } 26 | 27 | Pose2D(const Pose2D& other) : x(other.x), y(other.y), yaw(MyUtility::CastRadian(other.yaw)) { 28 | CalcRotationMatrix(); 29 | } 30 | 31 | void CalcRotationMatrix() { 32 | yaw = MyUtility::CastRadian(yaw); 33 | double cos_yaw = cos(yaw); 34 | double sin_yaw = sin(yaw); 35 | rotation_matrix[0][0] = cos_yaw; rotation_matrix[0][1] = -sin_yaw; 36 | rotation_matrix[1][0] = sin_yaw; rotation_matrix[1][1] = cos_yaw; 37 | return; 38 | } 39 | 40 | // x' = R*x + t 41 | void LocalCoord2GlobalCoord(const LaserPoint2D& local_laser_point, LaserPoint2D& global_laser_point) const { 42 | global_laser_point.x = rotation_matrix[0][0]*local_laser_point.x + rotation_matrix[0][1]*local_laser_point.y; 43 | global_laser_point.y = rotation_matrix[1][0]*local_laser_point.x + rotation_matrix[1][1]*local_laser_point.y; 44 | 45 | global_laser_point.x += x; 46 | global_laser_point.y += y; 47 | global_laser_point.id = local_laser_point.id; 48 | return; 49 | } 50 | 51 | // x = R^{-1}*(x' - t) 52 | void GlobalCoord2LocalCoord(const LaserPoint2D& global_laser_point, LaserPoint2D& local_laser_point) const { 53 | double dx = global_laser_point.x - x; 54 | double dy = global_laser_point.y - y; 55 | 56 | double inverse_rotation_matrix[2][2]; 57 | inverse_rotation_matrix[0][0] = rotation_matrix[0][0]; inverse_rotation_matrix[0][1] = rotation_matrix[1][0]; 58 | inverse_rotation_matrix[1][0] = rotation_matrix[0][1]; inverse_rotation_matrix[1][1] = rotation_matrix[1][1]; 59 | 60 | local_laser_point.x = inverse_rotation_matrix[0][0]*dx + inverse_rotation_matrix[0][1]*dy; 61 | local_laser_point.y = inverse_rotation_matrix[1][0]*dx + inverse_rotation_matrix[1][1]*dy; 62 | local_laser_point.id = global_laser_point.id; 63 | return; 64 | } 65 | 66 | // n' = R*n 67 | // local coord ---> global coord 68 | void RotateNormalVector(const LaserPoint2D& laser_point, LaserPoint2D& rotated_laser_point) const { 69 | rotated_laser_point.normal_x = rotation_matrix[0][0]*laser_point.normal_x + rotation_matrix[0][1]*laser_point.normal_y; 70 | rotated_laser_point.normal_y = rotation_matrix[1][0]*laser_point.normal_x + rotation_matrix[1][1]*laser_point.normal_y; 71 | return; 72 | } 73 | 74 | // n = R^{-1}*n' 75 | // global coord ---> local coord 76 | void InverseRotateNormalVector(const LaserPoint2D& laser_point, LaserPoint2D& inverse_rotated_laser_point) const { 77 | double inverse_rotation_matrix[2][2]; 78 | inverse_rotation_matrix[0][0] = rotation_matrix[0][0]; inverse_rotation_matrix[0][1] = rotation_matrix[1][0]; 79 | inverse_rotation_matrix[1][0] = rotation_matrix[0][1]; inverse_rotation_matrix[1][1] = rotation_matrix[1][1]; 80 | 81 | inverse_rotated_laser_point.normal_x = inverse_rotation_matrix[0][0]*laser_point.normal_x + inverse_rotation_matrix[0][1]*laser_point.normal_y; 82 | inverse_rotated_laser_point.normal_y = inverse_rotation_matrix[1][0]*laser_point.normal_x + inverse_rotation_matrix[1][1]*laser_point.normal_y; 83 | return; 84 | } 85 | 86 | // Compounding operator (+) 87 | Pose2D operator+(const Pose2D& addition_relative) const { 88 | Pose2D result_absolute; 89 | result_absolute.x = x + rotation_matrix[0][0]*addition_relative.x + rotation_matrix[0][1]*addition_relative.y; 90 | result_absolute.y = y + rotation_matrix[1][0]*addition_relative.x + rotation_matrix[1][1]*addition_relative.y; 91 | result_absolute.yaw = MyUtility::AddRadian(yaw, addition_relative.yaw); 92 | result_absolute.CalcRotationMatrix(); 93 | 94 | return result_absolute; 95 | } 96 | 97 | // Inverse compounding operator (-) 98 | // http://mrpt.ual.es/reference/0.9.1/classmrpt_1_1poses_1_1_c_pose.html#a007ab7e1ee2b722733f1bd52e6c89b3a 99 | Pose2D operator-(const Pose2D& subtraction_absolute) const { 100 | double dx = x - subtraction_absolute.x; 101 | double dy = y - subtraction_absolute.y; 102 | 103 | double inverse_rotation_matrix[2][2]; 104 | inverse_rotation_matrix[0][0] = subtraction_absolute.rotation_matrix[0][0]; inverse_rotation_matrix[0][1] = subtraction_absolute.rotation_matrix[1][0]; 105 | inverse_rotation_matrix[1][0] = subtraction_absolute.rotation_matrix[0][1]; inverse_rotation_matrix[1][1] = subtraction_absolute.rotation_matrix[1][1]; 106 | 107 | Pose2D result_relative; 108 | result_relative.x = inverse_rotation_matrix[0][0]*dx + inverse_rotation_matrix[0][1]*dy; 109 | result_relative.y = inverse_rotation_matrix[1][0]*dx + inverse_rotation_matrix[1][1]*dy; 110 | result_relative.yaw = MyUtility::SubtractRadian(yaw, subtraction_absolute.yaw); 111 | result_relative.CalcRotationMatrix(); 112 | 113 | return result_relative; 114 | } 115 | }; 116 | 117 | } // namespace sample_slam 118 | 119 | 120 | #endif // UTILITY_POSE2D_H_ -------------------------------------------------------------------------------- /sample_code_cpp/src/utility/Scan2D.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILITY_SCAN2D_H_ 2 | #define UTILITY_SCAN2D_H_ 3 | 4 | 5 | #include 6 | 7 | // utility/ 8 | #include "LaserPoint2D.h" 9 | #include "Pose2D.h" 10 | 11 | 12 | namespace sample_slam { 13 | 14 | struct Scan2D { 15 | static constexpr double MIN_SCAN_RANGE = 0.1; 16 | static constexpr double MAX_SCAN_RANGE = 6.0; 17 | 18 | int id; 19 | Pose2D pose; 20 | std::vector laser_points; 21 | 22 | Scan2D() : id(-1) {} 23 | 24 | Scan2D(int _id) : id(_id) {} 25 | }; 26 | 27 | } // namespace sample_slam 28 | 29 | 30 | #endif // UTILITY_SCAN2D_H_ -------------------------------------------------------------------------------- /sample_code_python/images/graph_based_slam1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_python/images/graph_based_slam1.png -------------------------------------------------------------------------------- /sample_code_python/images/graph_based_slam2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_python/images/graph_based_slam2.png -------------------------------------------------------------------------------- /sample_code_python/images/graph_based_slam3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/sample_code_python/images/graph_based_slam3.png -------------------------------------------------------------------------------- /tex/Graph-Based-SLAM_english.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/Graph-Based-SLAM_english.pdf -------------------------------------------------------------------------------- /tex/Graph-Based-SLAM_japanese.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/Graph-Based-SLAM_japanese.pdf -------------------------------------------------------------------------------- /tex/images/1-1_error_between_edges.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/1-1_error_between_edges.png -------------------------------------------------------------------------------- /tex/images/1-1_error_between_edges.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/1-1_error_between_edges.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 563 376 4 | %%HiResBoundingBox: 0.000000 0.000000 563.000000 376.000000 5 | %%CreationDate: Sat Jun 16 11:41:26 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/2-1_coordinate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/2-1_coordinate.png -------------------------------------------------------------------------------- /tex/images/2-1_coordinate.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/2-1_coordinate.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 960 540 4 | %%HiResBoundingBox: 0.000000 0.000000 960.000000 540.000000 5 | %%CreationDate: Sat Jun 16 11:41:26 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/2-2_rotation_matrix.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/2-2_rotation_matrix.png -------------------------------------------------------------------------------- /tex/images/2-2_rotation_matrix.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/2-2_rotation_matrix.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 960 540 4 | %%HiResBoundingBox: 0.000000 0.000000 960.000000 540.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/2-3_pose_representation_matrix.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/2-3_pose_representation_matrix.png -------------------------------------------------------------------------------- /tex/images/2-3_pose_representation_matrix.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/2-3_pose_representation_matrix.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 817 466 4 | %%HiResBoundingBox: 0.000000 0.000000 817.000000 466.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/2-4_pose_representation_matrix_inverse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/2-4_pose_representation_matrix_inverse.png -------------------------------------------------------------------------------- /tex/images/2-4_pose_representation_matrix_inverse.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/2-4_pose_representation_matrix_inverse.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 832 434 4 | %%HiResBoundingBox: 0.000000 0.000000 832.000000 434.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/2-5_observation_representation_matrix.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/2-5_observation_representation_matrix.png -------------------------------------------------------------------------------- /tex/images/2-5_observation_representation_matrix.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/2-5_observation_representation_matrix.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 960 540 4 | %%HiResBoundingBox: 0.000000 0.000000 960.000000 540.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/2-6_rotation_matrix.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/2-6_rotation_matrix.png -------------------------------------------------------------------------------- /tex/images/2-6_rotation_matrix.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/2-6_rotation_matrix.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 960 540 4 | %%HiResBoundingBox: 0.000000 0.000000 960.000000 540.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/2-7_sensor_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/2-7_sensor_model.png -------------------------------------------------------------------------------- /tex/images/2-7_sensor_model.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/2-7_sensor_model.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 960 540 4 | %%HiResBoundingBox: 0.000000 0.000000 960.000000 540.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/3-1_difference_between_nodes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/3-1_difference_between_nodes.png -------------------------------------------------------------------------------- /tex/images/3-1_difference_between_nodes.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/3-1_difference_between_nodes.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 960 540 4 | %%HiResBoundingBox: 0.000000 0.000000 960.000000 540.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/3-2_error_between_edges.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/3-2_error_between_edges.png -------------------------------------------------------------------------------- /tex/images/3-2_error_between_edges.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/3-2_error_between_edges.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 960 540 4 | %%HiResBoundingBox: 0.000000 0.000000 960.000000 540.000000 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/4-1_by_deleji_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/4-1_by_deleji_1.png -------------------------------------------------------------------------------- /tex/images/4-1_by_deleji_1.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/4-1_by_deleji_1.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 380 264 4 | %%HiResBoundingBox: 0.000000 0.000000 380.086575 264.060147 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/4-2_by_deleji_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/4-2_by_deleji_2.png -------------------------------------------------------------------------------- /tex/images/4-2_by_deleji_2.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/4-2_by_deleji_2.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 380 264 4 | %%HiResBoundingBox: 0.000000 0.000000 380.086575 264.060147 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/4-3_by_deleji_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/4-3_by_deleji_3.png -------------------------------------------------------------------------------- /tex/images/4-3_by_deleji_3.xbb: -------------------------------------------------------------------------------- 1 | %%Title: ./images/4-3_by_deleji_3.png 2 | %%Creator: extractbb 20130405 3 | %%BoundingBox: 0 0 380 264 4 | %%HiResBoundingBox: 0.000000 0.000000 380.086575 264.060147 5 | %%CreationDate: Sat Jun 16 11:41:27 2018 6 | 7 | -------------------------------------------------------------------------------- /tex/images/Graph-Based-SLAM_english-page-001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/Graph-Based-SLAM_english-page-001.jpg -------------------------------------------------------------------------------- /tex/images/Graph-Based-SLAM_english-page-003.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/Graph-Based-SLAM_english-page-003.jpg -------------------------------------------------------------------------------- /tex/images/Graph-Based-SLAM_english-page-005.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/Graph-Based-SLAM_english-page-005.jpg -------------------------------------------------------------------------------- /tex/images/Graph-Based-SLAM_english-page-007.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hotsuyuki/Graph-Based-SLAM/bd5d659095d15de2812078f7853c00425aefa7a8/tex/images/Graph-Based-SLAM_english-page-007.jpg --------------------------------------------------------------------------------