├── .clang-format ├── .github └── ISSUE_TEMPLATE │ └── bug_report.md ├── .gitignore ├── .pre-commit-config.yaml ├── 3rdparty ├── find_dependencies.cmake └── tbb │ ├── LICENSE │ └── tbb.cmake ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── ouster128.yaml ├── ouster64.yaml ├── velodyne16.yaml └── velodyne64.yaml ├── img ├── demo_kitti00_v2.gif ├── demo_terrain_v3.gif ├── kimera-multi.gif ├── kitti_activated.png ├── nonplanar_gpf.png ├── ouster128_right_elevation.png ├── ouster128_wrong_elevation.png ├── patchwork.gif ├── patchwork_concept_resized.jpg ├── precision_recall.png ├── profile_low.png ├── sensor_height_issue.png ├── seq_00_pr_zoom.pdf ├── wiki_gle1.png ├── wiki_gle2.png ├── wiki_gle3.png ├── wiki_gle4.png ├── wiki_gle_param_v2.png ├── wiki_intro1.png ├── wiki_intro2.png ├── wiki_intro3.png ├── wiki_intro4.png ├── wiki_intro5.png ├── wiki_method1.png ├── wiki_method2.png ├── wiki_method3.png ├── wiki_method4.png ├── wiki_overview.PNG └── wiki_test.png ├── include ├── label_generator │ ├── label_generator.hpp │ ├── nanoflann.hpp │ └── nanoflann_utils.hpp ├── patchwork │ ├── patchwork.hpp │ ├── sensor_configs.hpp │ ├── utils.hpp │ └── zone_models.hpp └── tools │ ├── kitti_loader.hpp │ ├── pcd_loader.hpp │ └── tictoc.hpp ├── launch ├── evaluate.launch.yaml ├── ros1 │ ├── kitti_publisher.launch │ ├── offline_ouster128.launch │ ├── pub_for_legoloam.launch │ ├── run_patchwork.launch │ └── run_patchwork_kimera_multi.launch └── run_patchwork.launch.yaml ├── materials ├── 000000.pcd ├── 000001.pcd └── 000002.pcd ├── msg ├── ground_estimate.msg └── node.msg ├── package.xml ├── rviz ├── acl_jackal2.rviz ├── patchwork.rviz └── semantickitti.rviz └── src ├── main.cpp ├── offline_own_data.cpp ├── pub_for_legoloam.cpp └── ros_kitti_publisher.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: All 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: false 21 | BinPackParameters: false 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | CommentPragmas: "^ IWYU pragma:" 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | ConstructorInitializerIndentWidth: 4 41 | ContinuationIndentWidth: 4 42 | Cpp11BracedListStyle: true 43 | DerivePointerAlignment: true 44 | DisableFormat: false 45 | ExperimentalAutoDetectBinPacking: false 46 | ForEachMacros: 47 | - foreach 48 | - Q_FOREACH 49 | - BOOST_FOREACH 50 | IncludeCategories: 51 | # Spacers 52 | - Regex: "^$" 53 | Priority: 15 54 | - Regex: "^$" 55 | Priority: 25 56 | - Regex: "^$" 57 | Priority: 35 58 | - Regex: "^$" 59 | Priority: 45 60 | # C system headers 61 | - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' 62 | Priority: 10 63 | # C++ system headers 64 | - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' 65 | Priority: 20 66 | # Other library h files (with angles) 67 | - Regex: "^<" 68 | Priority: 30 69 | # Your project's h files (with angles) 70 | - Regex: "^ 33 | ${ExternalProject_CMAKE_ARGS} 34 | ${ExternalProject_CMAKE_CXX_FLAGS} 35 | # custom flags 36 | -DTBB_BUILD_TBBMALLOC=ON 37 | -DTBB_BUILD_SHARED=OFF 38 | -DTBB_BUILD_STATIC=ON 39 | -DTBB_BUILD_TESTS=OFF) 40 | 41 | # Simulate importing TBB::tbb for OpenVDBHelper target 42 | ExternalProject_Get_Property(external_tbb INSTALL_DIR) 43 | set(TBB_ROOT ${INSTALL_DIR} CACHE INTERNAL "TBB_ROOT Install directory") 44 | add_library(TBBHelper INTERFACE) 45 | add_dependencies(TBBHelper external_tbb) 46 | target_include_directories(TBBHelper INTERFACE ${INSTALL_DIR}/include) 47 | target_link_directories(TBBHelper INTERFACE ${INSTALL_DIR}/lib) 48 | target_link_libraries(TBBHelper INTERFACE tbb Threads::Threads) 49 | add_library(TBB::tbb ALIAS TBBHelper) 50 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.24) 2 | project(patchwork) 3 | 4 | add_compile_options(-std=c++17) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | find_package(ament_cmake REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(rcutils REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(rclcpp_components REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | find_package(sensor_msgs REQUIRED) 19 | find_package(visualization_msgs REQUIRED) 20 | find_package(pcl_conversions REQUIRED) 21 | find_package(PCL REQUIRED) 22 | find_package(tf2_ros REQUIRED) 23 | find_package(tf2_eigen REQUIRED) 24 | find_package(tf2_geometry_msgs REQUIRED) 25 | find_package(tf2_sensor_msgs REQUIRED) 26 | find_package(TBB REQUIRED) 27 | 28 | include_directories( 29 | include 30 | src 31 | ${catkin_INCLUDE_DIRS} 32 | ${PCL_INCLUDE_DIRS} 33 | ) 34 | link_directories(${PCL_LIBRARY_DIRS}) 35 | add_definitions(${PCL_DEFINITIONS}) 36 | 37 | ################################################################################ 38 | add_executable(run_patchwork src/main.cpp) 39 | ament_target_dependencies(run_patchwork 40 | rclcpp 41 | tf2_ros 42 | sensor_msgs 43 | nav_msgs 44 | visualization_msgs 45 | pcl_conversions 46 | PCL 47 | ) 48 | target_link_libraries(run_patchwork 49 | ${PCL_LIBRARIES} 50 | TBB::tbb 51 | stdc++fs # To use std::filesystem 52 | ) 53 | 54 | install(TARGETS 55 | run_patchwork 56 | LIBRARY DESTINATION lib 57 | RUNTIME DESTINATION lib/${PROJECT_NAME} 58 | ) 59 | 60 | install(DIRECTORY launch config rviz 61 | DESTINATION share/${PROJECT_NAME} 62 | ) 63 | 64 | ament_package() 65 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Hyungtae Lim 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

Patchwork

3 | 4 | 5 | 6 | 7 | 8 |
9 |
10 | Video 11 |   •   12 | Install by ROS 13 |   •   14 | Paper 15 |   •   16 | Project Wiki (for beginners) 17 |
18 |
19 |
20 | animated 21 | animated 22 |
23 |
24 |
25 | 26 | --- 27 | 28 | **IMPORTANT**: (Aug. 18th, 2024) I employ TBB, so its FPS is increased from **50 Hz** to **100 Hz**! 29 | If you want to use the paper version of Patchwork for SOTA comparison purpose, Please use this [ground seg. benchmark code](https://github.com/url-kaist/Ground-Segmentation-Benchmark). 30 | 31 | 32 | Patchwork | Concept of our method (CZM & GLE) 33 | :-------------------------:|:-------------------------: 34 | ![](img/patchwork_concept_resized.jpg) | ![](img/patchwork.gif) 35 | 36 | It's an overall updated version of **R-GPF of ERASOR** [**[Code](https://github.com/LimHyungTae/ERASOR)**] [**[Paper](https://arxiv.org/abs/2103.04316)**]. 37 | 38 | --- 39 | 40 | 41 | ## :open_file_folder: Contents 42 | 0. [Test Env.](#Test-Env.) 43 | 0. [Requirements](#requirements) 44 | 0. [How to Run Patchwork](#How-to-Run-Patchwork) 45 | 0. [Citation](#citation) 46 | 47 | ### Test Env. 48 | 49 | The code is tested successfully at 50 | * Linux 24.04 LTS 51 | * ROS2 Jazzy 52 | 53 | ROS Noetic version can be found [here](https://github.com/LimHyungTae/patchwork/releases/tag/v0.2) 54 | 55 | ## :package: Prerequisite Installation 56 | 57 | ```bash 58 | mkdir -p ~/colcon/src 59 | cd ~/colcon/src 60 | git clone https://github.com/LimHyungTae/patchwork.git 61 | cd .. 62 | colcon build --packages-up-to patchwork --cmake-args -DCMAKE_BUILD_TYPE=Release 63 | ``` 64 | 65 | ## :gear: How to Run Patchwork 66 | 67 | #### :chart_with_upwards_trend: Offline KITTI dataset 68 | 69 | 1. Download [SemanticKITTI](http://www.semantic-kitti.org/dataset.html#download) Odometry dataset (We also need labels since we also open the evaluation code! :) 70 | 71 | 2. The `dataset_path` should consist of `velodyne` folder and `labels` folder as follows: 72 | 73 | ``` 74 | data_path (e.g. 00, 01, ..., or 10) 75 | _____velodyne 76 | |___000000.bin 77 | |___000001.bin 78 | |___000002.bin 79 | |... 80 | _____labels 81 | |___000000.label 82 | |___000001.label 83 | |___000002.label 84 | |... 85 | _____... 86 | 87 | ``` 88 | 89 | 3. Run launch file 90 | 91 | ``` 92 | ros2 launch patchwork evaluate.launch.yaml evaluate_semantickitti:=true dataset_path:=" 93 | e.g., 94 | ros2 launch patchwork evaluate.launch.yaml evaluate_semantickitti:=true dataset_path:="/home/hyungtae_lim/semkitti/dataset/sequences/04" 95 | ``` 96 | 97 | 98 | #### :runner: Online Ground Segmentation 99 | 100 | ``` 101 | ros2 launch patchwork run_patchwork.launch.yaml scan_topic:= sensor_type:= 102 | e.g., 103 | ros2 launch patchwork run_patchwork.launch.yaml scan_topic:="/acl_jackal2/lidar_points" sensor_type:="velodyne16" 104 | ``` 105 | 106 | 107 | For better understanding of the parameters of Patchwork, please read [our wiki, 4. IMPORTANT: Setting Parameters of Patchwork in Your Own Env.](https://github.com/LimHyungTae/patchwork/wiki/4.-IMPORTANT:-Setting-Parameters-of-Patchwork-in-Your-Own-Env.). 108 | 109 | --------- 110 | 111 | ## Citation 112 | 113 | If you use our code or method in your work, please consider citing the following: 114 | 115 | @article{lim2021patchwork, 116 | title={Patchwork: Concentric Zone-based Region-wise Ground Segmentation with Ground Likelihood Estimation Using a 3D LiDAR Sensor}, 117 | author={Lim, Hyungtae and Minho, Oh and Myung, Hyun}, 118 | journal={IEEE Robotics and Automation Letters}, 119 | year={2021} 120 | } 121 | 122 | 123 | --- 124 | 125 | ## Updates 126 | 127 | #### NEWS (22.12.24) 128 | - Merry christmas eve XD! `include/label_generator` is added to make the `.label` file, following the SemanticKITTI format. 129 | - The `.label` files can be directly used in [3DUIS benchmark](https://github.com/PRBonn/3DUIS) 130 | - Thank [Lucas Nunes](https://scholar.google.com/citations?user=PCxhsf4AAAAJ&hl=en&oi=ao) and [Xieyuanli Chen](https://scholar.google.com/citations?user=DvrngV4AAAAJ&hl=en&oi=sra) for providing code snippets to save a `.label` file. 131 | 132 | #### NEWS (22.07.25) 133 | - Pybinding + more advanced version is now available on [Patchwork++](https://github.com/url-kaist/patchwork-plusplus) as a preprocessing step for deep learning users (i.e., python users can also use our robust ground segmentation)! 134 | 135 | #### NEWS (22.07.13) 136 | - For increasing convenience of use, the examples and codes are extensively revised by reflecting [issue #12](https://github.com/LimHyungTae/patchwork/issues/12). 137 | 138 | #### NEWS (22.05.22) 139 | - The meaning of `elevation_thresholds` is changed to increase the usability. The meaning is explained in [wiki](https://github.com/LimHyungTae/patchwork/wiki/4.-IMPORTANT:-Setting-Parameters-of-Patchwork-in-Your-Own-Env.). 140 | - A novel height estimator, called *All-Terrain Automatic heighT estimator (ATAT)* is added within the patchwork code, which auto-calibrates the sensor height using the ground points in the vicinity of the vehicle/mobile robot. 141 | - Please refer to the function `consensus_set_based_height_estimation()`. 142 | -------------------------------------------------------------------------------- /config/ouster128.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # 22.05.02 Update 4 | # `sensor_height` denotes the elevation of the origin of the point cloud "with respect to the ground" 5 | # So, it mostly have a positive value 6 | # Note that if `patchwork/ATAT/ATAT_ON` is set to be true, the sensor_height is auto-calibrated 7 | # Nevertheless, `sensor_height` should be set appropriately! 8 | sensor_height: 1.5 9 | save_flag: true 10 | sensor_model: "OS1-128" 11 | 12 | # Extrinsics (Raw lidar coordinate -> Coordinate that is parallel to the X-Y plane of ground) 13 | # But, not in use 14 | extrinsic_trans: [0.0, 0.0, 0.0] 15 | extrinsic_rot: [1, 0, 0, 16 | 0, 1, 0, 17 | 0, 0, 1] 18 | mode: "czm" 19 | # Ground Plane Fitting parameters 20 | num_iter: 3 21 | num_lpr: 20 22 | num_min_pts: 10 23 | th_seeds: 0.3 24 | th_dist: 0.125 25 | max_r: 80.0 26 | min_r: 2.7 # to consider vicinity of mobile plot form. 27 | uprightness_thr: 0.707 # For uprightness. 45: 0.707 / 60: 0.866. The larger, the more conservative 28 | 29 | # The points below the adaptive_seed_selection_margin * sensor_height are filtered 30 | # Tor rejecti points caused by reflection or multipath problems. 31 | # it should be lower than -1.0 32 | adaptive_seed_selection_margin: -1.1 33 | 34 | # It is not in the paper 35 | # It is also not matched our philosophy, but it is employed to reject some FPs easily & intuitively. 36 | # For patchwork, the global elevation threshold is only applied on Z3 and Z4 37 | using_global_elevation: false 38 | # W.r.t sensor frame (That is, if it is 0.0, then the candidates whose z is higher than z height of 3D LiDAR sensor are rejected 39 | global_elevation_threshold: -0.5 40 | 41 | # 22.05.02 Update 42 | # ATAT is the abbrev. for All-Terrain Automatic heighT estimator 43 | # It automatically corrects the wrong sensor height input 44 | ATAT: 45 | ATAT_ON: true 46 | # 22.05.02 Update 47 | # IMPORTANT - `max_r_for_ATAT` affects the quality of the estimation of sensor height 48 | # If it is too large, then the z-elevation values of the bins become more ambiguous 49 | # If it is too small, there is a potential risk that does not include the cloud points in the vicinity of the vehicles/mobile robots 50 | # Therefore, it should be set appropriately! 51 | max_r_for_ATAT: 5.0 52 | num_sectors_for_ATAT: 20 53 | noise_bound: 0.2 54 | 55 | czm: 56 | # 22.05.02 Update 57 | # For usability, the MEANING OF THE ELEVATION THRESHOLDS IS CHANGED!!!! 58 | # Original - the elevation w.r.t. to the sensor frame 59 | # Modified - the elevation w.r.t. to the ground 60 | # Thus, -sensor_height + elevation_threshold is the criteria. 61 | # E.g. for the first ring, -1.723 (sensor height in the KITTI) + 0.523 = -1.2 becomes the criteria of the elevation filter 62 | # That is, if the z-elevation of the bin is higher than -1.2, then the cloud points from the bin are rejected 63 | elevation_thresholds: [0.5, 0.8, 1.0, 1.1] # For elevation. The size should be equal to flatness_thresholds vector 64 | flatness_thresholds: [0.0, 0.000125, 0.000185, 0.000185] # For flatness. The size should be equal to elevation_thresholds vector 65 | -------------------------------------------------------------------------------- /config/ouster64.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # 22.05.02 Update 4 | # `sensor_height` denotes the elevation of the origin of the point cloud "with respect to the ground" 5 | # So, it mostly have a positive value 6 | # Note that if `patchwork/ATAT/ATAT_ON` is set to be true, the sensor_height is auto-calibrated 7 | # Nevertheless, `sensor_height` should be set appropriately! 8 | sensor_height: 0.7 9 | save_flag: false 10 | sensor_model: "OS1-64" 11 | 12 | # Extrinsics (Raw lidar coordinate -> Coordinate that is parallel to the X-Y plane of ground) 13 | # But, not in use 14 | extrinsic_trans: [0.0, 0.0, 0.0] 15 | extrinsic_rot: [1, 0, 0, 16 | 0, 1, 0, 17 | 0, 0, 1] 18 | mode: "czm" 19 | # Ground Plane Fitting parameters 20 | num_iter: 3 21 | num_lpr: 20 22 | num_min_pts: 10 23 | th_seeds: 0.3 24 | th_dist: 0.125 25 | max_r: 80.0 26 | min_r: 1.0 # to consider vicinity of mobile plot form. 27 | uprightness_thr: 0.707 # For uprightness. 45: 0.707 / 60: 0.866. The larger, the more conservative 28 | 29 | # The points below the adaptive_seed_selection_margin * sensor_height are filtered 30 | # Tor rejecti points caused by reflection or multipath problems. 31 | # it should be lower than -1.0 32 | adaptive_seed_selection_margin: -1.1 33 | 34 | # It is not in the paper 35 | # It is also not matched our philosophy, but it is employed to reject some FPs easily & intuitively. 36 | # For patchwork, the global elevation threshold is only applied on Z3 and Z4 37 | using_global_elevation: true 38 | # W.r.t sensor frame (That is, if it is 0.0, then the candidates whose z is higher than z height of 3D LiDAR sensor are rejected 39 | global_elevation_threshold: -0.25 40 | 41 | # 22.05.02 Update 42 | # ATAT is the abbrev. for All-Terrain Automatic heighT estimator 43 | # It automatically corrects the wrong sensor height input 44 | ATAT: 45 | ATAT_ON: true 46 | # 22.05.02 Update 47 | # IMPORTANT - `max_r_for_ATAT` affects the quality of the estimation of sensor height 48 | # If it is too large, then the z-elevation values of the bins become more ambiguous 49 | # If it is too small, there is a potential risk that does not include the cloud points in the vicinity of the vehicles/mobile robots 50 | # Therefore, it should be set appropriately! 51 | max_r_for_ATAT: 5.0 52 | num_sectors_for_ATAT: 20 53 | noise_bound: 0.2 54 | 55 | czm: 56 | # 22.05.02 Update 57 | # For usability, the MEANING OF THE ELEVATION THRESHOLDS IS CHANGED!!!! 58 | # Original - the elevation w.r.t. to the sensor frame 59 | # Modified - the elevation w.r.t. to the ground 60 | # Thus, -sensor_height + elevation_threshold is the criteria. 61 | # E.g. for the first ring, -1.723 (sensor height in the KITTI) + 0.523 = -1.2 becomes the criteria of the elevation filter 62 | # That is, if the z-elevation of the bin is higher than -1.2, then the cloud points from the bin are rejected 63 | elevation_thresholds: [0.175, 0.175, 0.20, 0.25] # For elevation. The size should be equal to flatness_thresholds vector 64 | flatness_thresholds: [0.0, 0.000125, 0.000185, 0.000185] # For flatness. The size should be equal to elevation_thresholds vector 65 | -------------------------------------------------------------------------------- /config/velodyne16.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # 22.05.02 Update 4 | # `sensor_height` denotes the elevation of the origin of the point cloud "with respect to the ground" 5 | # So, it mostly have a positive value 6 | # Note that if `patchwork/ATAT/ATAT_ON` is set to be true, the sensor_height is auto-calibrated 7 | # Nevertheless, `sensor_height` should be set appropriately! 8 | sensor_height: 0.6 9 | sensor_model: "VLP-16" 10 | # Extrinsics (Raw lidar coordinate -> Coordinate that is parallel to the X-Y plane of ground) 11 | # But, not in use 12 | extrinsic_trans: [0.0, 0.0, 0.0] 13 | extrinsic_rot: [1, 0, 0, 14 | 0, 1, 0, 15 | 0, 0, 1] 16 | 17 | mode: "czm" 18 | verbose: false # To check effect of uprightness/elevation/flatness 19 | visualize: true # Ground Likelihood Estimation is visualized 20 | # Ground Plane Fitting parameters 21 | num_iter: 3 22 | num_lpr: 20 23 | num_min_pts: 10 24 | th_seeds: 0.2 25 | th_dist: 0.075 26 | max_r: 60.0 27 | min_r: 0.6 # to consider vicinity of mobile plot form. 28 | uprightness_thr: 0.707 # For uprightness. 45: 0.707 / 60: 0.866. The larger, the more conservative 29 | 30 | # The points below the adaptive_seed_selection_margin * sensor_height are filtered 31 | # For reject points caused by reflection or multipath problems. 32 | # it should be lower than -1.0 33 | adaptive_seed_selection_margin: -1.1 34 | 35 | # It is not in the paper 36 | # It is also not matched our philosophy, but it is employed to reject some FPs easily & intuitively. 37 | # For patchwork, the global elevation threshold is only applied on Z3 and Z4 38 | # In Kimera-Multi, the environments are mostly flat, so it is used. 39 | using_global_elevation: true 40 | # W.r.t sensor frame (That is, if it is 0.0, then the candidates whose z is higher than z height of 3D LiDAR sensor are rejected 41 | global_elevation_threshold: 0.0 42 | 43 | # 22.05.02 Update 44 | # ATAT is the abbrev. for All-Terrain Automatic heighT estimator 45 | # It automatically corrects the wrong sensor height input 46 | ATAT: 47 | ATAT_ON: true 48 | # 22.05.02 Update 49 | # IMPORTANT - `max_r_for_ATAT` affects the quality of the estimation of sensor height 50 | # If it is too large, then the z-elevation values of the bins become more ambiguous 51 | # If it is too small, there is a potential risk that does not include the cloud points in the vicinity of the vehicles/mobile robots 52 | # Therefore, it should be set appropriately! 53 | max_r_for_ATAT: 5.0 54 | num_sectors_for_ATAT: 20 55 | noise_bound: 0.2 56 | 57 | czm: 58 | # 22.05.02 Update 59 | # For usability, the MEANING OF THE ELEVATION THRESHOLDS IS CHANGED!!!! 60 | # Original - the elevation w.r.t. to the sensor frame 61 | # Modified - the elevation w.r.t. to the ground 62 | # Thus, -sensor_height + elevation_threshold is the criteria. 63 | # E.g. for the first ring, -1.723 (sensor height in the KITTI) + 0.523 = -1.2 becomes the criteria of the elevation filter 64 | # That is, if the z-elevation of the bin is higher than -1.2, then the cloud points from the bin are rejected 65 | elevation_thresholds: [0.523, 0.746, 0.879, 1.125] # For elevation. The size should be equal to flatness_thresholds vector 66 | flatness_thresholds: [0.0005, 0.000725, 0.001, 0.001] # For flatness. The size should be equal to elevation_thresholds vector 67 | -------------------------------------------------------------------------------- /config/velodyne64.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # 22.05.02 Update 4 | # `sensor_height` denotes the elevation of the origin of the point cloud "with respect to the ground" 5 | # So, it mostly have a positive value 6 | # Note that if `patchwork/ATAT/ATAT_ON` is set to be true, the sensor_height is auto-calibrated 7 | sensor_height: 1.723 8 | sensor_model: "HDL-64E" 9 | # Extrinsics (Raw lidar coordinate -> Coordinate that is parallel to the X-Y plane of ground) 10 | # But, not in use 11 | extrinsic_trans: [0.0, 0.0, 0.0] 12 | extrinsic_rot: [1, 0, 0, 13 | 0, 1, 0, 14 | 0, 0, 1] 15 | mode: "czm" 16 | # Ground Plane Fitting parameters 17 | num_iter: 3 18 | num_lpr: 20 19 | num_min_pts: 10 20 | th_seeds: 0.5 21 | th_dist: 0.125 22 | max_r: 80.0 23 | min_r: 2.7 # to consider vicinity of mobile plot form. 24 | uprightness_thr: 0.707 # For uprightness. 45: 0.707 / 60: 0.866. The larger, the more conservative 25 | 26 | # The points below the adaptive_seed_selection_margin * sensor_height are filtered 27 | # For reject points caused by reflection or multipath problems. 28 | # it should be lower than -1.0 29 | adaptive_seed_selection_margin: -1.1 30 | 31 | # It is not in the paper 32 | # It is also not matched our philosophy, but it is employed to reject some FPs easily & intuitively. 33 | # For patchwork, the global elevation threshold is only applied on Z3 and Z4 34 | using_global_elevation: false 35 | # W.r.t sensor frame (That is, if it is 0.0, then the candidates whose z is higher than z height of 3D LiDAR sensor are rejected 36 | global_elevation_threshold: -0.5 37 | 38 | # 22.05.02 Update 39 | # ATAT is the abbrev. for All-Terrain Automatic heighT estimator 40 | # It automatically corrects the wrong sensor height input 41 | ATAT: 42 | ATAT_ON: true 43 | # 22.05.02 Update 44 | # IMPORTANT - `max_r_for_ATAT` affects the quality of the estimation of sensor height 45 | # If it is too large, then the z-elevation values of the bins become more ambiguous 46 | # If it is too small, there is a potential risk that does not include the cloud points in the vicinity of the vehicles/mobile robots 47 | # Therefore, it should be set appropriately! 48 | max_r_for_ATAT: 5.0 49 | num_sectors_for_ATAT: 20 50 | noise_bound: 0.2 51 | 52 | czm: 53 | # 22.05.02 Update 54 | # For usability, the MEANING OF THE ELEVATION THRESHOLDS IS CHANGED!!!! 55 | # Original - the elevation w.r.t. to the sensor frame 56 | # Modified - the elevation w.r.t. to the ground 57 | # Thus, -sensor_height + elevation_threshold is the criteria. 58 | # E.g. for the first ring, -1.723 (sensor height in the KITTI) + 0.523 = -1.2 becomes the criteria of the elevation filter 59 | # That is, if the z-elevation of the bin is higher than -1.2, then the cloud points from the bin are rejected 60 | elevation_thresholds: [0.523, 0.746, 0.879, 1.125] # For elevation. The size should be equal to flatness_thresholds vector 61 | flatness_thresholds: [0.0005, 0.000725, 0.001, 0.001] # For flatness. The size should be equal to elevation_thresholds vector 62 | -------------------------------------------------------------------------------- /img/demo_kitti00_v2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/demo_kitti00_v2.gif -------------------------------------------------------------------------------- /img/demo_terrain_v3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/demo_terrain_v3.gif -------------------------------------------------------------------------------- /img/kimera-multi.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/kimera-multi.gif -------------------------------------------------------------------------------- /img/kitti_activated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/kitti_activated.png -------------------------------------------------------------------------------- /img/nonplanar_gpf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/nonplanar_gpf.png -------------------------------------------------------------------------------- /img/ouster128_right_elevation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/ouster128_right_elevation.png -------------------------------------------------------------------------------- /img/ouster128_wrong_elevation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/ouster128_wrong_elevation.png -------------------------------------------------------------------------------- /img/patchwork.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/patchwork.gif -------------------------------------------------------------------------------- /img/patchwork_concept_resized.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/patchwork_concept_resized.jpg -------------------------------------------------------------------------------- /img/precision_recall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/precision_recall.png -------------------------------------------------------------------------------- /img/profile_low.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/profile_low.png -------------------------------------------------------------------------------- /img/sensor_height_issue.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/sensor_height_issue.png -------------------------------------------------------------------------------- /img/seq_00_pr_zoom.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/seq_00_pr_zoom.pdf -------------------------------------------------------------------------------- /img/wiki_gle1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_gle1.png -------------------------------------------------------------------------------- /img/wiki_gle2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_gle2.png -------------------------------------------------------------------------------- /img/wiki_gle3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_gle3.png -------------------------------------------------------------------------------- /img/wiki_gle4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_gle4.png -------------------------------------------------------------------------------- /img/wiki_gle_param_v2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_gle_param_v2.png -------------------------------------------------------------------------------- /img/wiki_intro1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_intro1.png -------------------------------------------------------------------------------- /img/wiki_intro2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_intro2.png -------------------------------------------------------------------------------- /img/wiki_intro3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_intro3.png -------------------------------------------------------------------------------- /img/wiki_intro4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_intro4.png -------------------------------------------------------------------------------- /img/wiki_intro5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_intro5.png -------------------------------------------------------------------------------- /img/wiki_method1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_method1.png -------------------------------------------------------------------------------- /img/wiki_method2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_method2.png -------------------------------------------------------------------------------- /img/wiki_method3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_method3.png -------------------------------------------------------------------------------- /img/wiki_method4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_method4.png -------------------------------------------------------------------------------- /img/wiki_overview.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_overview.PNG -------------------------------------------------------------------------------- /img/wiki_test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/img/wiki_test.png -------------------------------------------------------------------------------- /include/label_generator/label_generator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_LABEL_GENERATOR_LABEL_GENERATOR_HPP_ 2 | #define INCLUDE_LABEL_GENERATOR_LABEL_GENERATOR_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "label_generator/nanoflann.hpp" 20 | #include "label_generator/nanoflann_utils.hpp" 21 | 22 | #define EST_NON_GROUND 0 23 | using namespace std; 24 | 25 | using num_t = float; // for NanoFlann 26 | 27 | template 28 | void filter_by_sor(const pcl::PointCloud &src, 29 | pcl::PointCloud &filtered, 30 | int num_neighbor_pts = 20, 31 | float std_multiplier = 5.0) { 32 | typename pcl::PointCloud::Ptr tmp(new pcl::PointCloud); 33 | *tmp = src; 34 | 35 | pcl::StatisticalOutlierRemoval sor; 36 | sor.setInputCloud(tmp); 37 | sor.setMeanK(num_neighbor_pts); 38 | sor.setStddevMulThresh(std_multiplier); 39 | sor.filter(filtered); 40 | } 41 | 42 | template 43 | void save_ground_label(const std::string abs_dir, 44 | const int frame_num, 45 | const pcl::PointCloud &cloud_raw, 46 | const pcl::PointCloud &cloud_est_ground, 47 | uint32_t GROUND_LABEL = 1) { 48 | // Save the estimate ground points into a `.label` file 49 | // It is relevant to 3DUIS benchmark and Stachniss lab's format 50 | // https://codalab.lisn.upsaclay.fr/competitions/2183?secret_key=4763e3d2-1f22-45e6-803a-a862528426d2 51 | const float SQR_EPSILON = 0.00001; 52 | 53 | int num_cloud_raw = cloud_raw.points.size(); 54 | std::vector labels(num_cloud_raw, 55 | EST_NON_GROUND); // 0: Non ground points 56 | 57 | size_t N = cloud_est_ground.points.size(); 58 | PointCloud cloud; 59 | cloud.pts.resize(N); 60 | for (size_t i = 0; i < N; i++) { 61 | cloud.pts[i].x = cloud_est_ground.points[i].x; 62 | cloud.pts[i].y = cloud_est_ground.points[i].y; 63 | cloud.pts[i].z = cloud_est_ground.points[i].z; 64 | } 65 | 66 | // construct a kd-tree index: 67 | using my_kd_tree_t = 68 | nanoflann::KDTreeSingleIndexAdaptor>, 69 | PointCloud, 70 | 3 /* dim */ 71 | >; 72 | 73 | my_kd_tree_t index(3 /*dim*/, cloud, {10 /* max leaf */}); 74 | 75 | int num_valid = 0; 76 | for (size_t j = 0; j < cloud_raw.points.size(); ++j) { 77 | const auto query_pcl = cloud_raw.points[j]; 78 | const num_t query_pt[3] = {query_pcl.x, query_pcl.y, query_pcl.z}; 79 | 80 | size_t num_results = 1; 81 | std::vector ret_index(num_results); 82 | std::vector out_dist_sqr(num_results); 83 | 84 | num_results = index.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]); 85 | 86 | ret_index.resize(num_results); 87 | out_dist_sqr.resize(num_results); 88 | if (out_dist_sqr[0] < SQR_EPSILON) { // it is the same point! 89 | // Est. ground - 1, Est. nonground - 0 90 | // But BE CAREFUL! In some cases, ground are labeled as `9`, 91 | // which is Rhiney (Xieyuanli Chen)'s previous researches 92 | // Please refer to the link: 93 | labels[j] = GROUND_LABEL; 94 | ++num_valid; 95 | } 96 | } 97 | 98 | // Must be equal to the # of above-ground points 99 | std::cout << "# of valid points: " << num_valid << std::endl; 100 | 101 | // To follow the KITTI format, # of zeros are set to 6 102 | const int NUM_ZEROS = 6; 103 | 104 | std::string count_str = std::to_string(frame_num); 105 | std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 106 | std::string abs_label_path = abs_dir + "/" + count_str_padded + ".label"; 107 | 108 | std::cout << "\033[1;32m" << abs_label_path << "\033[0m" << std::endl; 109 | std::ofstream output_file(abs_label_path, std::ios::out | std::ios::binary); 110 | output_file.write(reinterpret_cast(&labels[0]), num_cloud_raw * sizeof(uint32_t)); 111 | } 112 | 113 | #endif // INCLUDE_LABEL_GENERATOR_LABEL_GENERATOR_HPP_ 114 | -------------------------------------------------------------------------------- /include/label_generator/nanoflann_utils.hpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2011-2022 Jose Luis Blanco (joseluisblancoc@gmail.com). 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | #ifndef INCLUDE_LABEL_GENERATOR_NANOFLANN_UTILS_HPP_ 29 | #define INCLUDE_LABEL_GENERATOR_NANOFLANN_UTILS_HPP_ 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | template 36 | struct PointCloud { 37 | struct Point { 38 | T x, y, z; 39 | }; 40 | 41 | using coord_t = T; //!< The type of each coordinate 42 | 43 | std::vector pts; 44 | 45 | // Must return the number of data points 46 | inline size_t kdtree_get_point_count() const { return pts.size(); } 47 | 48 | // Returns the dim'th component of the idx'th point in the class: 49 | // Since this is inlined and the "dim" argument is typically an immediate 50 | // value, the 51 | // "if/else's" are actually solved at compile time. 52 | inline T kdtree_get_pt(const size_t idx, const size_t dim) const { 53 | if (dim == 0) 54 | return pts[idx].x; 55 | else if (dim == 1) 56 | return pts[idx].y; 57 | else 58 | return pts[idx].z; 59 | } 60 | 61 | // Optional bounding-box computation: return false to default to a standard 62 | // bbox computation loop. 63 | // Return true if the BBOX was already computed by the class and returned 64 | // in "bb" so it can be avoided to redo it again. Look at bb.size() to 65 | // find out the expected dimensionality (e.g. 2 or 3 for point clouds) 66 | template 67 | bool kdtree_get_bbox(BBOX & /* bb */) const { 68 | return false; 69 | } 70 | }; 71 | 72 | template 73 | void generateRandomPointCloudRanges(PointCloud &pc, 74 | const size_t N, 75 | const T max_range_x, 76 | const T max_range_y, 77 | const T max_range_z) { 78 | // Generating Random Point Cloud 79 | pc.pts.resize(N); 80 | for (size_t i = 0; i < N; i++) { 81 | pc.pts[i].x = max_range_x * (rand() % 1000) / T(1000); 82 | pc.pts[i].y = max_range_y * (rand() % 1000) / T(1000); 83 | pc.pts[i].z = max_range_z * (rand() % 1000) / T(1000); 84 | } 85 | } 86 | 87 | template 88 | void generateRandomPointCloud(PointCloud &pc, const size_t N, const T max_range = 10) { 89 | generateRandomPointCloudRanges(pc, N, max_range, max_range, max_range); 90 | } 91 | 92 | // This is an exampleof a custom data set class 93 | template 94 | struct PointCloud_Quat { 95 | struct Point { 96 | T w, x, y, z; 97 | }; 98 | 99 | std::vector pts; 100 | 101 | // Must return the number of data points 102 | inline size_t kdtree_get_point_count() const { return pts.size(); } 103 | 104 | // Returns the dim'th component of the idx'th point in the class: 105 | // Since this is inlined and the "dim" argument is typically an immediate 106 | // value, the 107 | // "if/else's" are actually solved at compile time. 108 | inline T kdtree_get_pt(const size_t idx, const size_t dim) const { 109 | if (dim == 0) 110 | return pts[idx].w; 111 | else if (dim == 1) 112 | return pts[idx].x; 113 | else if (dim == 2) 114 | return pts[idx].y; 115 | else 116 | return pts[idx].z; 117 | } 118 | 119 | // Optional bounding-box computation: return false to default to a standard 120 | // bbox computation loop. 121 | // Return true if the BBOX was already computed by the class and returned 122 | // in "bb" so it can be avoided to redo it again. Look at bb.size() to 123 | // find out the expected dimensionality (e.g. 2 or 3 for point clouds) 124 | template 125 | bool kdtree_get_bbox(BBOX & /* bb */) const { 126 | return false; 127 | } 128 | }; 129 | 130 | template 131 | void generateRandomPointCloud_Quat(PointCloud_Quat &point, const size_t N) { 132 | // Generating Random Quaternions 133 | point.pts.resize(N); 134 | T theta, X, Y, Z, sinAng, cosAng, mag; 135 | for (size_t i = 0; i < N; i++) { 136 | theta = static_cast(nanoflann::pi_const() * (((double)rand()) / RAND_MAX)); 137 | // Generate random value in [-1, 1] 138 | X = static_cast(2 * (((double)rand()) / RAND_MAX) - 1); 139 | Y = static_cast(2 * (((double)rand()) / RAND_MAX) - 1); 140 | Z = static_cast(2 * (((double)rand()) / RAND_MAX) - 1); 141 | mag = sqrt(X * X + Y * Y + Z * Z); 142 | X /= mag; 143 | Y /= mag; 144 | Z /= mag; 145 | cosAng = cos(theta / 2); 146 | sinAng = sin(theta / 2); 147 | point.pts[i].w = cosAng; 148 | point.pts[i].x = X * sinAng; 149 | point.pts[i].y = Y * sinAng; 150 | point.pts[i].z = Z * sinAng; 151 | } 152 | } 153 | 154 | // This is an exampleof a custom data set class 155 | template 156 | struct PointCloud_Orient { 157 | struct Point { 158 | T theta; 159 | }; 160 | 161 | std::vector pts; 162 | 163 | // Must return the number of data points 164 | inline size_t kdtree_get_point_count() const { return pts.size(); } 165 | 166 | // Returns the dim'th component of the idx'th point in the class: 167 | // Since this is inlined and the "dim" argument is typically an immediate 168 | // value, the 169 | // "if/else's" are actually solved at compile time. 170 | inline T kdtree_get_pt(const size_t idx, const size_t dim = 0) const { return pts[idx].theta; } 171 | 172 | // Optional bounding-box computation: return false to default to a standard 173 | // bbox computation loop. 174 | // Return true if the BBOX was already computed by the class and returned 175 | // in "bb" so it can be avoided to redo it again. Look at bb.size() to 176 | // find out the expected dimensionality (e.g. 2 or 3 for point clouds) 177 | template 178 | bool kdtree_get_bbox(BBOX & /* bb */) const { 179 | return false; 180 | } 181 | }; 182 | 183 | template 184 | void generateRandomPointCloud_Orient(PointCloud_Orient &point, const size_t N) { 185 | // Generating Random Orientations 186 | point.pts.resize(N); 187 | for (size_t i = 0; i < N; i++) { 188 | point.pts[i].theta = 189 | static_cast((2 * nanoflann::pi_const() * (((double)rand()) / RAND_MAX)) - 190 | nanoflann::pi_const()); 191 | } 192 | } 193 | 194 | inline void dump_mem_usage() { 195 | FILE *f = fopen("/proc/self/statm", "rt"); 196 | if (!f) return; 197 | char str[300]; 198 | size_t n = fread(str, 1, 200, f); 199 | str[n] = 0; 200 | printf("MEM: %s\n", str); 201 | fclose(f); 202 | } 203 | #endif // INCLUDE_LABEL_GENERATOR_NANOFLANN_UTILS_HPP_ 204 | -------------------------------------------------------------------------------- /include/patchwork/patchwork.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_PATCHWORK_PATCHWORK_HPP_ 2 | #define INCLUDE_PATCHWORK_PATCHWORK_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | // #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "./zone_models.hpp" 23 | #include "tools/tictoc.hpp" 24 | 25 | // vector cout operator 26 | template 27 | std::ostream &operator<<(std::ostream &os, const std::vector &vec) { 28 | os << "[ "; 29 | for (const auto &element : vec) { 30 | os << element << " "; 31 | } 32 | os << "]"; 33 | return os; 34 | } 35 | 36 | #define NUM_HEURISTIC_MAX_PTS_IN_PATCH 3000 37 | #define MARKER_Z_VALUE -2.2 38 | 39 | // Below colors are for visualization purpose 40 | #define COLOR_CYAN 0.55 // cyan 41 | #define COLOR_GREEN 0.2 // green 42 | #define COLOR_BLUE 0.0 // blue 43 | #define COLOR_RED 1.0 // red 44 | #define COLOR_GLOBALLY_TOO_HIGH_ELEVATION 0.8 // I'm not sure...haha 45 | 46 | int NOT_ASSIGNED = -2; 47 | int FEW_POINTS = -1; 48 | int UPRIGHT_ENOUGH = 0; // cyan 49 | int FLAT_ENOUGH = 1; // green 50 | int TOO_HIGH_ELEVATION = 2; // blue 51 | int TOO_TILTED = 3; // red 52 | int GLOBALLY_TOO_HIGH_ELEVATION = 4; 53 | 54 | std::vector COLOR_MAP = {COLOR_CYAN, 55 | COLOR_GREEN, 56 | COLOR_BLUE, 57 | COLOR_RED, 58 | COLOR_GLOBALLY_TOO_HIGH_ELEVATION}; 59 | 60 | using Eigen::JacobiSVD; 61 | using Eigen::MatrixXf; 62 | using Eigen::VectorXf; 63 | 64 | using namespace std; 65 | 66 | /* 67 | @brief PathWork ROS Node. 68 | */ 69 | template 70 | bool point_z_cmp(PointT a, PointT b) { 71 | return a.z < b.z; 72 | } 73 | 74 | struct PCAFeature { 75 | Eigen::Vector3f principal_; 76 | Eigen::Vector3f normal_; 77 | Eigen::Vector3f singular_values_; 78 | Eigen::Vector3f mean_; 79 | float d_; 80 | float th_dist_d_; 81 | float linearity_; 82 | float planarity_; 83 | }; 84 | 85 | template 86 | struct Patch { 87 | bool is_close_to_origin_ = false; // If so, we can set threshold more conservatively 88 | int ring_idx_ = NOT_ASSIGNED; 89 | int sector_idx_ = NOT_ASSIGNED; 90 | 91 | int status_ = NOT_ASSIGNED; 92 | 93 | PCAFeature feature_; 94 | 95 | pcl::PointCloud cloud_; 96 | pcl::PointCloud ground_; 97 | pcl::PointCloud non_ground_; 98 | 99 | void clear() { 100 | if (!cloud_.empty()) cloud_.clear(); 101 | if (!ground_.empty()) ground_.clear(); 102 | if (!non_ground_.empty()) non_ground_.clear(); 103 | } 104 | }; 105 | 106 | template 107 | class PatchWork { 108 | public: 109 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 110 | 111 | typedef std::vector> Ring; 112 | typedef std::vector RegionwisePatches; 113 | 114 | PatchWork() {} 115 | 116 | explicit PatchWork(rclcpp::Node *node) { 117 | RCLCPP_INFO(node->get_logger(), "Initializing PatchWork..."); 118 | 119 | verbose_ = node->declare_parameter("verbose", false); 120 | sensor_height_ = node->declare_parameter("sensor_height", 1.723); 121 | sensor_model_ = node->declare_parameter("sensor_model", "HDL-64E"); 122 | 123 | ATAT_ON_ = node->declare_parameter("ATAT.ATAT_ON", false); 124 | max_r_for_ATAT_ = node->declare_parameter("ATAT.max_r_for_ATAT", 5.0); 125 | num_sectors_for_ATAT_ = node->declare_parameter("ATAT.num_sectors_for_ATAT", 20); 126 | noise_bound_ = node->declare_parameter("ATAT.noise_bound", 0.2); 127 | 128 | num_iter_ = node->declare_parameter("num_iter", 3); 129 | num_lpr_ = node->declare_parameter("num_lpr", 20); 130 | num_min_pts_ = node->declare_parameter("num_min_pts", 10); 131 | th_seeds_ = node->declare_parameter("th_seeds", 0.5); 132 | th_dist_ = node->declare_parameter("th_dist", 0.125); 133 | max_range_ = node->declare_parameter("max_r", 80.0); 134 | min_range_ = node->declare_parameter("min_r", 2.7); 135 | 136 | uprightness_thr_ = node->declare_parameter("uprightness_thr", 0.5); 137 | adaptive_seed_selection_margin_ = 138 | node->declare_parameter("adaptive_seed_selection_margin", -1.1); 139 | 140 | using_global_thr_ = node->declare_parameter("using_global_elevation", true); 141 | global_elevation_thr_ = node->declare_parameter("global_elevation_threshold", 0.0); 142 | 143 | elevation_thr_ = node->declare_parameter>("czm.elevation_thresholds", 144 | {0.523, 0.746, 0.879, 1.125}); 145 | flatness_thr_ = node->declare_parameter>("czm.flatness_thresholds", 146 | {0.0005, 0.000725, 0.001, 0.001}); 147 | 148 | visualize_ = node->declare_parameter("visualize", true); 149 | 150 | // poly_list_.header.frame_id = frame_patchwork; 151 | // poly_list_.polygons.reserve(130000); 152 | reverted_points_by_flatness_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); 153 | 154 | if (using_global_thr_) { 155 | RCLCPP_WARN( 156 | node->get_logger(), "Global elevation threshold is ON: %f", global_elevation_thr_); 157 | } else { 158 | RCLCPP_INFO(node->get_logger(), "Global elevation threshold is OFF."); 159 | } 160 | 161 | RCLCPP_INFO(node->get_logger(), "Sensor model: %s", sensor_model_.c_str()); 162 | RCLCPP_INFO(node->get_logger(), "Sensor height: %.3f", sensor_height_); 163 | RCLCPP_INFO(node->get_logger(), "Range: [%.2f, %.2f]", min_range_, max_range_); 164 | RCLCPP_INFO( 165 | node->get_logger(), "Seed threshold: %.3f | Distance threshold: %.3f", th_seeds_, th_dist_); 166 | RCLCPP_INFO(node->get_logger(), "Uprightness threshold: %.3f", uprightness_thr_); 167 | RCLCPP_INFO(node->get_logger(), 168 | "Elevation thresholds: %.3f %.3f %.3f %.3f", 169 | elevation_thr_[0], 170 | elevation_thr_[1], 171 | elevation_thr_[2], 172 | elevation_thr_[3]); 173 | 174 | zone_model_ = ConcentricZoneModel(sensor_model_, sensor_height_, min_range_, max_range_); 175 | num_rings_of_interest_ = elevation_thr_.size(); 176 | 177 | const auto &num_sectors_each_zone_ = zone_model_.sensor_config_.num_sectors_for_each_zone_; 178 | sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 179 | 2 * M_PI / num_sectors_each_zone_.at(1), 180 | 2 * M_PI / num_sectors_each_zone_.at(2), 181 | 2 * M_PI / num_sectors_each_zone_.at(3)}; 182 | 183 | initialize(regionwise_patches_, zone_model_); 184 | 185 | RCLCPP_INFO(node->get_logger(), "PatchWork initialization complete."); 186 | } 187 | 188 | void estimate_ground(const pcl::PointCloud &cloud_in, 189 | pcl::PointCloud &ground, 190 | pcl::PointCloud &nonground); 191 | 192 | inline double get_time() { return time_taken_; } 193 | 194 | geometry_msgs::msg::PolygonStamped set_plane_polygon(const MatrixXf &normal_v, const float &d); 195 | 196 | private: 197 | // For ATAT (All-Terrain Automatic heighT estimator) 198 | bool ATAT_ON_; 199 | double noise_bound_; 200 | double max_r_for_ATAT_; 201 | int num_sectors_for_ATAT_; 202 | 203 | int num_iter_; 204 | int num_lpr_; 205 | int num_min_pts_; 206 | int num_rings_of_interest_; 207 | 208 | double sensor_height_; 209 | double th_seeds_; 210 | double th_dist_; 211 | double max_range_; 212 | double min_range_; 213 | double uprightness_thr_; 214 | double adaptive_seed_selection_margin_; 215 | 216 | bool verbose_; 217 | bool initialized_ = true; 218 | 219 | double time_taken_; 220 | 221 | // For global threshold 222 | bool using_global_thr_; 223 | double global_elevation_thr_; 224 | 225 | // For visualization 226 | bool visualize_; 227 | 228 | std::string sensor_model_; 229 | vector> patch_indices_; // For multi-threading. {ring_idx, sector_idx} 230 | ConcentricZoneModel zone_model_; 231 | 232 | vector sector_sizes_; 233 | vector ring_sizes_; 234 | vector min_ranges_; 235 | vector elevation_thr_; 236 | vector flatness_thr_; 237 | 238 | RegionwisePatches regionwise_patches_; 239 | 240 | // TOGO(hlim): Now, jsk_recognition_msgs does not support ROS2 241 | // jsk_recognition_msgs::PolygonArray poly_list_; 242 | 243 | pcl::PointCloud reverted_points_by_flatness_, rejected_points_by_elevation_; 244 | 245 | void initialize(RegionwisePatches &patches, const ConcentricZoneModel &zone_model); 246 | 247 | void flush_patches(RegionwisePatches &patches); 248 | 249 | double xy2theta(const double &x, const double &y); 250 | 251 | double xy2radius(const double &x, const double &y); 252 | 253 | void pc2regionwise_patches(const pcl::PointCloud &src, RegionwisePatches &patches); 254 | 255 | void estimate_plane_(const pcl::PointCloud &ground, PCAFeature &feat); 256 | 257 | void perform_regionwise_ground_segmentation(Patch &patch, 258 | const bool is_h_available = true); 259 | 260 | double consensus_set_based_height_estimation(const Eigen::RowVectorXd &X, 261 | const Eigen::RowVectorXd &ranges, 262 | const Eigen::RowVectorXd &weights); 263 | 264 | void estimate_sensor_height(pcl::PointCloud cloud_in); 265 | 266 | void extract_initial_seeds_(const pcl::PointCloud &p_sorted, 267 | pcl::PointCloud &init_seeds, 268 | bool is_close_to_origin, 269 | bool is_h_available); 270 | 271 | /*** 272 | * For visulization of Ground Likelihood Estimation 273 | */ 274 | // geometry_msgs::msg::PolygonStamped set_polygons(int ring_idx, int sector_idx, int num_split); 275 | 276 | int determine_ground_likelihood_estimation_status(const int concentric_idx, 277 | const double z_vec, 278 | const double z_elevation, 279 | const double surface_variable); 280 | }; 281 | 282 | template 283 | inline void PatchWork::initialize(RegionwisePatches &patches, 284 | const ConcentricZoneModel &zone_model) { 285 | patches.clear(); 286 | patch_indices_.clear(); 287 | Patch patch; 288 | 289 | // Reserve memory in advance to boost speed 290 | patch.cloud_.reserve(1000); 291 | patch.ground_.reserve(1000); 292 | patch.non_ground_.reserve(1000); 293 | 294 | // In polar coordinates, `num_columns` are `num_sectors` 295 | // and `num_rows` are `num_rings`, respectively 296 | int num_rows = zone_model_.num_total_rings_; 297 | const auto &num_sectors_per_ring = zone_model.num_sectors_per_ring_; 298 | 299 | for (int j = 0; j < num_rows; j++) { 300 | Ring ring; 301 | patch.ring_idx_ = j; 302 | patch.is_close_to_origin_ = j < zone_model.max_ring_index_in_first_zone; 303 | for (int i = 0; i < num_sectors_per_ring[j]; i++) { 304 | patch.sector_idx_ = i; 305 | ring.emplace_back(patch); 306 | 307 | patch_indices_.emplace_back(j, i); 308 | } 309 | patches.emplace_back(ring); 310 | } 311 | } 312 | 313 | template 314 | inline void PatchWork::flush_patches(RegionwisePatches &patches) { 315 | int num_rows = patches.size(); 316 | for (int j = 0; j < num_rows; j++) { 317 | int num_columns = patches[j].size(); 318 | for (int i = 0; i < num_columns; i++) { 319 | patches[j][i].clear(); 320 | } 321 | } 322 | } 323 | 324 | template 325 | inline void PatchWork::estimate_plane_(const pcl::PointCloud &ground, 326 | PCAFeature &feat) { 327 | Eigen::Vector4f pc_mean; 328 | Eigen::Matrix3f cov; 329 | pcl::computeMeanAndCovarianceMatrix(ground, cov, pc_mean); 330 | 331 | // Singular Value Decomposition: SVD 332 | Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); 333 | feat.singular_values_ = svd.singularValues(); 334 | 335 | feat.linearity_ = 336 | (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); 337 | feat.planarity_ = 338 | (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); 339 | 340 | // use the least singular vector as normal 341 | feat.normal_ = (svd.matrixU().col(2)); 342 | if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive 343 | feat.normal_ = -feat.normal_; 344 | } 345 | // mean ground seeds value 346 | feat.mean_ = pc_mean.head<3>(); 347 | // according to normal.T*[x,y,z] = -d 348 | feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); 349 | feat.th_dist_d_ = th_dist_ - feat.d_; 350 | } 351 | 352 | template 353 | inline void PatchWork::extract_initial_seeds_(const pcl::PointCloud &p_sorted, 354 | pcl::PointCloud &init_seeds, 355 | bool is_close_to_origin, 356 | bool is_h_available) { 357 | init_seeds.points.clear(); 358 | 359 | // LPR is the mean of low point representative 360 | double sum = 0; 361 | int cnt = 0; 362 | 363 | int init_idx = 0; 364 | // Empirically, adaptive seed selection applying to Z1 is fine 365 | if (is_h_available) { 366 | static double lowest_h_margin_in_close_zone = 367 | (sensor_height_ == 0.0) ? -0.1 : adaptive_seed_selection_margin_ * sensor_height_; 368 | if (is_close_to_origin) { 369 | for (size_t i = 0; i < p_sorted.points.size(); i++) { 370 | if (p_sorted.points[i].z < lowest_h_margin_in_close_zone) { 371 | ++init_idx; 372 | } else { 373 | break; 374 | } 375 | } 376 | } 377 | } 378 | 379 | // Calculate the mean height value. 380 | for (size_t i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { 381 | sum += p_sorted.points[i].z; 382 | cnt++; 383 | } 384 | double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0 385 | 386 | // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ 387 | for (size_t i = 0; i < p_sorted.points.size(); i++) { 388 | if (p_sorted.points[i].z < lpr_height + th_seeds_) { 389 | init_seeds.points.push_back(p_sorted.points[i]); 390 | } 391 | } 392 | } 393 | 394 | template 395 | inline double PatchWork::consensus_set_based_height_estimation( 396 | const Eigen::RowVectorXd &X, 397 | const Eigen::RowVectorXd &ranges, 398 | const Eigen::RowVectorXd &weights) { 399 | // check input parameters - dimension inconsistent test 400 | assert(!((X.rows() != ranges.rows()) || (X.cols() != ranges.cols()))); 401 | 402 | // check input parameters - only one element test 403 | assert(!((X.rows() == 1) && (X.cols() == 1))); 404 | 405 | int N = X.cols(); 406 | std::vector> h; 407 | for (int i = 0; i < N; ++i) { 408 | h.push_back(std::make_pair(X(i) - ranges(i), i + 1)); 409 | h.push_back(std::make_pair(X(i) + ranges(i), -i - 1)); 410 | } 411 | 412 | // ascending order 413 | std::sort(h.begin(), h.end(), [](std::pair a, std::pair b) { 414 | return a.first < b.first; 415 | }); 416 | 417 | int nr_centers = 2 * N; 418 | Eigen::RowVectorXd x_hat = Eigen::MatrixXd::Zero(1, nr_centers); 419 | Eigen::RowVectorXd x_cost = Eigen::MatrixXd::Zero(1, nr_centers); 420 | 421 | double ranges_inverse_sum = ranges.sum(); 422 | double dot_X_weights = 0; 423 | double dot_weights_consensus = 0; 424 | int consensus_set_cardinal = 0; 425 | double sum_xi = 0; 426 | double sum_xi_square = 0; 427 | 428 | for (size_t i = 0; i < nr_centers; ++i) { 429 | int idx = static_cast(std::abs(h.at(i).second)) - 1; // Indices starting at 1 430 | int epsilon = (h.at(i).second > 0) ? 1 : -1; 431 | 432 | consensus_set_cardinal += epsilon; 433 | dot_weights_consensus += epsilon * weights(idx); 434 | dot_X_weights += epsilon * weights(idx) * X(idx); 435 | ranges_inverse_sum -= epsilon * ranges(idx); 436 | sum_xi += epsilon * X(idx); 437 | sum_xi_square += epsilon * X(idx) * X(idx); 438 | 439 | x_hat(i) = dot_X_weights / dot_weights_consensus; 440 | 441 | double residual = 442 | consensus_set_cardinal * x_hat(i) * x_hat(i) + sum_xi_square - 2 * sum_xi * x_hat(i); 443 | x_cost(i) = residual + ranges_inverse_sum; 444 | } 445 | 446 | size_t min_idx; 447 | x_cost.minCoeff(&min_idx); 448 | double estimate_temp = x_hat(min_idx); 449 | return estimate_temp; 450 | } 451 | 452 | template 453 | inline void PatchWork::estimate_sensor_height(pcl::PointCloud cloud_in) { 454 | // ATAT: All-Terrain Automatic HeighT estimator 455 | Ring ring_for_ATAT(num_sectors_for_ATAT_); 456 | for (auto const &pt : cloud_in.points) { 457 | int sector_idx; 458 | double r = xy2radius(pt.x, pt.y); 459 | 460 | float sector_size_for_ATAT = 2 * M_PI / num_sectors_for_ATAT_; 461 | 462 | if ((r <= max_r_for_ATAT_) && (r > min_range_)) { 463 | double theta = xy2theta(pt.x, pt.y); 464 | 465 | sector_idx = min(static_cast((theta / sector_size_for_ATAT)), num_sectors_for_ATAT_); 466 | ring_for_ATAT[sector_idx].cloud_.points.emplace_back(pt); 467 | } 468 | } 469 | 470 | // Assign valid measurements and corresponding linearities/planarities 471 | vector ground_elevations_wrt_the_origin; 472 | vector linearities; 473 | vector planarities; 474 | for (int i = 0; i < num_sectors_for_ATAT_; ++i) { 475 | if (static_cast(ring_for_ATAT[i].cloud_.size()) < num_min_pts_) { 476 | continue; 477 | } 478 | 479 | pcl::PointCloud dummy_est_ground; 480 | pcl::PointCloud dummy_est_non_ground; 481 | perform_regionwise_ground_segmentation(ring_for_ATAT[i], false); 482 | 483 | const auto &feat = ring_for_ATAT[i].feature_; 484 | 485 | const double ground_z_vec = abs(feat.normal_(2)); 486 | const double ground_z_elevation = feat.mean_(2); 487 | 488 | // Check whether the vector is sufficiently upright and flat 489 | if (ground_z_vec > uprightness_thr_ && feat.linearity_ < 0.9) { 490 | ground_elevations_wrt_the_origin.push_back(ground_z_elevation); 491 | linearities.push_back(feat.linearity_); 492 | planarities.push_back(feat.planarity_); 493 | } 494 | } 495 | 496 | // Setting for consensus set-based height estimation 497 | // It is equal to component-wise translation estimation (COTE) in TEASER++ 498 | int N = ground_elevations_wrt_the_origin.size(); 499 | cout << "\033[1;33m[ATAT] N: " << N << " -> " << ground_elevations_wrt_the_origin.size() 500 | << "\033[0m" << endl; 501 | if (ground_elevations_wrt_the_origin.size() == 0) { 502 | throw invalid_argument( 503 | "No valid ground points for ATAT! Please check the " 504 | "input data and `max_r_for_ATAT`"); 505 | } 506 | Eigen::Matrix values = Eigen::MatrixXd::Ones(1, N); 507 | Eigen::Matrix ranges = noise_bound_ * Eigen::MatrixXd::Ones(1, N); 508 | Eigen::Matrix weights = 509 | 1.0 / (noise_bound_ * noise_bound_) * Eigen::MatrixXd::Ones(1, N); 510 | for (int i = 0; i < N; ++i) { 511 | values(0, i) = ground_elevations_wrt_the_origin[i]; 512 | ranges(0, i) = ranges(0, i) * linearities[i]; 513 | weights(0, i) = weights(0, i) * planarities[i] * planarities[i]; 514 | } 515 | 516 | double estimated_h = consensus_set_based_height_estimation(values, ranges, weights); 517 | cout << "\033[1;33m[ATAT] The sensor height is auto-calibrated via the " 518 | "ground points in the vicinity of the vehicle\033[0m" 519 | << endl; 520 | cout << "\033[1;33m[ATAT] Elevation of the ground w.r.t. the origin is " << estimated_h 521 | << " m\033[0m" << endl; 522 | 523 | // Note that these are opposites 524 | sensor_height_ = -estimated_h; 525 | } 526 | 527 | template 528 | inline void PatchWork::estimate_ground(const pcl::PointCloud &cloud_in, 529 | pcl::PointCloud &ground, 530 | pcl::PointCloud &nonground) { 531 | // Just for visualization 532 | // poly_list_.header.stamp = rclcpp::Clock().now(); 533 | // if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); 534 | // if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); 535 | 536 | if (initialized_ && ATAT_ON_) { 537 | estimate_sensor_height(cloud_in); 538 | initialized_ = false; 539 | std::cout << "\033[1;32m=> Complete to estimate the sensor height: " << sensor_height_ 540 | << "\033[0m" << std::endl; 541 | } 542 | 543 | // static double start, t0, t1, t2; 544 | // double t_total_ground = 0.0; 545 | double t_total_estimate = 0.0; 546 | // 1.Msg to pointcloud 547 | pcl::PointCloud cloud_in_tmp = cloud_in; 548 | 549 | auto timer = patchwork::TicToc(); 550 | 551 | // Error point removal 552 | // As there are some error mirror reflection under the ground, 553 | // Sort point according to height, here uses z-axis in default 554 | // -2.0 is a rough criteria 555 | size_t i = 0; 556 | while (i < cloud_in_tmp.points.size()) { 557 | if (cloud_in_tmp.points[i].z < -sensor_height_ - 2.0) { 558 | std::iter_swap(cloud_in_tmp.points.begin() + i, cloud_in_tmp.points.end() - 1); 559 | cloud_in_tmp.points.pop_back(); 560 | } else { 561 | ++i; 562 | } 563 | } 564 | 565 | // t1 = rclcpp::Clock().now().toSec(); 566 | flush_patches(regionwise_patches_); 567 | pc2regionwise_patches(cloud_in_tmp, regionwise_patches_); 568 | 569 | ground.clear(); 570 | nonground.clear(); 571 | reverted_points_by_flatness_.clear(); 572 | rejected_points_by_elevation_.clear(); 573 | 574 | int num_patches = patch_indices_.size(); 575 | 576 | // HT comments: TBB w/ blocked_range was faster in my desktop 577 | // 117 Hz vs 126 Hz 578 | // tbb::parallel_for(0, num_patches, [&](int i) { 579 | tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { 580 | for (int k = r.begin(); k < r.end(); ++k) { 581 | const auto &[ring_idx, sector_idx] = patch_indices_[k]; 582 | auto &patch = regionwise_patches_[ring_idx][sector_idx]; 583 | 584 | if (patch.cloud_.points.size() > num_min_pts_) { 585 | // 2022.05.02 update 586 | // Region-wise sorting is adopted, which is much faster than global 587 | // sorting! 588 | sort(patch.cloud_.points.begin(), patch.cloud_.points.end(), point_z_cmp); 589 | perform_regionwise_ground_segmentation(patch); 590 | 591 | const auto &feat = patch.feature_; 592 | 593 | const double ground_z_vec = abs(feat.normal_(2)); 594 | const double ground_z_elevation = feat.mean_(2); 595 | const double surface_variable = 596 | feat.singular_values_.minCoeff() / 597 | (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); 598 | 599 | patch.status_ = determine_ground_likelihood_estimation_status( 600 | ring_idx, ground_z_vec, ground_z_elevation, surface_variable); 601 | } else { 602 | // Why? Because it is better to reject noise points 603 | // That is, these noise points sometimes lead to mis-recognition or 604 | // wrong clustering Thus, in practice, just rejecting them is better 605 | // than using them But note that this may degrade quantitative 606 | // ground segmentation performance 607 | patch.ground_ = patch.cloud_; 608 | patch.non_ground_.clear(); 609 | patch.status_ = FEW_POINTS; 610 | } 611 | } 612 | }); 613 | 614 | std::for_each( 615 | patch_indices_.begin(), patch_indices_.end(), [&](const std::pair &index_pair) { 616 | int ring_idx = index_pair.first; 617 | int sector_idx = index_pair.second; 618 | 619 | const auto &patch = regionwise_patches_[ring_idx][sector_idx]; 620 | 621 | const auto &feat = patch.feature_; 622 | const auto ®ionwise_ground = patch.ground_; 623 | const auto ®ionwise_nonground = patch.non_ground_; 624 | const auto status = patch.status_; 625 | 626 | // if (visualize_ && (status != FEW_POINTS && status != NOT_ASSIGNED)) { 627 | // auto polygons = set_polygons(ring_idx, sector_idx, 3); 628 | // polygons.header = poly_list_.header; 629 | // poly_list_.polygons.emplace_back(polygons); 630 | // poly_list_.likelihood.emplace_back(COLOR_MAP[status]); 631 | // } 632 | 633 | double t_tmp2 = rclcpp::Clock().now().seconds(); 634 | if (status == FEW_POINTS) { 635 | ground += regionwise_ground; 636 | nonground += regionwise_nonground; 637 | } else if (status == TOO_TILTED) { 638 | // All points are rejected 639 | nonground += regionwise_ground; 640 | nonground += regionwise_nonground; 641 | } else if (status == GLOBALLY_TOO_HIGH_ELEVATION) { 642 | if (verbose_) { 643 | cout << "\033[1;33m[Global elevation] " << feat.mean_(2) << " > " 644 | << global_elevation_thr_ << "\033[0m\n"; 645 | } 646 | nonground += regionwise_ground; 647 | nonground += regionwise_nonground; 648 | } else if (status == TOO_HIGH_ELEVATION) { 649 | if (verbose_) { 650 | std::cout << "\033[1;34m[Elevation] Rejection operated. Check " << ring_idx 651 | << "th param. of elevation_thr_: " 652 | << -sensor_height_ + elevation_thr_[ring_idx] << " < " << feat.mean_(2) 653 | << "\033[0m\n"; 654 | rejected_points_by_elevation_ += regionwise_ground; 655 | } 656 | nonground += regionwise_ground; 657 | nonground += regionwise_nonground; 658 | } else if (status == FLAT_ENOUGH) { 659 | if (verbose_) { 660 | std::cout << "\033[1;36m[Flatness] Recovery operated. Check " << ring_idx 661 | << "th param. flatness_thr_: " << flatness_thr_[ring_idx] << " > " 662 | << feat.singular_values_.minCoeff() / 663 | (feat.singular_values_(0) + feat.singular_values_(1) + 664 | feat.singular_values_(2)) 665 | << "\033[0m\n"; 666 | reverted_points_by_flatness_ += regionwise_ground; 667 | } 668 | ground += regionwise_ground; 669 | nonground += regionwise_nonground; 670 | } else if (status == UPRIGHT_ENOUGH) { 671 | ground += regionwise_ground; 672 | nonground += regionwise_nonground; 673 | } else { 674 | std::invalid_argument( 675 | "Something wrong in " 676 | "`determine_ground_likelihood_estimation_status()` fn!"); 677 | } 678 | }); 679 | 680 | time_taken_ = timer.toc(); 681 | } 682 | 683 | template 684 | inline double PatchWork::xy2theta(const double &x, 685 | const double &y) { // 0 ~ 2 * PI 686 | /* 687 | if (y >= 0) { 688 | return atan2(y, x); // 1, 2 quadrant 689 | } else { 690 | return 2 * M_PI + atan2(y, x);// 3, 4 quadrant 691 | } 692 | */ 693 | auto atan_value = atan2(y, x); // EDITED! 694 | return atan_value > 0 ? atan_value : atan_value + 2 * M_PI; // EDITED! 695 | } 696 | 697 | template 698 | inline double PatchWork::xy2radius(const double &x, const double &y) { 699 | return sqrt(pow(x, 2) + pow(y, 2)); 700 | } 701 | 702 | template 703 | inline void PatchWork::pc2regionwise_patches(const pcl::PointCloud &src, 704 | RegionwisePatches &patches) { 705 | std::for_each(src.points.begin(), src.points.end(), [&](const auto &pt) { 706 | const auto &[ring_idx, sector_idx] = zone_model_.get_ring_sector_idx(pt.x, pt.y); 707 | if (ring_idx != INVALID_RING_IDX && ring_idx != OVERFLOWED_IDX) { 708 | patches[ring_idx][sector_idx].cloud_.points.emplace_back(pt); 709 | } 710 | }); 711 | } 712 | 713 | // For adaptive 714 | template 715 | inline void PatchWork::perform_regionwise_ground_segmentation(Patch &patch, 716 | const bool is_h_available) { 717 | // 0. Initialization 718 | int N = patch.cloud_.points.size(); 719 | pcl::PointCloud ground_tmp; 720 | ground_tmp.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); 721 | if (!patch.ground_.empty()) patch.ground_.clear(); 722 | if (!patch.non_ground_.empty()) patch.non_ground_.clear(); 723 | 724 | // 1. set seeds! 725 | extract_initial_seeds_(patch.cloud_, ground_tmp, patch.is_close_to_origin_, is_h_available); 726 | 727 | // 2. Extract ground 728 | for (int i = 0; i < num_iter_; i++) { 729 | estimate_plane_(ground_tmp, patch.feature_); 730 | ground_tmp.clear(); 731 | 732 | // pointcloud to matrix 733 | Eigen::MatrixXf points(N, 3); 734 | int j = 0; 735 | for (auto &p : patch.cloud_.points) { 736 | points.row(j++) = p.getVector3fMap(); 737 | } 738 | // ground plane model 739 | Eigen::VectorXf result = points * patch.feature_.normal_; 740 | // threshold filter 741 | for (int r = 0; r < N; r++) { 742 | if (i < num_iter_ - 1) { 743 | if (result[r] < patch.feature_.th_dist_d_) { 744 | ground_tmp.points.emplace_back(patch.cloud_[r]); 745 | } 746 | } else { // Final stage 747 | if (result[r] < patch.feature_.th_dist_d_) { 748 | patch.ground_.points.emplace_back(patch.cloud_.points[r]); 749 | } else { 750 | patch.non_ground_.points.emplace_back(patch.cloud_.points[r]); 751 | } 752 | } 753 | } 754 | } 755 | } 756 | 757 | // template 758 | // inline geometry_msgs::msg::PolygonStamped PatchWork::set_polygons(int ring_idx, 759 | // int sector_idx, 760 | // int num_split) { 761 | // static const auto &boundary_ranges = zone_model_.boundary_ranges_; 762 | // int num_sectors = zone_model_.num_sectors_per_ring_[ring_idx]; 763 | // geometry_msgs::msg::PolygonStamped polygons; 764 | // polygons.header.frame_id = frame_patchwork; 765 | // // Set point of polygon. Start from RL and ccw 766 | // geometry_msgs::msg::Point32 point; 767 | // double sector_size = 2.0 * M_PI / static_cast(num_sectors); 768 | // double angle_incremental = sector_size / static_cast(num_split); 769 | // // RL 770 | // double r_len = boundary_ranges[ring_idx]; 771 | // double angle = sector_idx * sector_size; 772 | 773 | // point.x = r_len * cos(angle); 774 | // point.y = r_len * sin(angle); 775 | // point.z = MARKER_Z_VALUE; 776 | // polygons.polygon.points.push_back(point); 777 | // // RU 778 | // r_len = boundary_ranges[ring_idx + 1]; 779 | // point.x = r_len * cos(angle); 780 | // point.y = r_len * sin(angle); 781 | // point.z = MARKER_Z_VALUE; 782 | // polygons.polygon.points.push_back(point); 783 | 784 | // // RU -> LU 785 | // for (int idx = 1; idx <= num_split; ++idx) { 786 | // angle = angle + angle_incremental; 787 | // point.x = r_len * cos(angle); 788 | // point.y = r_len * sin(angle); 789 | // point.z = MARKER_Z_VALUE; 790 | // polygons.polygon.points.push_back(point); 791 | // } 792 | 793 | // r_len = boundary_ranges[ring_idx]; 794 | // point.x = r_len * cos(angle); 795 | // point.y = r_len * sin(angle); 796 | // point.z = MARKER_Z_VALUE; 797 | // polygons.polygon.points.push_back(point); 798 | 799 | // for (int idx = 1; idx < num_split; ++idx) { 800 | // angle = angle - angle_incremental; 801 | // point.x = r_len * cos(angle); 802 | // point.y = r_len * sin(angle); 803 | // point.z = MARKER_Z_VALUE; 804 | // polygons.polygon.points.push_back(point); 805 | // } 806 | // return polygons; 807 | // } 808 | 809 | template 810 | inline int PatchWork::determine_ground_likelihood_estimation_status( 811 | const int ring_idx, 812 | const double z_vec, 813 | const double z_elevation, 814 | const double surface_variable) { 815 | if (z_vec < uprightness_thr_) { 816 | return TOO_TILTED; 817 | } else { // orthogonal 818 | if (ring_idx < num_rings_of_interest_) { 819 | if (z_elevation > -sensor_height_ + elevation_thr_[ring_idx]) { 820 | if (flatness_thr_[ring_idx] > surface_variable) { 821 | return FLAT_ENOUGH; 822 | } else { 823 | return TOO_HIGH_ELEVATION; 824 | } 825 | } else { 826 | return UPRIGHT_ENOUGH; 827 | } 828 | } else { 829 | if (using_global_thr_ && (z_elevation > global_elevation_thr_)) { 830 | return GLOBALLY_TOO_HIGH_ELEVATION; 831 | } else { 832 | return UPRIGHT_ENOUGH; 833 | } 834 | } 835 | } 836 | } 837 | 838 | #endif // INCLUDE_PATCHWORK_PATCHWORK_HPP_ 839 | -------------------------------------------------------------------------------- /include/patchwork/sensor_configs.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shapelim on 22. 10. 23. 3 | // 4 | 5 | #ifndef PATCHWORK_SENSOR_CONFIG_HPP 6 | #define PATCHWORK_SENSOR_CONFIG_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | struct SensorConfig { 15 | vector> num_laser_channels_per_zone_; 16 | vector num_rings_for_each_zone_; 17 | vector num_sectors_for_each_zone_; 18 | // Please refer to setPriorIdxesOfPatches 19 | 20 | int num_channels_; 21 | float lower_fov_boundary_; 22 | float vertical_angular_resolution_; 23 | int horizontal_resolution_; 24 | 25 | /** 26 | * Note: Only use-defined parameters are `num_laser_channels_per_zone_` and 27 | * `num_sectors_for_each_zone_` 28 | */ 29 | explicit SensorConfig(const string &sensor_name) { 30 | cout << "\033[1;32mTarget Sensor: " << sensor_name << "\033[0m" << endl; 31 | if (sensor_name == "VLP-16") { // For Kimera-Multi dataset 32 | // https://www.amtechs.co.jp/product/VLP-16-Puck.pdf 33 | num_laser_channels_per_zone_ = {{2, 1}, {1, 1}, {1, 1}, {1}}; 34 | num_sectors_for_each_zone_ = {16, 32, 56, 32}; 35 | lower_fov_boundary_ = -15; 36 | vertical_angular_resolution_ = 2.0; 37 | // https://github.com/TixiaoShan/LIO-SAM/blob/master/config/params.yaml 38 | horizontal_resolution_ = 1800; 39 | num_channels_ = 16; 40 | } else if (sensor_name == "HDL-32E") { 41 | // https://velodynelidar.com/wp-content/uploads/2019/12/97-0038-Rev-N-97-0038-DATASHEETWEBHDL32E_Web.pdf 42 | num_laser_channels_per_zone_ = {{10, 5}, {3, 2, 1, 1}, {1, 1, 1}, {1, 1, 1}}; 43 | num_sectors_for_each_zone_ = {16, 32, 56, 32}; 44 | lower_fov_boundary_ = -30.67; 45 | vertical_angular_resolution_ = 1.33; 46 | horizontal_resolution_ = 1080; 47 | num_channels_ = 32; 48 | } else if (sensor_name == "HDL-64E") { 49 | num_laser_channels_per_zone_ = {{24, 12}, {4, 3, 2, 2}, {2, 2, 2, 1}, {1, 1, 1, 1, 1}}; 50 | num_sectors_for_each_zone_ = {16, 32, 56, 32}; 51 | lower_fov_boundary_ = -24.8; 52 | vertical_angular_resolution_ = 0.4; 53 | // https://github.com/TixiaoShan/LIO-SAM/blob/master/config/params.yaml 54 | horizontal_resolution_ = 1800; 55 | num_channels_ = 64; 56 | } else if (sensor_name == "OS1-16") { // For NTU_VIRAL dataset 57 | // https://www.dataspeedinc.com/app/uploads/2019/10/Ouster-OS1-Datasheet.pdf 58 | // But not work in NTU-VIRAL dataset, haha 59 | num_laser_channels_per_zone_ = {{2, 1}, {1, 1}, {1}, {1}}; 60 | num_sectors_for_each_zone_ = {16, 32, 56, 32}; 61 | lower_fov_boundary_ = -16.6; 62 | vertical_angular_resolution_ = 2.075; 63 | // There are three main options: 512, 1024, 2048 64 | // https://github.com/TixiaoShan/LIO-SAM/blob/master/config/params.yaml 65 | horizontal_resolution_ = 1024; 66 | num_channels_ = 16; 67 | // It's not thorough, yet once the cloud points are sufficiently dense, 68 | // then it's fine 69 | } else if (sensor_name == "OS1-64" || sensor_name == "OS1-128") { 70 | num_laser_channels_per_zone_ = {{12, 6}, {3, 2, 1, 1}, {1, 1, 1}, {1, 1, 1}}; 71 | num_sectors_for_each_zone_ = {16, 32, 56, 32}; 72 | lower_fov_boundary_ = -22.5; 73 | vertical_angular_resolution_ = 0.7; 74 | horizontal_resolution_ = 1024; 75 | num_channels_ = 64; 76 | } else { 77 | throw invalid_argument("Sensor name is wrong! Please check the parameter"); 78 | } 79 | } 80 | SensorConfig() = default; 81 | }; 82 | 83 | #endif // PATCHWORK_SENSOR_CONFIG_HPP 84 | -------------------------------------------------------------------------------- /include/patchwork/utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | // CLASSES 27 | #define SENSOR_HEIGHT 1.73 28 | 29 | #define UNLABELED 0 30 | #define OUTLIER 1 31 | #define NUM_ALL_CLASSES 34 32 | #define ROAD 40 33 | #define PARKING 44 34 | #define SIDEWALKR 48 35 | #define OTHER_GROUND 49 36 | #define BUILDING 50 37 | #define FENSE 51 38 | #define LANE_MARKING 60 39 | #define VEGETATION 70 40 | #define TERRAIN 72 41 | 42 | #define TRUEPOSITIVE 3 43 | #define TRUENEGATIVE 2 44 | #define FALSEPOSITIVE 1 45 | #define FALSENEGATIVE 0 46 | 47 | int NUM_ZEROS = 5; 48 | 49 | using namespace std; 50 | 51 | double VEGETATION_THR = -SENSOR_HEIGHT * 3 / 4; 52 | /** Euclidean Velodyne coordinate, including intensity and ring number, and 53 | * label. */ 54 | struct PointXYZILID { 55 | PCL_ADD_POINT4D; // quad-word XYZ 56 | float intensity; ///< laser intensity reading 57 | uint16_t label; ///< point label 58 | uint16_t id; 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 60 | } EIGEN_ALIGN16; 61 | 62 | // Register custom point struct according to PCL 63 | POINT_CLOUD_REGISTER_POINT_STRUCT( 64 | PointXYZILID, 65 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, 66 | label, 67 | label)(std::uint16_t, 68 | id, 69 | id)) 70 | 71 | void PointT2XYZI(pcl::PointCloud &src, pcl::PointCloud::Ptr dst) { 72 | dst->points.clear(); 73 | for (const auto &pt : src.points) { 74 | pcl::PointXYZI pt_xyzi; 75 | pt_xyzi.x = pt.x; 76 | pt_xyzi.y = pt.y; 77 | pt_xyzi.z = pt.z; 78 | pt_xyzi.intensity = pt.intensity; 79 | dst->points.push_back(pt_xyzi); 80 | } 81 | } 82 | std::vector outlier_classes = {UNLABELED, OUTLIER}; 83 | std::vector ground_classes = 84 | {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, VEGETATION, TERRAIN}; 85 | std::vector ground_classes_except_terrain = {ROAD, 86 | PARKING, 87 | SIDEWALKR, 88 | OTHER_GROUND, 89 | LANE_MARKING}; 90 | 91 | template 92 | int count_num_ground(const pcl::PointCloud &pc) { 93 | int num_ground = 0; 94 | 95 | std::vector::iterator iter; 96 | 97 | for (auto const &pt : pc.points) { 98 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 99 | if (iter != ground_classes.end()) { // corresponding class is in ground classes 100 | if (pt.label == VEGETATION) { 101 | if (pt.z < VEGETATION_THR) { 102 | num_ground++; 103 | } 104 | } else { 105 | num_ground++; 106 | } 107 | } 108 | } 109 | return num_ground; 110 | } 111 | 112 | std::map set_initial_gt_counts(std::vector >_classes) { 113 | map gt_counts; 114 | for (size_t i = 0; i < gt_classes.size(); ++i) { 115 | gt_counts.insert(pair(gt_classes.at(i), 0)); 116 | } 117 | return gt_counts; 118 | } 119 | 120 | template 121 | std::map count_num_each_class(const pcl::PointCloud &pc) { 122 | auto gt_counts = set_initial_gt_counts(ground_classes); 123 | std::vector::iterator iter; 124 | 125 | for (auto const &pt : pc.points) { 126 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 127 | if (iter != ground_classes.end()) { // corresponding class is in ground classes 128 | if (pt.label == VEGETATION) { 129 | if (pt.z < VEGETATION_THR) { 130 | gt_counts.find(pt.label)->second++; 131 | } 132 | } else { 133 | gt_counts.find(pt.label)->second++; 134 | } 135 | } 136 | } 137 | return gt_counts; 138 | } 139 | 140 | template 141 | int count_num_outliers(const pcl::PointCloud &pc) { 142 | int num_outliers = 0; 143 | 144 | std::vector::iterator iter; 145 | 146 | for (auto const &pt : pc.points) { 147 | iter = std::find(outlier_classes.begin(), outlier_classes.end(), pt.label); 148 | if (iter != outlier_classes.end()) { // corresponding class is in ground classes 149 | num_outliers++; 150 | } 151 | } 152 | return num_outliers; 153 | } 154 | 155 | template 156 | void discern_ground(const pcl::PointCloud &src, 157 | pcl::PointCloud &ground, 158 | pcl::PointCloud &non_ground) { 159 | // Ensure that PointT is PointT because this function only works in the 160 | // SemanticKITTI 161 | if (!std::is_same::value) { 162 | throw invalid_argument( 163 | "This function only supports `PointT`. Not " 164 | "implemented for this point type."); 165 | } 166 | 167 | ground.clear(); 168 | non_ground.clear(); 169 | std::vector::iterator iter; 170 | for (auto const &pt : src.points) { 171 | if (pt.label == UNLABELED || pt.label == OUTLIER) continue; 172 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 173 | if (iter != ground_classes.end()) { // corresponding class is in ground classes 174 | if (pt.label == VEGETATION) { 175 | if (pt.z < VEGETATION_THR) { 176 | ground.push_back(pt); 177 | } else { 178 | non_ground.push_back(pt); 179 | } 180 | } else { 181 | ground.push_back(pt); 182 | } 183 | } else { 184 | non_ground.push_back(pt); 185 | } 186 | } 187 | } 188 | 189 | template 190 | void calculate_precision_recall(const pcl::PointCloud &pc_curr, 191 | pcl::PointCloud &ground_estimated, 192 | double &precision, 193 | double &recall, 194 | bool consider_outliers = true) { 195 | // Ensure that PointT is PointT because this function only works in the 196 | // SemanticKITTI 197 | if (!std::is_same::value) { 198 | throw invalid_argument( 199 | "This function only supports `PointT`. Not " 200 | "implemented for this point type."); 201 | } 202 | 203 | int num_ground_est = ground_estimated.points.size(); 204 | int num_ground_gt = count_num_ground(pc_curr); 205 | int num_TP = count_num_ground(ground_estimated); 206 | if (consider_outliers) { 207 | int num_outliers_est = count_num_outliers(ground_estimated); 208 | precision = static_cast(num_TP) / (num_ground_est - num_outliers_est) * 100; 209 | recall = static_cast(num_TP) / num_ground_gt * 100; 210 | } else { 211 | precision = static_cast(num_TP) / num_ground_est * 100; 212 | recall = static_cast(num_TP) / num_ground_gt * 100; 213 | } 214 | } 215 | 216 | template 217 | void save_all_labels(const pcl::PointCloud &pc, string ABS_DIR, string seq, int count) { 218 | std::string count_str = std::to_string(count); 219 | std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 220 | std::string output_filename = ABS_DIR + "/" + seq + "/" + count_str_padded + ".csv"; 221 | ofstream sc_output(output_filename); 222 | 223 | vector labels(NUM_ALL_CLASSES, 0); 224 | for (auto const &pt : pc.points) { 225 | if (pt.label == 0) 226 | labels[0]++; 227 | else if (pt.label == 1) 228 | labels[1]++; 229 | else if (pt.label == 10) 230 | labels[2]++; 231 | else if (pt.label == 11) 232 | labels[3]++; 233 | else if (pt.label == 13) 234 | labels[4]++; 235 | else if (pt.label == 15) 236 | labels[5]++; 237 | else if (pt.label == 16) 238 | labels[6]++; 239 | else if (pt.label == 18) 240 | labels[7]++; 241 | else if (pt.label == 20) 242 | labels[8]++; 243 | else if (pt.label == 30) 244 | labels[9]++; 245 | else if (pt.label == 31) 246 | labels[10]++; 247 | else if (pt.label == 32) 248 | labels[11]++; 249 | else if (pt.label == 40) 250 | labels[12]++; 251 | else if (pt.label == 44) 252 | labels[13]++; 253 | else if (pt.label == 48) 254 | labels[14]++; 255 | else if (pt.label == 49) 256 | labels[15]++; 257 | else if (pt.label == 50) 258 | labels[16]++; 259 | else if (pt.label == 51) 260 | labels[17]++; 261 | else if (pt.label == 52) 262 | labels[18]++; 263 | else if (pt.label == 60) 264 | labels[19]++; 265 | else if (pt.label == 70) 266 | labels[20]++; 267 | else if (pt.label == 71) 268 | labels[21]++; 269 | else if (pt.label == 72) 270 | labels[22]++; 271 | else if (pt.label == 80) 272 | labels[23]++; 273 | else if (pt.label == 81) 274 | labels[24]++; 275 | else if (pt.label == 99) 276 | labels[25]++; 277 | else if (pt.label == 252) 278 | labels[26]++; 279 | else if (pt.label == 253) 280 | labels[27]++; 281 | else if (pt.label == 254) 282 | labels[28]++; 283 | else if (pt.label == 255) 284 | labels[29]++; 285 | else if (pt.label == 256) 286 | labels[30]++; 287 | else if (pt.label == 257) 288 | labels[31]++; 289 | else if (pt.label == 258) 290 | labels[32]++; 291 | else if (pt.label == 259) 292 | labels[33]++; 293 | } 294 | 295 | for (uint8_t i = 0; i < NUM_ALL_CLASSES; ++i) { 296 | if (i != 33) { 297 | sc_output << labels[i] << ","; 298 | } else { 299 | sc_output << labels[i] << endl; 300 | } 301 | } 302 | sc_output.close(); 303 | } 304 | 305 | template 306 | void save_all_accuracy(const pcl::PointCloud &pc_curr, 307 | pcl::PointCloud &ground_estimated, 308 | string acc_filename, 309 | double &accuracy, 310 | map &pc_curr_gt_counts, 311 | map &g_est_gt_counts) { 312 | // Ensure that PointT is PointT because this function only works in the 313 | // SemanticKITTI 314 | if (!std::is_same::value) { 315 | throw invalid_argument( 316 | "This function only supports `PointT`. Not " 317 | "implemented for this point type."); 318 | } 319 | 320 | // std::cout<<"debug: "<(num_TP + (num_False - num_FP)) / num_total_gt * 100.0; 334 | 335 | pc_curr_gt_counts = count_num_each_class(pc_curr); 336 | g_est_gt_counts = count_num_each_class(ground_estimated); 337 | 338 | // save output 339 | for (auto const &class_id : ground_classes) { 340 | sc_output2 << g_est_gt_counts.find(class_id)->second << "," 341 | << pc_curr_gt_counts.find(class_id)->second << ","; 342 | } 343 | sc_output2 << accuracy << endl; 344 | 345 | sc_output2.close(); 346 | } 347 | 348 | template 349 | void pc2pcdfile(const pcl::PointCloud &TP, 350 | const pcl::PointCloud &FP, 351 | const pcl::PointCloud &FN, 352 | const pcl::PointCloud &TN, 353 | std::string pcd_filename) { 354 | pcl::PointCloud pc_out; 355 | 356 | for (auto const pt : TP.points) { 357 | pcl::PointXYZI pt_est; 358 | pt_est.x = pt.x; 359 | pt_est.y = pt.y; 360 | pt_est.z = pt.z; 361 | pt_est.intensity = TRUEPOSITIVE; 362 | pc_out.points.push_back(pt_est); 363 | } 364 | for (auto const pt : FP.points) { 365 | pcl::PointXYZI pt_est; 366 | pt_est.x = pt.x; 367 | pt_est.y = pt.y; 368 | pt_est.z = pt.z; 369 | pt_est.intensity = FALSEPOSITIVE; 370 | pc_out.points.push_back(pt_est); 371 | } 372 | for (auto const pt : FN.points) { 373 | pcl::PointXYZI pt_est; 374 | pt_est.x = pt.x; 375 | pt_est.y = pt.y; 376 | pt_est.z = pt.z; 377 | pt_est.intensity = FALSENEGATIVE; 378 | pc_out.points.push_back(pt_est); 379 | } 380 | for (auto const pt : TN.points) { 381 | pcl::PointXYZI pt_est; 382 | pt_est.x = pt.x; 383 | pt_est.y = pt.y; 384 | pt_est.z = pt.z; 385 | pt_est.intensity = TRUENEGATIVE; 386 | pc_out.points.push_back(pt_est); 387 | } 388 | pc_out.width = pc_out.points.size(); 389 | pc_out.height = 1; 390 | pcl::io::savePCDFileASCII(pcd_filename, pc_out); 391 | } 392 | 393 | #endif 394 | -------------------------------------------------------------------------------- /include/patchwork/zone_models.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shapelim on 22. 10. 23. 3 | // 4 | 5 | #ifndef INCLUDE_PATCHWORK_ZONE_MODELS_HPP_ 6 | #define INCLUDE_PATCHWORK_ZONE_MODELS_HPP_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "patchwork/sensor_configs.hpp" 15 | 16 | #define INVALID_RING_IDX -1 17 | #define OVERFLOWED_IDX -2 18 | 19 | class ZoneModel { 20 | public: 21 | ZoneModel() {} 22 | 23 | ~ZoneModel() {} 24 | 25 | SensorConfig sensor_config_; 26 | 27 | /* 28 | * Common functions 29 | */ 30 | inline size_t size() const; 31 | // Important! 'virtual' is necessary to be declared in the base class 32 | // + the functions must be declared, i.e. {} is needed 33 | }; 34 | 35 | class ConcentricZoneModel : public ZoneModel { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | ConcentricZoneModel() {} 39 | 40 | ConcentricZoneModel(const std::string &sensor_model, 41 | const double sensor_height, 42 | const float min_range, 43 | const float max_range) { 44 | sensor_config_ = SensorConfig(sensor_model); 45 | num_zones_ = sensor_config_.num_laser_channels_per_zone_.size(); 46 | max_ring_index_in_first_zone = sensor_config_.num_laser_channels_per_zone_[0].size(); 47 | 48 | sensor_height_ = sensor_height; 49 | min_range_ = min_range; 50 | max_range_ = max_range; 51 | 52 | sqr_min_range_ = min_range * min_range; 53 | sqr_max_range_ = max_range * max_range; 54 | 55 | set_concentric_zone_model(); 56 | } 57 | bool is_range_boundary_set_; 58 | size_t max_ring_index_in_first_zone; 59 | size_t num_zones_; 60 | size_t num_total_rings_; 61 | double sensor_height_; 62 | float min_range_; 63 | float max_range_; 64 | float sqr_min_range_; 65 | float sqr_max_range_; 66 | 67 | vector num_sectors_per_ring_; 68 | // sqr: For reducing computational complexity 69 | vector sqr_boundary_ranges_; 70 | // For visualization 71 | vector boundary_ranges_; 72 | vector boundary_ratios_; 73 | 74 | ~ConcentricZoneModel() {} 75 | 76 | inline void set_concentric_zone_model() { 77 | set_num_sectors_for_each_ring(); 78 | // Seting ring boundaries to consider # of laser rings 79 | float smallest_incidence_angle = 90.0 + sensor_config_.lower_fov_boundary_; 80 | // For defensive programming 81 | if (tan(DEG2RAD(smallest_incidence_angle)) * sensor_height_ < min_range_) { 82 | cout << tan(DEG2RAD(smallest_incidence_angle)) * sensor_height_ << " vs " << min_range_ 83 | << endl; 84 | throw invalid_argument( 85 | "\033[1;31m[CZM] The parameter `min_r` is wrong. " 86 | "Check your sensor height or min. range\033[0m"); 87 | } 88 | sanity_check(); 89 | set_sqr_boundary_ranges(sensor_height_, smallest_incidence_angle); 90 | } 91 | 92 | inline void sanity_check() { 93 | string SET_SAME_SIZES_OF_PARAMETERS = 94 | "Some parameters are wrong! the size of parameters should be same"; 95 | 96 | int n_z = num_zones_; 97 | int n_r = sensor_config_.num_laser_channels_per_zone_.size(); 98 | int n_s = sensor_config_.num_sectors_for_each_zone_.size(); 99 | 100 | if ((n_z != n_r) || (n_z != n_s) || (n_r != n_s)) { 101 | throw invalid_argument(SET_SAME_SIZES_OF_PARAMETERS); 102 | } 103 | } 104 | 105 | bool is_boundary_set() { return is_range_boundary_set_; } 106 | inline void set_num_sectors_for_each_ring() { 107 | num_sectors_per_ring_.clear(); 108 | int count = 0; 109 | for (const auto &channel_set : sensor_config_.num_laser_channels_per_zone_) { 110 | for (int j = 0; j < channel_set.size(); ++j) { 111 | num_sectors_per_ring_.push_back(sensor_config_.num_sectors_for_each_zone_[count]); 112 | } 113 | count++; 114 | } 115 | num_total_rings_ = num_sectors_per_ring_.size(); 116 | } 117 | 118 | void set_sqr_boundary_ranges(const double sensor_height, const float smallest_incidence_angle) { 119 | /*** 120 | * Why squared value? 121 | * Because `sqrt` operation requires more computational cost 122 | */ 123 | // sqr (square): For speed-up 124 | // original : For viz polygon 125 | is_range_boundary_set_ = true; 126 | 127 | boundary_ranges_.clear(); 128 | boundary_ranges_.push_back(min_range_); 129 | sqr_boundary_ranges_.clear(); 130 | sqr_boundary_ranges_.push_back(pow(min_range_, 2)); 131 | float incidence_angle = smallest_incidence_angle; 132 | std::cout << "min_range: " << min_range_ << std::endl; 133 | 134 | float incidence_angle_prev = incidence_angle; 135 | int count = 0; 136 | for (int i = 0; i < sensor_config_.num_laser_channels_per_zone_.size(); ++i) { 137 | vector num_channels_per_ring = sensor_config_.num_laser_channels_per_zone_[i]; 138 | for (int j = 0; j < num_channels_per_ring.size(); ++j) { 139 | incidence_angle += static_cast(num_channels_per_ring[j]) * 140 | sensor_config_.vertical_angular_resolution_; 141 | float incidence_angle_w_margin = 142 | incidence_angle + 143 | 0.5 * sensor_config_.vertical_angular_resolution_; // For safety margin 144 | cout << "\033[1;32m" << incidence_angle_w_margin << "\033[0m" << endl; 145 | 146 | // Check whether the angle is over the 90 degrees 147 | float boundary_range; 148 | if (incidence_angle_w_margin >= 90) { 149 | cout << "\033[1;33mIncidence angle is over the 90 deg!!\033[0m" << endl; 150 | cout << "\033[1;33mBins are evenly divided!\033[0m" << endl; 151 | static float denominator = static_cast(num_total_rings_ - count + 1); 152 | static float left_b = boundary_ranges_.back(); 153 | static float right_b = max_range_; 154 | float k = num_total_rings_ - count; 155 | boundary_range = left_b * (denominator - k) / denominator + max_range_ * k / denominator; 156 | } else { 157 | boundary_range = tan(DEG2RAD(incidence_angle_w_margin)) * sensor_height; 158 | incidence_angle_prev = incidence_angle_w_margin; 159 | } 160 | boundary_ranges_.push_back(boundary_range); 161 | sqr_boundary_ranges_.push_back(pow(boundary_range, 2)); 162 | cout << boundary_range << " m " << endl; 163 | ++count; 164 | } 165 | } 166 | float total_diff = boundary_ranges_.back() - min_range_; 167 | for (const auto boundary_range : boundary_ranges_) { 168 | float ratio = (boundary_range - min_range_) / total_diff; 169 | boundary_ratios_.push_back(ratio); 170 | } 171 | 172 | // This part is important! Without this line, a segmentation fault occurs. 173 | // If you want to enlarge the max range, please modify 174 | // `num_laser_channels_per_zone_` in `sensor_configs.hpp`. 175 | // if (boundary_ranges_.back() < max_range_) { 176 | // boundary_ranges_.push_back(max_range_); 177 | // sqr_boundary_ranges_.push_back(max_range_ * max_range_); 178 | // // Just copy the last value 179 | // num_sectors_per_ring_.push_back(num_sectors_per_ring_.back()); 180 | // } 181 | 182 | if (boundary_ranges_.back() < max_range_) { 183 | std::cout << "\033[1;33m" 184 | << "Max range is shrinked: "; 185 | std::cout << max_range_ << " -> " << boundary_ranges_.back() << "\033[0m" << std::endl; 186 | sqr_max_range_ = boundary_ranges_.back() * boundary_ranges_.back(); 187 | max_range_ = boundary_ranges_.back(); 188 | } 189 | } 190 | 191 | inline void cout_params() { 192 | const auto &num_sectors_each_zone_ = sensor_config_.num_sectors_for_each_zone_; 193 | const auto &num_rings_each_zone_ = sensor_config_.num_rings_for_each_zone_; 194 | 195 | std::cout << "Boundary range: "; 196 | for (const auto &range : boundary_ranges_) { 197 | std::cout << range << " "; 198 | } 199 | std::cout << std::endl; 200 | } 201 | 202 | inline float xy2sqr_r(const float &x, const float &y) { return x * x + y * y; } 203 | 204 | inline float xy2theta(const float &x, const float &y) { // 0 ~ 2 * PI 205 | /* 206 | if (y >= 0) { 207 | return atan2(y, x); // 1, 2 quadrant 208 | } else { 209 | return 2 * M_PI + atan2(y, x);// 3, 4 quadrant 210 | } 211 | */ 212 | auto atan_value = atan2(y, x); // EDITED! 213 | return atan_value > 0 ? atan_value : atan_value + 2 * M_PI; // EDITED! 214 | } 215 | 216 | inline int get_ring_idx(const float &x, const float &y) { 217 | static auto &b = sqr_boundary_ranges_; 218 | float sqr_r = xy2sqr_r(x, y); 219 | // Exception for UAVs such as NTU VIRAL dataset 220 | if (sqr_r < b[0]) { 221 | // std::cout << "\033[1;33mInvalid ring idx has come:"; 222 | // std::cout << "(" << sqrt(sqr_r) << " < " << sqrt(b[0]) << 223 | // ")\033[0m" << std::endl; 224 | return INVALID_RING_IDX; 225 | } 226 | if (sqr_r > sqr_max_range_) { 227 | return OVERFLOWED_IDX; 228 | } 229 | 230 | int bound_idx = lower_bound(b.begin(), b.end(), sqr_r) - b.begin(); 231 | // bound_idx: idx whose value is larger than sqr_r 232 | // Thus, b[bound_idx - 1] < sqr_r < b[bound_idx] 233 | // And note that num_rings + 1 = b.size(); thus, minus 1 is needed 234 | return bound_idx - 1; 235 | } 236 | 237 | inline int get_sector_idx(const float &x, const float &y, const int ring_idx) { 238 | float theta = xy2theta(x, y); 239 | int num_sectors = num_sectors_per_ring_[ring_idx]; 240 | float sector_size = 2.0 * M_PI / static_cast(num_sectors); 241 | 242 | // min: for defensive programming 243 | return min(static_cast(theta / sector_size), num_sectors - 1); 244 | } 245 | 246 | inline std::pair get_ring_sector_idx(const float &x, const float &y) { 247 | int ring_idx = get_ring_idx(x, y); 248 | if (ring_idx == INVALID_RING_IDX) { 249 | return std::make_pair(INVALID_RING_IDX, INVALID_RING_IDX); 250 | } 251 | if (ring_idx == OVERFLOWED_IDX) { 252 | return std::make_pair(OVERFLOWED_IDX, OVERFLOWED_IDX); 253 | } 254 | 255 | int sector_idx = get_sector_idx(x, y, ring_idx); 256 | return std::make_pair(ring_idx, sector_idx); 257 | } 258 | }; 259 | 260 | #endif // INCLUDE_PATCHWORK_ZONE_MODELS_HPP_ 261 | -------------------------------------------------------------------------------- /include/tools/kitti_loader.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shapelim on 6/23/21. 3 | // 4 | #include 5 | #include 6 | #include 7 | 8 | #include "patchwork/utils.hpp" 9 | 10 | #ifndef PATCHWORK_PCD_LOADER_HPP 11 | #define PATCHWORK_PCD_LOADER_HPP 12 | 13 | namespace fs = std::filesystem; 14 | 15 | class KittiLoader { 16 | public: 17 | explicit KittiLoader(const std::string &abs_path) { 18 | pc_path_ = abs_path + "/velodyne"; 19 | label_path_ = abs_path + "/labels"; 20 | 21 | for (num_frames_ = 0;; num_frames_++) { 22 | std::stringstream ss; 23 | ss << pc_path_ << "/" << std::setfill('0') << std::setw(6) << num_frames_ << ".bin"; 24 | std::string filename = ss.str(); 25 | if (!fs::exists(filename)) { 26 | break; 27 | } 28 | } 29 | int num_labels; 30 | for (num_labels = 0;; num_labels++) { 31 | std::stringstream ss; 32 | ss << label_path_ << "/" << std::setfill('0') << std::setw(6) << num_labels << ".label"; 33 | std::string filename = ss.str(); 34 | if (!fs::exists(filename)) { 35 | break; 36 | } 37 | } 38 | 39 | if (num_frames_ == 0) { 40 | std::cerr << "\033[1;31mError: No files in " << pc_path_ << "\033[0m" << std::endl; 41 | } 42 | if (num_frames_ != num_labels) { 43 | std::cerr << "\033[1;31mError: The # of point clouds and # of labels are " 44 | "not same\033[0m" 45 | << std::endl; 46 | } 47 | std::cout << "Total " << num_frames_ << " files are loaded" << std::endl; 48 | } 49 | 50 | ~KittiLoader() {} 51 | 52 | size_t size() const { return num_frames_; } 53 | 54 | template 55 | void get_cloud(size_t idx, pcl::PointCloud &cloud) const { 56 | std::stringstream ss; 57 | ss << pc_path_ << "/" << std::setfill('0') << std::setw(6) << idx << ".bin"; 58 | std::string filename = ss.str(); 59 | FILE *file = fopen(filename.c_str(), "rb"); 60 | if (!file) { 61 | throw invalid_argument("Could not open the .bin file!"); 62 | } 63 | std::vector buffer(3000000); 64 | size_t num_points = 65 | fread(reinterpret_cast(buffer.data()), sizeof(float), buffer.size(), file) / 4; 66 | fclose(file); 67 | 68 | cloud.points.resize(num_points); 69 | if (std::is_same::value) { 70 | for (int i = 0; i < num_points; i++) { 71 | auto &pt = cloud.at(i); 72 | pt.x = buffer[i * 4]; 73 | pt.y = buffer[i * 4 + 1]; 74 | pt.z = buffer[i * 4 + 2]; 75 | } 76 | } else if (std::is_same::value) { 77 | for (int i = 0; i < num_points; i++) { 78 | auto &pt = cloud.at(i); 79 | pt.x = buffer[i * 4]; 80 | pt.y = buffer[i * 4 + 1]; 81 | pt.z = buffer[i * 4 + 2]; 82 | pt.intensity = buffer[i * 4 + 3]; 83 | } 84 | } else if (std::is_same::value) { 85 | std::stringstream ss; 86 | ss << label_path_ << "/" << std::setfill('0') << std::setw(6) << idx << ".label"; 87 | std::string label_name = ss.str(); 88 | // 89 | // std::cout << label_name << std::endl; 90 | std::ifstream label_input(label_name, std::ios::binary); 91 | if (!label_input.is_open()) { 92 | throw invalid_argument("Could not open the label!"); 93 | } 94 | label_input.seekg(0, std::ios::beg); 95 | std::vector labels(num_points); 96 | label_input.read(reinterpret_cast(&labels[0]), num_points * sizeof(uint32_t)); 97 | 98 | for (size_t i = 0; i < num_points; i++) { 99 | auto &pt = cloud.at(i); 100 | pt.x = buffer[i * 4]; 101 | pt.y = buffer[i * 4 + 1]; 102 | pt.z = buffer[i * 4 + 2]; 103 | pt.intensity = buffer[i * 4 + 3]; 104 | pt.label = labels[i] & 0xFFFF; 105 | pt.id = labels[i] >> 16; 106 | } 107 | } 108 | } 109 | 110 | private: 111 | int num_frames_; 112 | std::string label_path_; 113 | std::string pc_path_; 114 | }; 115 | 116 | #endif // PATCHWORK_PCD_LOADER_HPP 117 | -------------------------------------------------------------------------------- /include/tools/pcd_loader.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shapelim on 6/23/21. 3 | // 4 | #ifndef INCLUDE_TOOLS_PCD_LOADER_HPP_ 5 | #define INCLUDE_TOOLS_PCD_LOADER_HPP_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace fs = std::filesystem; 16 | 17 | template 18 | class PcdLoader { 19 | public: 20 | explicit PcdLoader(const std::string &pcd_path) : pcd_path_(pcd_path) { 21 | for (num_frames_ = 0;; num_frames_++) { 22 | std::stringstream ss; 23 | ss << pcd_path_ << "/" << std::setfill('0') << std::setw(6) << num_frames_ << ".pcd"; 24 | std::string filename = ss.str(); 25 | if (!fs::exists(filename)) { 26 | break; 27 | } 28 | } 29 | 30 | if (num_frames_ == 0) { 31 | std::cerr << "error: no files in " << pcd_path << std::endl; 32 | } 33 | } 34 | 35 | ~PcdLoader() {} 36 | 37 | size_t size() const { return num_frames_; } 38 | 39 | typename pcl::PointCloud::ConstPtr cloud(size_t i) const { 40 | std::stringstream ss; 41 | ss << pcd_path_ << "/" << std::setfill('0') << std::setw(6) << i << ".pcd"; 42 | std::string filename = ss.str(); 43 | FILE *file = fopen(filename.c_str(), "rb"); 44 | if (!file) { 45 | std::cerr << "error: failed to load " << filename << std::endl; 46 | return nullptr; 47 | } 48 | 49 | std::vector buffer(1000000); 50 | size_t num_points = 51 | fread(reinterpret_cast(buffer.data()), sizeof(float), buffer.size(), file) / 4; 52 | fclose(file); 53 | 54 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 55 | cloud->resize(num_points); 56 | 57 | for (int i = 0; i < num_points; i++) { 58 | auto &pt = cloud->at(i); 59 | pt.x = buffer[i * 4]; 60 | pt.y = buffer[i * 4 + 1]; 61 | pt.z = buffer[i * 4 + 2]; 62 | // pt.intensity = buffer[i * 4 + 3]; 63 | } 64 | 65 | return cloud; 66 | } 67 | 68 | private: 69 | int num_frames_; 70 | std::string pcd_path_; 71 | }; 72 | #endif // INCLUDE_TOOLS_PCD_LOADER_HPP_ 73 | -------------------------------------------------------------------------------- /include/tools/tictoc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace patchwork { 8 | class TicToc { 9 | public: 10 | TicToc() { tic(); } 11 | 12 | // Reset the timer 13 | void tic() { start_ = std::chrono::steady_clock::now(); } 14 | 15 | // Return elapsed time in milliseconds or seconds 16 | double toc(const std::string& unit = "sec") const { 17 | const auto end = std::chrono::steady_clock::now(); 18 | std::chrono::duration elapsed_seconds = end - start_; 19 | 20 | if (unit == "msec") { 21 | return elapsed_seconds.count() * 1000.0; 22 | } else if (unit == "sec") { 23 | return elapsed_seconds.count(); 24 | } else { 25 | throw std::invalid_argument("Unsupported unit: " + unit + ". Use 'msec' or 'sec'."); 26 | } 27 | } 28 | 29 | private: 30 | std::chrono::time_point start_; 31 | }; 32 | } // namespace patchwork 33 | -------------------------------------------------------------------------------- /launch/evaluate.launch.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | launch: 3 | - arg: {name: start_rviz, default: "true", description: automatically start rviz} 4 | - arg: {name: namespace, default: "patchwork", description: Namespace for multiple run } 5 | 6 | - arg: {name: evaluate_semantickitti, default: "true", description: "If true, runs offline evaluation using SemanticKITTI dataset instead of subscribing to live ROS2 topics" } 7 | - arg: {name: save_flag, default: "false", description: "Whether to save evaluation outputs such as accuracy metrics and .label or .pcd files" } 8 | - arg: {name: use_sor_before_save, default: "false", description: "Whether to apply Statistical Outlier Removal (SOR)" } 9 | - arg: {name: dataset_path, default: "", description: "Absolute path to the SemanticKITTI dataset root directory (e.g., /path/to/sequences/00)" } 10 | 11 | - node: 12 | namespace: $(var namespace) 13 | pkg: patchwork 14 | exec: run_patchwork 15 | name: patchwork 16 | output: screen 17 | on_exit: shutdown 18 | 19 | param: 20 | - name: "evaluate_semantickitti" 21 | value: "$(var evaluate_semantickitti)" 22 | - name: "save_flag" 23 | value: "$(var save_flag)" 24 | - name: "use_sor_before_save" 25 | value: "$(var use_sor_before_save)" 26 | - name: "dataset_path" 27 | value: "$(var dataset_path)" 28 | 29 | - from: $(find-pkg-share patchwork)/config/velodyne64.yaml 30 | 31 | - node: 32 | if: $(var start_rviz) 33 | pkg: rviz2 34 | exec: rviz2 35 | name: "rviz" 36 | launch-prefix: "nice" 37 | output: "screen" 38 | args: > 39 | -d $(find-pkg-share patchwork)/rviz/semantickitti.rviz 40 | -------------------------------------------------------------------------------- /launch/ros1/kitti_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | "patchwork" 3 | "00" 4 | "/media/shapelim/UX960NVMe11/kitti_semantic/dataset/sequences/00" 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/ros1/offline_ouster128.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | "patchwork" 7 | "pcd" 8 | "/home/shapelim/catkin_ws/src/patchwork/materials" 9 | false 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/ros1/pub_for_legoloam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "patchwork" 5 | "00" 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/ros1/run_patchwork.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/ros1/run_patchwork_kimera_multi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/run_patchwork.launch.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | launch: 3 | - arg: {name: start_rviz, default: "true", description: automatically start rviz} 4 | - arg: {name: rviz_path, default: $(find-pkg-share patchwork)/rviz/patchwork.rviz} 5 | - arg: {name: namespace, default: "patchwork", description: Namespace for multiple run } 6 | - arg: {name: scan_topic, default: "/cloud_registered"} 7 | - arg: {name: sensor_type, default: "velodyne64"} 8 | 9 | - arg: {name: verbose, default: "false"} 10 | - arg: {name: visualize, default: "false"} 11 | 12 | - node: 13 | namespace: $(var namespace) 14 | pkg: patchwork 15 | exec: run_patchwork 16 | name: patchwork 17 | output: screen 18 | on_exit: shutdown 19 | 20 | remap: 21 | - { from: "/patchwork/input_cloud", to: '$(var scan_topic)' } 22 | 23 | param: 24 | - name: "verbose" 25 | value: "$(var verbose)" 26 | - name: "visualize" 27 | value: "$(var visualize)" 28 | 29 | - from: $(find-pkg-share patchwork)/config/$(var sensor_type).yaml 30 | 31 | - node: 32 | if: $(var start_rviz) 33 | pkg: rviz2 34 | exec: rviz2 35 | name: "rviz" 36 | launch-prefix: "nice" 37 | output: "screen" 38 | args: > 39 | -d $(var rviz_path) 40 | -------------------------------------------------------------------------------- /materials/000000.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/materials/000000.pcd -------------------------------------------------------------------------------- /materials/000001.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/materials/000001.pcd -------------------------------------------------------------------------------- /materials/000002.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LimHyungTae/patchwork/00672c83ca75d0b4a598cd430cfa97393a460059/materials/000002.pcd -------------------------------------------------------------------------------- /msg/ground_estimate.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | sensor_msgs/PointCloud2 curr 3 | sensor_msgs/PointCloud2 ground # estimate 4 | -------------------------------------------------------------------------------- /msg/node.msg: -------------------------------------------------------------------------------- 1 | int32 idx 2 | std_msgs/Header header 3 | geometry_msgs/Pose pose 4 | sensor_msgs/PointCloud2 lidar 5 | sensor_msgs/PointCloud2 ground_estimate # estimate 6 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | patchwork 4 | 1.0.0 5 | patchwork ROS2 package 6 | 7 | Hyungtae Lim 8 | 9 | 10 | 11 | 12 | 13 | MIT 14 | 15 | rcutils 16 | rclcpp 17 | rclcpp_components 18 | geometry_msgs 19 | nav_msgs 20 | std_msgs 21 | sensor_msgs 22 | visualization_msgs 23 | tf2_ros 24 | tf2_eigen 25 | tf2_geometry_msgs 26 | tf2_sensor_msgs 27 | pcl_ros 28 | pcl_conversions 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /rviz/acl_jackal2.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Labeled cloud1 11 | Splitter Ratio: 0.5 12 | Tree Height: 725 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Labeled cloud 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 10 36 | Class: rviz_default_plugins/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 5 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Autocompute Intensity Bounds: false 54 | Autocompute Value Bounds: 55 | Max Value: 10 56 | Min Value: -10 57 | Value: true 58 | Axis: Z 59 | Channel Name: label 60 | Class: rviz_default_plugins/PointCloud2 61 | Color: 255; 255; 255 62 | Color Transformer: Intensity 63 | Decay Time: 0 64 | Enabled: true 65 | Invert Rainbow: false 66 | Max Color: 255; 255; 255 67 | Max Intensity: 1 68 | Min Color: 0; 0; 0 69 | Min Intensity: 0 70 | Name: Labeled cloud 71 | Position Transformer: XYZ 72 | Selectable: true 73 | Size (Pixels): 3 74 | Size (m): 0.009999999776482582 75 | Style: Points 76 | Topic: 77 | Depth: 5 78 | Durability Policy: Volatile 79 | History Policy: Keep Last 80 | Reliability Policy: Reliable 81 | Value: /acl_jackal2/cloud_groundseg_lidar 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | - Alpha: 1 86 | Autocompute Intensity Bounds: true 87 | Autocompute Value Bounds: 88 | Max Value: 10 89 | Min Value: -10 90 | Value: true 91 | Axis: Z 92 | Channel Name: intensity 93 | Class: rviz_default_plugins/PointCloud2 94 | Color: 20; 200; 255 95 | Color Transformer: FlatColor 96 | Decay Time: 0 97 | Enabled: true 98 | Invert Rainbow: false 99 | Max Color: 255; 255; 255 100 | Max Intensity: 4096 101 | Min Color: 0; 0; 0 102 | Min Intensity: 0 103 | Name: Ground 104 | Position Transformer: XYZ 105 | Selectable: true 106 | Size (Pixels): 3 107 | Size (m): 0.30000001192092896 108 | Style: Points 109 | Topic: 110 | Depth: 5 111 | Durability Policy: Volatile 112 | History Policy: Keep Last 113 | Reliability Policy: Reliable 114 | Value: /acl_jackal2/ground 115 | Use Fixed Frame: true 116 | Use rainbow: true 117 | Value: true 118 | - Alpha: 1 119 | Autocompute Intensity Bounds: true 120 | Autocompute Value Bounds: 121 | Max Value: 10 122 | Min Value: -10 123 | Value: true 124 | Axis: Z 125 | Channel Name: intensity 126 | Class: rviz_default_plugins/PointCloud2 127 | Color: 255; 160; 0 128 | Color Transformer: FlatColor 129 | Decay Time: 0 130 | Enabled: true 131 | Invert Rainbow: false 132 | Max Color: 255; 255; 255 133 | Max Intensity: 4096 134 | Min Color: 0; 0; 0 135 | Min Intensity: 0 136 | Name: Non-ground 137 | Position Transformer: XYZ 138 | Selectable: true 139 | Size (Pixels): 3 140 | Size (m): 0.009999999776482582 141 | Style: Points 142 | Topic: 143 | Depth: 5 144 | Durability Policy: Volatile 145 | History Policy: Keep Last 146 | Reliability Policy: Reliable 147 | Value: /acl_jackal2/non_ground 148 | Use Fixed Frame: true 149 | Use rainbow: true 150 | Value: true 151 | Enabled: true 152 | Global Options: 153 | Background Color: 0; 0; 0 154 | Fixed Frame: acl_jackal2/velodyne_link 155 | Frame Rate: 30 156 | Name: root 157 | Tools: 158 | - Class: rviz_default_plugins/Interact 159 | Hide Inactive Objects: true 160 | - Class: rviz_default_plugins/MoveCamera 161 | - Class: rviz_default_plugins/Select 162 | - Class: rviz_default_plugins/FocusCamera 163 | - Class: rviz_default_plugins/Measure 164 | Line color: 128; 128; 0 165 | - Class: rviz_default_plugins/SetInitialPose 166 | Covariance x: 0.25 167 | Covariance y: 0.25 168 | Covariance yaw: 0.06853891909122467 169 | Topic: 170 | Depth: 5 171 | Durability Policy: Volatile 172 | History Policy: Keep Last 173 | Reliability Policy: Reliable 174 | Value: /initialpose 175 | - Class: rviz_default_plugins/SetGoal 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | History Policy: Keep Last 180 | Reliability Policy: Reliable 181 | Value: /goal_pose 182 | - Class: rviz_default_plugins/PublishPoint 183 | Single click: true 184 | Topic: 185 | Depth: 5 186 | Durability Policy: Volatile 187 | History Policy: Keep Last 188 | Reliability Policy: Reliable 189 | Value: /clicked_point 190 | Transformation: 191 | Current: 192 | Class: rviz_default_plugins/TF 193 | Value: true 194 | Views: 195 | Current: 196 | Class: rviz_default_plugins/Orbit 197 | Distance: 56.36219787597656 198 | Enable Stereo Rendering: 199 | Stereo Eye Separation: 0.05999999865889549 200 | Stereo Focal Distance: 1 201 | Swap Stereo Eyes: false 202 | Value: false 203 | Focal Point: 204 | X: 1.1543935537338257 205 | Y: -0.7959945797920227 206 | Z: -0.22548089921474457 207 | Focal Shape Fixed Size: true 208 | Focal Shape Size: 0.05000000074505806 209 | Invert Z Axis: false 210 | Name: Current View 211 | Near Clip Distance: 0.009999999776482582 212 | Pitch: 0.7953980565071106 213 | Target Frame: 214 | Value: Orbit (rviz) 215 | Yaw: 0.73539799451828 216 | Saved: ~ 217 | Window Geometry: 218 | Displays: 219 | collapsed: false 220 | Height: 1029 221 | Hide Left Dock: false 222 | Hide Right Dock: false 223 | QMainWindow State: 000000ff00000000fd0000000400000000000001d900000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000363000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000363000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007700000003efc0100000002fb0000000800540069006d00650100000000000007700000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000047c0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 224 | Selection: 225 | collapsed: false 226 | Time: 227 | collapsed: false 228 | Tool Properties: 229 | collapsed: false 230 | Views: 231 | collapsed: false 232 | Width: 1904 233 | X: 4292 234 | Y: 560 235 | -------------------------------------------------------------------------------- /rviz/patchwork.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Labeled cloud1/Status1 11 | - /Ground1 12 | - /Non-ground1 13 | Splitter Ratio: 0.5 14 | Tree Height: 725 15 | - Class: rviz_common/Selection 16 | Name: Selection 17 | - Class: rviz_common/Tool Properties 18 | Expanded: 19 | - /2D Goal Pose1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz_common/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz_common/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: Labeled cloud 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 10 38 | Class: rviz_default_plugins/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 5 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Autocompute Intensity Bounds: false 56 | Autocompute Value Bounds: 57 | Max Value: 10 58 | Min Value: -10 59 | Value: true 60 | Axis: Z 61 | Channel Name: intensity 62 | Class: rviz_default_plugins/PointCloud2 63 | Color: 255; 255; 255 64 | Color Transformer: Intensity 65 | Decay Time: 0 66 | Enabled: true 67 | Invert Rainbow: false 68 | Max Color: 255; 255; 255 69 | Max Intensity: 1 70 | Min Color: 0; 0; 0 71 | Min Intensity: 0 72 | Name: Labeled cloud 73 | Position Transformer: XYZ 74 | Selectable: true 75 | Size (Pixels): 3 76 | Size (m): 0.009999999776482582 77 | Style: Flat Squares 78 | Topic: 79 | Depth: 5 80 | Durability Policy: Volatile 81 | History Policy: Keep Last 82 | Reliability Policy: Reliable 83 | Value: /labeled_cloud 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | - Alpha: 1 88 | Autocompute Intensity Bounds: true 89 | Autocompute Value Bounds: 90 | Max Value: 10 91 | Min Value: -10 92 | Value: true 93 | Axis: Z 94 | Channel Name: intensity 95 | Class: rviz_default_plugins/PointCloud2 96 | Color: 20; 200; 255 97 | Color Transformer: FlatColor 98 | Decay Time: 0 99 | Enabled: true 100 | Invert Rainbow: false 101 | Max Color: 255; 255; 255 102 | Max Intensity: 4096 103 | Min Color: 0; 0; 0 104 | Min Intensity: 0 105 | Name: Ground 106 | Position Transformer: XYZ 107 | Selectable: true 108 | Size (Pixels): 3 109 | Size (m): 0.30000001192092896 110 | Style: Points 111 | Topic: 112 | Depth: 5 113 | Durability Policy: Volatile 114 | History Policy: Keep Last 115 | Reliability Policy: Reliable 116 | Value: /patchwork/ground 117 | Use Fixed Frame: true 118 | Use rainbow: true 119 | Value: true 120 | - Alpha: 1 121 | Autocompute Intensity Bounds: true 122 | Autocompute Value Bounds: 123 | Max Value: 10 124 | Min Value: -10 125 | Value: true 126 | Axis: Z 127 | Channel Name: intensity 128 | Class: rviz_default_plugins/PointCloud2 129 | Color: 255; 160; 0 130 | Color Transformer: FlatColor 131 | Decay Time: 0 132 | Enabled: true 133 | Invert Rainbow: false 134 | Max Color: 255; 255; 255 135 | Max Intensity: 4096 136 | Min Color: 0; 0; 0 137 | Min Intensity: 0 138 | Name: Non-ground 139 | Position Transformer: XYZ 140 | Selectable: true 141 | Size (Pixels): 3 142 | Size (m): 0.009999999776482582 143 | Style: Points 144 | Topic: 145 | Depth: 5 146 | Durability Policy: Volatile 147 | History Policy: Keep Last 148 | Reliability Policy: Reliable 149 | Value: /patchwork/non_ground 150 | Use Fixed Frame: true 151 | Use rainbow: true 152 | Value: true 153 | Enabled: true 154 | Global Options: 155 | Background Color: 0; 0; 0 156 | Fixed Frame: acl_jackal2/velodyne_link 157 | Frame Rate: 30 158 | Name: root 159 | Tools: 160 | - Class: rviz_default_plugins/Interact 161 | Hide Inactive Objects: true 162 | - Class: rviz_default_plugins/MoveCamera 163 | - Class: rviz_default_plugins/Select 164 | - Class: rviz_default_plugins/FocusCamera 165 | - Class: rviz_default_plugins/Measure 166 | Line color: 128; 128; 0 167 | - Class: rviz_default_plugins/SetInitialPose 168 | Covariance x: 0.25 169 | Covariance y: 0.25 170 | Covariance yaw: 0.06853891909122467 171 | Topic: 172 | Depth: 5 173 | Durability Policy: Volatile 174 | History Policy: Keep Last 175 | Reliability Policy: Reliable 176 | Value: /initialpose 177 | - Class: rviz_default_plugins/SetGoal 178 | Topic: 179 | Depth: 5 180 | Durability Policy: Volatile 181 | History Policy: Keep Last 182 | Reliability Policy: Reliable 183 | Value: /goal_pose 184 | - Class: rviz_default_plugins/PublishPoint 185 | Single click: true 186 | Topic: 187 | Depth: 5 188 | Durability Policy: Volatile 189 | History Policy: Keep Last 190 | Reliability Policy: Reliable 191 | Value: /clicked_point 192 | Transformation: 193 | Current: 194 | Class: rviz_default_plugins/TF 195 | Value: true 196 | Views: 197 | Current: 198 | Class: rviz_default_plugins/Orbit 199 | Distance: 56.36219787597656 200 | Enable Stereo Rendering: 201 | Stereo Eye Separation: 0.05999999865889549 202 | Stereo Focal Distance: 1 203 | Swap Stereo Eyes: false 204 | Value: false 205 | Focal Point: 206 | X: 1.1543935537338257 207 | Y: -0.7959945797920227 208 | Z: -0.22548089921474457 209 | Focal Shape Fixed Size: true 210 | Focal Shape Size: 0.05000000074505806 211 | Invert Z Axis: false 212 | Name: Current View 213 | Near Clip Distance: 0.009999999776482582 214 | Pitch: 0.7953980565071106 215 | Target Frame: 216 | Value: Orbit (rviz) 217 | Yaw: 0.73539799451828 218 | Saved: ~ 219 | Window Geometry: 220 | Displays: 221 | collapsed: false 222 | Height: 1029 223 | Hide Left Dock: false 224 | Hide Right Dock: false 225 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000363000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000363000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007700000003efc0100000002fb0000000800540069006d00650100000000000007700000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000004ff0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 226 | Selection: 227 | collapsed: false 228 | Time: 229 | collapsed: false 230 | Tool Properties: 231 | collapsed: false 232 | Views: 233 | collapsed: false 234 | Width: 1904 235 | X: 1956 236 | Y: 476 237 | -------------------------------------------------------------------------------- /rviz/semantickitti.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /FN1 11 | - /TP1 12 | - /FP1 13 | Splitter Ratio: 0.5 14 | Tree Height: 542 15 | - Class: rviz_common/Selection 16 | Name: Selection 17 | - Class: rviz_common/Tool Properties 18 | Expanded: 19 | - /2D Goal Pose1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz_common/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz_common/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: FN 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 10 38 | Class: rviz_default_plugins/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Autocompute Intensity Bounds: true 56 | Autocompute Value Bounds: 57 | Max Value: 10 58 | Min Value: -10 59 | Value: true 60 | Axis: Z 61 | Channel Name: intensity 62 | Class: rviz_default_plugins/PointCloud2 63 | Color: 0; 0; 255 64 | Color Transformer: FlatColor 65 | Decay Time: 0 66 | Enabled: true 67 | Invert Rainbow: false 68 | Max Color: 255; 255; 255 69 | Max Intensity: 0.9900000095367432 70 | Min Color: 0; 0; 0 71 | Min Intensity: 0 72 | Name: FN 73 | Position Transformer: XYZ 74 | Selectable: true 75 | Size (Pixels): 4 76 | Size (m): 0.009999999776482582 77 | Style: Points 78 | Topic: 79 | Depth: 5 80 | Durability Policy: Volatile 81 | History Policy: Keep Last 82 | Reliability Policy: Reliable 83 | Value: /patchwork/benchmark/FN 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | - Alpha: 1 88 | Autocompute Intensity Bounds: true 89 | Autocompute Value Bounds: 90 | Max Value: 10 91 | Min Value: -10 92 | Value: true 93 | Axis: Z 94 | Channel Name: intensity 95 | Class: rviz_default_plugins/PointCloud2 96 | Color: 0; 255; 0 97 | Color Transformer: FlatColor 98 | Decay Time: 0 99 | Enabled: true 100 | Invert Rainbow: false 101 | Max Color: 255; 255; 255 102 | Max Intensity: 0.7799999713897705 103 | Min Color: 0; 0; 0 104 | Min Intensity: 0 105 | Name: TP 106 | Position Transformer: XYZ 107 | Selectable: true 108 | Size (Pixels): 4 109 | Size (m): 0.009999999776482582 110 | Style: Points 111 | Topic: 112 | Depth: 5 113 | Durability Policy: Volatile 114 | History Policy: Keep Last 115 | Reliability Policy: Reliable 116 | Value: /patchwork/benchmark/TP 117 | Use Fixed Frame: true 118 | Use rainbow: true 119 | Value: true 120 | - Alpha: 1 121 | Autocompute Intensity Bounds: true 122 | Autocompute Value Bounds: 123 | Max Value: 10 124 | Min Value: -10 125 | Value: true 126 | Axis: Z 127 | Channel Name: intensity 128 | Class: rviz_default_plugins/PointCloud2 129 | Color: 255; 0; 0 130 | Color Transformer: FlatColor 131 | Decay Time: 0 132 | Enabled: true 133 | Invert Rainbow: false 134 | Max Color: 255; 255; 255 135 | Max Intensity: 0.7799999713897705 136 | Min Color: 0; 0; 0 137 | Min Intensity: 0 138 | Name: FP 139 | Position Transformer: XYZ 140 | Selectable: true 141 | Size (Pixels): 4 142 | Size (m): 0.009999999776482582 143 | Style: Points 144 | Topic: 145 | Depth: 5 146 | Durability Policy: Volatile 147 | History Policy: Keep Last 148 | Reliability Policy: Reliable 149 | Value: /patchwork/benchmark/FP 150 | Use Fixed Frame: true 151 | Use rainbow: true 152 | Value: true 153 | Enabled: true 154 | Global Options: 155 | Background Color: 0; 0; 0 156 | Fixed Frame: velodyne_link 157 | Frame Rate: 30 158 | Name: root 159 | Tools: 160 | - Class: rviz_default_plugins/Interact 161 | Hide Inactive Objects: true 162 | - Class: rviz_default_plugins/MoveCamera 163 | - Class: rviz_default_plugins/Select 164 | - Class: rviz_default_plugins/FocusCamera 165 | - Class: rviz_default_plugins/Measure 166 | Line color: 128; 128; 0 167 | - Class: rviz_default_plugins/SetInitialPose 168 | Covariance x: 0.25 169 | Covariance y: 0.25 170 | Covariance yaw: 0.06853891909122467 171 | Topic: 172 | Depth: 5 173 | Durability Policy: Volatile 174 | History Policy: Keep Last 175 | Reliability Policy: Reliable 176 | Value: /initialpose 177 | - Class: rviz_default_plugins/SetGoal 178 | Topic: 179 | Depth: 5 180 | Durability Policy: Volatile 181 | History Policy: Keep Last 182 | Reliability Policy: Reliable 183 | Value: /goal_pose 184 | - Class: rviz_default_plugins/PublishPoint 185 | Single click: true 186 | Topic: 187 | Depth: 5 188 | Durability Policy: Volatile 189 | History Policy: Keep Last 190 | Reliability Policy: Reliable 191 | Value: /clicked_point 192 | Transformation: 193 | Current: 194 | Class: rviz_default_plugins/TF 195 | Value: true 196 | Views: 197 | Current: 198 | Class: rviz_default_plugins/Orbit 199 | Distance: 118.07080078125 200 | Enable Stereo Rendering: 201 | Stereo Eye Separation: 0.05999999865889549 202 | Stereo Focal Distance: 1 203 | Swap Stereo Eyes: false 204 | Value: false 205 | Focal Point: 206 | X: -0.048571065068244934 207 | Y: 2.2070000171661377 208 | Z: -0.06272349506616592 209 | Focal Shape Fixed Size: true 210 | Focal Shape Size: 0.05000000074505806 211 | Invert Z Axis: false 212 | Name: Current View 213 | Near Clip Distance: 0.009999999776482582 214 | Pitch: 1.5403972864151 215 | Target Frame: 216 | Value: Orbit (rviz) 217 | Yaw: 3.1354010105133057 218 | Saved: ~ 219 | Window Geometry: 220 | Displays: 221 | collapsed: false 222 | Height: 846 223 | Hide Left Dock: false 224 | Hide Right Dock: false 225 | QMainWindow State: 000000ff00000000fd0000000400000000000001ce000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007470000003efc0100000002fb0000000800540069006d00650100000000000007470000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000045e000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 226 | Selection: 227 | collapsed: false 228 | Time: 229 | collapsed: false 230 | Tool Properties: 231 | collapsed: false 232 | Views: 233 | collapsed: false 234 | Width: 1863 235 | X: 4381 236 | Y: 568 237 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define PCL_NO_PRECOMPILE 9 | // #define EST_GROUND 1 // In general cases 10 | #define EST_GROUND 14 // For adk20k_mit 11 | #define EST_NONGROUND 0 12 | 13 | #include "patchwork/patchwork.hpp" // Your implementation 14 | #include "tools/kitti_loader.hpp" 15 | #include "tools/pcd_loader.hpp" 16 | 17 | using std::placeholders::_1; 18 | using PointType = pcl::PointXYZL; 19 | 20 | using RosPointCloud2 = sensor_msgs::msg::PointCloud2; 21 | 22 | class InterfaceNode : public rclcpp::Node { 23 | public: 24 | std::shared_ptr> patchwork_; 25 | 26 | InterfaceNode() : Node("patchwork_benchmark_node") { 27 | // Declare and get parameters 28 | seq_ = this->declare_parameter("seq", "00"); 29 | acc_filename_ = this->declare_parameter("acc_filename", ""); 30 | pcd_savepath_ = this->declare_parameter("pcd_savepath", ""); 31 | 32 | rclcpp::QoS qos(5); 33 | qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); 34 | qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); 35 | 36 | // Publishers 37 | cloud_pub_ = create_publisher("lidar_out", qos); 38 | ground_pub_ = create_publisher("ground", qos); 39 | nonground_pub_ = create_publisher("non_ground", qos); 40 | labeled_pub_ = create_publisher("cloud_groundseg_lidar", qos); 41 | 42 | // Subscriber 43 | cloud_sub_ = create_subscription( 44 | "/patchwork/input_cloud", 10, std::bind(&InterfaceNode::pointcloudCallback, this, _1)); 45 | 46 | RCLCPP_INFO(this->get_logger(), "Patchwork Benchmark Node Initialized."); 47 | } 48 | 49 | private: 50 | void pointcloudCallback(const RosPointCloud2::SharedPtr msg) { 51 | pcl::PointCloud pc_curr; 52 | pcl::fromROSMsg(*msg, pc_curr); 53 | 54 | pcl::PointCloud pc_ground, pc_non_ground, pc_labeled; 55 | 56 | patchwork_->estimate_ground(pc_curr, pc_ground, pc_non_ground); 57 | const double time_taken = patchwork_->get_time(); 58 | const double hz = 1.0 / time_taken; 59 | 60 | RCLCPP_INFO(this->get_logger(), 61 | "[%s] Time: %.2f ms | Hz: %.2f | Points: %zu -> %zu", 62 | msg->header.frame_id.c_str(), 63 | time_taken * 1000.0, 64 | hz, 65 | pc_curr.size(), 66 | pc_ground.size()); 67 | 68 | // Publish input 69 | RosPointCloud2 out_msg; 70 | pcl::toROSMsg(pc_curr, out_msg); 71 | out_msg.header = msg->header; 72 | cloud_pub_->publish(out_msg); 73 | 74 | ground_pub_->publish(to_msg(pc_ground, msg->header)); 75 | nonground_pub_->publish(to_msg(pc_non_ground, msg->header)); 76 | 77 | for (auto &pt : pc_ground.points) pt.label = EST_GROUND; 78 | for (auto &pt : pc_non_ground.points) pt.label = EST_NONGROUND; 79 | pc_labeled = pc_ground; 80 | pc_labeled.insert(pc_labeled.end(), pc_non_ground.begin(), pc_non_ground.end()); 81 | labeled_pub_->publish(to_msg(pc_labeled, msg->header)); 82 | } 83 | 84 | RosPointCloud2 to_msg(const pcl::PointCloud &cloud, 85 | const std_msgs::msg::Header &header) { 86 | RosPointCloud2 msg; 87 | pcl::toROSMsg(cloud, msg); 88 | msg.header = header; 89 | return msg; 90 | } 91 | 92 | std::string seq_; 93 | std::string acc_filename_; 94 | std::string pcd_savepath_; 95 | std::string data_path_; 96 | 97 | bool evaluate_semantickitti_; 98 | bool save_flag_; 99 | 100 | // ROS 2 publishers and subscriber 101 | rclcpp::Publisher::SharedPtr cloud_pub_; 102 | rclcpp::Publisher::SharedPtr ground_pub_; 103 | rclcpp::Publisher::SharedPtr nonground_pub_; 104 | rclcpp::Publisher::SharedPtr labeled_pub_; 105 | 106 | rclcpp::Subscription::SharedPtr cloud_sub_; 107 | }; 108 | 109 | int main(int argc, char **argv) { 110 | rclcpp::init(argc, argv); 111 | auto node = std::make_shared(); 112 | 113 | bool evaluate_semantickitti = node->declare_parameter("evaluate_semantickitti", false); 114 | bool save_flag = node->declare_parameter("save_flag", false); 115 | bool use_sor_before_save = node->declare_parameter("use_sor_before_save", false); 116 | std::string dataset_path = 117 | node->declare_parameter("dataset_path", "/path/to/SemKITTI"); 118 | std::string seq = fs::path(dataset_path).filename().string(); 119 | 120 | if (evaluate_semantickitti) { 121 | rclcpp::Publisher::SharedPtr tp_pub; 122 | rclcpp::Publisher::SharedPtr fp_pub; 123 | rclcpp::Publisher::SharedPtr fn_pub; 124 | tp_pub = node->create_publisher("benchmark/TP", 1); 125 | fp_pub = node->create_publisher("benchmark/FP", 1); 126 | fn_pub = node->create_publisher("benchmark/FN", 1); 127 | 128 | using PointEvalType = PointXYZILID; 129 | auto patchwork_eval = std::make_shared>(node.get()); 130 | 131 | // Setting data loader and paths to be saved 132 | KittiLoader loader(dataset_path); 133 | const char *home_dir = std::getenv("HOME"); 134 | if (home_dir == nullptr) { 135 | std::cerr << "Error: HOME environment variable not set." << std::endl; 136 | return 1; 137 | } 138 | std::string abs_save_dir = std::string(home_dir) + "/patchwork"; 139 | fs::create_directory(abs_save_dir); 140 | std::string output_filename = abs_save_dir + "/" + seq + ".txt"; 141 | 142 | const int N = loader.size(); 143 | for (int n = 0; rclcpp::ok() && n < N; ++n) { 144 | pcl::PointCloud cloud; 145 | loader.get_cloud(n, cloud); 146 | 147 | pcl::PointCloud ground, non_ground; 148 | patchwork_eval->estimate_ground(cloud, ground, non_ground); 149 | double time_taken = patchwork_eval->get_time(); 150 | 151 | double precision, recall, precision_naive, recall_naive; 152 | calculate_precision_recall(cloud, ground, precision, recall); 153 | calculate_precision_recall(cloud, ground, precision_naive, recall_naive, false); 154 | 155 | RCLCPP_INFO(node->get_logger(), "[%d] Precision: %.2f | Recall: %.2f", n, precision, recall); 156 | 157 | ofstream ground_output(output_filename, ios::app); 158 | ground_output << n << "," << time_taken << "," << precision << "," << recall << "," 159 | << precision_naive << "," << recall_naive; 160 | ground_output << std::endl; 161 | ground_output.close(); 162 | 163 | pcl::PointCloud TP; 164 | pcl::PointCloud FP; 165 | pcl::PointCloud FN; 166 | pcl::PointCloud TN; 167 | discern_ground(ground, TP, FP); 168 | discern_ground(non_ground, FN, TN); 169 | 170 | // if (save_flag) { // To make `.label` file 171 | // if (use_sor_before_save) { 172 | // pcl::PointCloud::Ptr filtered; 173 | // filtered.reset(new pcl::PointCloud()); 174 | // filter_by_sor(ground, *filtered); 175 | // save_ground_label(abs_save_dir, n, cloud, *filtered); 176 | // } else { 177 | // save_ground_label(abs_save_dir, n, cloud, ground); 178 | // } 179 | // } 180 | 181 | const auto curr_time = rclcpp::Clock().now(); 182 | sensor_msgs::msg::PointCloud2 ros_msg; 183 | pcl::toROSMsg(TP, ros_msg); 184 | ros_msg.header.frame_id = "velodyne_link"; 185 | ros_msg.header.stamp = curr_time; 186 | tp_pub->publish(ros_msg); 187 | 188 | pcl::toROSMsg(FP, ros_msg); 189 | ros_msg.header.frame_id = "velodyne_link"; 190 | ros_msg.header.stamp = curr_time; 191 | fp_pub->publish(ros_msg); 192 | 193 | pcl::toROSMsg(FN, ros_msg); 194 | ros_msg.header.frame_id = "velodyne_link"; 195 | ros_msg.header.stamp = curr_time; 196 | fn_pub->publish(ros_msg); 197 | } 198 | 199 | RCLCPP_INFO(node->get_logger(), "Finished SemanticKITTI Evaluation."); 200 | rclcpp::shutdown(); 201 | return 0; 202 | } else { 203 | node->patchwork_ = std::make_shared>(node.get()); 204 | } 205 | 206 | rclcpp::spin(node); 207 | rclcpp::shutdown(); 208 | return 0; 209 | } 210 | -------------------------------------------------------------------------------- /src/offline_own_data.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Hyungtae Lim on 6/23/21. 3 | // 4 | 5 | // For disable PCL complile lib, to use PointXYZILID 6 | #define PCL_NO_PRECOMPILE 7 | #include 8 | 9 | #include 10 | 11 | #include "patchwork/patchwork.hpp" 12 | 13 | using PointType = pcl::PointXYZ; 14 | using namespace std; 15 | 16 | ros::Publisher CloudPublisher; 17 | ros::Publisher PositivePublisher; 18 | ros::Publisher NegativePublisher; 19 | 20 | boost::shared_ptr> PatchworkGroundSeg; 21 | 22 | std::string output_filename; 23 | std::string acc_filename, pcd_savepath; 24 | string algorithm; 25 | string extension; 26 | string file_dir; 27 | string mode; 28 | string seq; 29 | bool save_flag; 30 | 31 | template 32 | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id) { 33 | sensor_msgs::PointCloud2 cloud_ROS; 34 | pcl::toROSMsg(cloud, cloud_ROS); 35 | cloud_ROS.header.frame_id = frame_id; 36 | return cloud_ROS; 37 | } 38 | 39 | void signal_callback_handler(int signum) { 40 | cout << "Caught Ctrl + c " << endl; 41 | // Terminate program 42 | exit(signum); 43 | } 44 | 45 | int count_num_files(const string folder_dir, const string extension) { 46 | int num_frames = 0; 47 | for (num_frames = 0;; num_frames++) { 48 | std::string filename = 49 | (boost::format("%s/%06d.%s") % folder_dir % num_frames % extension).str(); 50 | if (!boost::filesystem::exists(filename)) { 51 | break; 52 | } 53 | } 54 | if (!num_frames) throw invalid_argument("Something is wrong. The # of files is zero."); 55 | 56 | return num_frames; 57 | } 58 | 59 | int main(int argc, char **argv) { 60 | bool stop_each_frame; 61 | ros::init(argc, argv, "Benchmark"); 62 | ros::NodeHandle nh; 63 | condParam(&nh, "/algorithm", algorithm, "patchwork", ""); 64 | condParam(&nh, "/extension", extension, "pcd", ""); 65 | condParam(&nh, "/file_dir", file_dir, "", ""); 66 | condParam(&nh, "/stop_each_frame", stop_each_frame, false, ""); 67 | ros::Rate loop_rate(10); 68 | 69 | PatchworkGroundSeg.reset(new PatchWork(&nh)); 70 | 71 | CloudPublisher = nh.advertise("/benchmark/cloud", 100, true); 72 | PositivePublisher = nh.advertise("/patchwork/ground", 100, true); 73 | NegativePublisher = nh.advertise("/patchwork/non_ground", 100, true); 74 | 75 | cout << "\033[1;32mTarget directory: " << file_dir << endl; 76 | int num_pcds = count_num_files(file_dir, extension); 77 | 78 | for (int i = 0; i < num_pcds; ++i) { 79 | signal(SIGINT, signal_callback_handler); 80 | // An example for Loading own data 81 | pcl::PointCloud pc_curr; 82 | std::string filename = (boost::format("%s/%06d.%s") % file_dir % i % extension).str(); 83 | if (pcl::io::loadPCDFile(filename, pc_curr) == -1) { 84 | PCL_ERROR("Couldn't read file test_pcd.pcd \n"); 85 | return (-1); 86 | } 87 | 88 | pcl::PointCloud pc_ground; 89 | pcl::PointCloud pc_non_ground; 90 | 91 | static double time_taken; 92 | cout << i << "th operation..." << endl; 93 | 94 | PatchworkGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 95 | CloudPublisher.publish(cloud2msg(pc_curr, PatchworkGroundSeg->frame_patchwork)); 96 | PositivePublisher.publish(cloud2msg(pc_ground, PatchworkGroundSeg->frame_patchwork)); 97 | NegativePublisher.publish(cloud2msg(pc_non_ground, PatchworkGroundSeg->frame_patchwork)); 98 | 99 | if (stop_each_frame) { 100 | cout << "STOP!" << endl; 101 | cin.ignore(); 102 | } 103 | } 104 | ros::spin(); 105 | 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /src/pub_for_legoloam.cpp: -------------------------------------------------------------------------------- 1 | // For disable PCL complile lib, to use PointXYZIR 2 | #define PCL_NO_PRECOMPILE 3 | #include 4 | #include 5 | #include 6 | 7 | #include "patchwork/patchwork.hpp" 8 | #include "tools/kitti_loader.hpp" 9 | #include "tools/pcd_loader.hpp" 10 | 11 | using namespace std; 12 | 13 | ros::Publisher CloudPublisher; 14 | ros::Publisher PositivePublisher; 15 | ros::Publisher NegativePublisher; 16 | ros::Publisher EstimatePublisher; 17 | 18 | // using PointType = PointXYZILID; 19 | using PointType = pcl::PointXYZ; 20 | boost::shared_ptr> PatchworkGroundSeg; 21 | 22 | std::string output_filename; 23 | std::string acc_filename, pcd_savepath; 24 | string algorithm; 25 | string mode; 26 | string seq; 27 | bool save_flag; 28 | 29 | template 30 | pcl::PointCloud cloudmsg2cloud(const sensor_msgs::PointCloud2::ConstPtr &cloudmsg) { 31 | pcl::PointCloud cloudresult; 32 | pcl::fromROSMsg(*cloudmsg, cloudresult); 33 | return cloudresult; 34 | } 35 | 36 | template 37 | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud) { 38 | sensor_msgs::PointCloud2 cloud_ROS; 39 | pcl::toROSMsg(cloud, cloud_ROS); 40 | return cloud_ROS; 41 | } 42 | 43 | void callbackNode(const sensor_msgs::PointCloud2::ConstPtr &msg) { 44 | cout << msg->header.seq << "th node come" << endl; 45 | pcl::PointCloud pc_curr = cloudmsg2cloud(msg); 46 | pcl::PointCloud pc_ground; 47 | pcl::PointCloud pc_non_ground; 48 | pc_ground.header = pc_curr.header; 49 | pc_non_ground.header = pc_curr.header; 50 | 51 | static double time_taken; 52 | 53 | cout << "Operating patchwork..." << endl; 54 | PatchworkGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 55 | 56 | auto msg_curr = cloud2msg(pc_curr); 57 | auto msg_ground = cloud2msg(pc_ground); 58 | 59 | patchwork::ground_estimate cloud_estimate; 60 | cloud_estimate.header = msg->header; 61 | cloud_estimate.curr = msg_curr; 62 | cloud_estimate.ground = msg_ground; 63 | EstimatePublisher.publish(cloud_estimate); 64 | 65 | /* 66 | CloudPublisher.publish(cloud2msg(pc_curr)); 67 | PositivePublisher.publish(cloud2msg(pc_ground)); 68 | NegativePublisher.publish(cloud2msg(pc_non_ground)); 69 | */ 70 | } 71 | 72 | int main(int argc, char **argv) { 73 | ros::init(argc, argv, "Benchmark"); 74 | ros::NodeHandle nh; 75 | condParam(&nh, "/algorithm", algorithm, "patchwork", ""); 76 | condParam(&nh, "/seq", seq, "00", ""); 77 | 78 | PatchworkGroundSeg.reset(new PatchWork(&nh)); 79 | 80 | /* Publisher for source cloud, ground, non-ground */ 81 | CloudPublisher = nh.advertise("/benchmark/cloud", 100, true); 82 | PositivePublisher = nh.advertise("/benchmark/P", 100, true); 83 | NegativePublisher = nh.advertise("/benchmark/N", 100, true); 84 | 85 | /* Publisher for combined msg of source cloud, ground cloud */ 86 | EstimatePublisher = 87 | nh.advertise("/benchmark/ground_estimate", 100, true); 88 | 89 | ros::Subscriber NodeSubscriber = 90 | nh.subscribe("/node", 5000, callbackNode); 91 | 92 | ros::spin(); 93 | 94 | return 0; 95 | } 96 | -------------------------------------------------------------------------------- /src/ros_kitti_publisher.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Hyungtae Lim on 6/23/21. 3 | // 4 | 5 | // For disable PCL complile lib, to use PointXYZILID 6 | #define PCL_NO_PRECOMPILE 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "patchwork/patchwork.hpp" 13 | #include "tools/kitti_loader.hpp" 14 | 15 | using PointType = PointXYZILID; 16 | using namespace std; 17 | 18 | ros::Publisher CloudPublisher; 19 | 20 | boost::shared_ptr> PatchworkGroundSeg; 21 | 22 | std::string output_filename; 23 | std::string acc_filename; 24 | std::string pcd_savepath; 25 | std::string data_path; 26 | string algorithm; 27 | string seq; 28 | bool save_flag; 29 | 30 | void signal_callback_handler(int signum) { 31 | cout << "Caught Ctrl + c " << endl; 32 | // Terminate program 33 | exit(signum); 34 | } 35 | 36 | template 37 | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id) { 38 | sensor_msgs::PointCloud2 cloud_ROS; 39 | pcl::toROSMsg(cloud, cloud_ROS); 40 | cloud_ROS.header.frame_id = frame_id; 41 | return cloud_ROS; 42 | } 43 | 44 | int main(int argc, char **argv) { 45 | ros::init(argc, argv, "Ros-Kitti-Publisher"); 46 | 47 | ros::NodeHandle nh; 48 | condParam(&nh, "/algorithm", algorithm, "patchwork", ""); 49 | condParam(&nh, "/seq", seq, "00", ""); 50 | condParam(&nh, "/data_path", data_path, "/", ""); 51 | 52 | ros::Rate r(10); 53 | ros::Publisher CloudPublisher = 54 | nh.advertise("/patchwork/cloud", 100, true); 55 | 56 | signal(SIGINT, signal_callback_handler); 57 | 58 | KittiLoader loader(data_path); 59 | int N = loader.size(); 60 | 61 | std::cerr << "\033[1;32m[Kitti Publisher] Total " << N << " clouds are loaded\033[0m" 62 | << std::endl; 63 | for (int n = 0; n < N; ++n) { 64 | cout << n << "th node is published!" << endl; 65 | pcl::PointCloud pc_curr; 66 | loader.get_cloud(n, pc_curr); 67 | CloudPublisher.publish(cloud2msg(pc_curr, PatchworkGroundSeg->frame_patchwork)); 68 | r.sleep(); 69 | } 70 | return 0; 71 | } 72 | --------------------------------------------------------------------------------