├── .gitignore ├── CMakeLists.txt ├── README.md ├── config ├── materials │ ├── gpf_rviz.png │ ├── open3d.png │ └── seq00_results.png └── params.yaml ├── include ├── cascadedseg │ └── cascaded_groundseg.hpp ├── common.hpp ├── gpf │ └── groundplanefit.hpp ├── gpregression │ └── GaussianFloorSegmentation.h ├── linefit │ ├── bin.cc │ ├── bin.h │ ├── ground_segmentation.cc │ ├── ground_segmentation.h │ ├── segment.cc │ └── segment.h ├── patchwork │ └── patchwork.hpp ├── r_gpf │ └── r_gpf.hpp └── ransac │ └── ransac_gpf.hpp ├── launch ├── deprecated │ ├── cascaded_gseg.launch │ ├── gpf.launch │ ├── gpregression.launch │ ├── ground_semgentation.launch │ ├── patchwork.launch │ ├── r_gpf.launch │ ├── ransac_gpf.launch │ ├── simple.launch │ └── tro_cascaded_gseg.launch └── gseg_benchmark.launch ├── license ├── msg └── node.msg ├── package.xml ├── rviz ├── basic.rviz ├── cascaded_gseg.rviz ├── classviz.rviz ├── estimateviz.rviz ├── fig_02_3300.rviz ├── ground.rviz ├── ground4r_gpf.rviz ├── kitti_mapgen.rviz ├── patchwork_viz.rviz ├── sotacomparison.rviz └── vizmap.rviz ├── scripts └── semantickitti2bag │ ├── README.md │ ├── debug_tf.py │ ├── gen_rosbag.sh │ ├── kitti2bag.py │ ├── kitti2bag.pyc │ ├── kitti2node.py │ ├── pykitti │ ├── __init__.py │ ├── __init__.pyc │ ├── odometry.py │ ├── odometry.pyc │ ├── raw.py │ ├── raw.pyc │ ├── utils.py │ └── utils.pyc │ ├── scribble.py │ ├── semantic_kitti_utils.py │ └── semantic_kitti_utils.pyc ├── shellscripts ├── autosave_all.sh ├── autosave_cascaded_gseg.sh ├── autosave_gpf.sh ├── autosave_gpregression.sh ├── autosave_linefit.sh ├── autosave_patchwork.sh ├── autosave_r_gpf.sh ├── autosave_ransac.sh └── common.sh └── src ├── lib ├── cvt.h └── datahandle.h ├── main_offline.cpp ├── main_rosbag.cpp └── utils ├── count_num.py ├── viz_all_frames.py ├── viz_each_class.cpp ├── viz_estimates.cpp └── viz_one_frame.py /.gitignore: -------------------------------------------------------------------------------- 1 | .codes/* 2 | .cmake/* 3 | 4 | build/* 5 | 6 | cmake-build-release/* 7 | cmake-build-debug/* 8 | 9 | materials/*.bin 10 | materials/*.pcd 11 | *bin 12 | *pcd 13 | 14 | .idea/ 15 | .vscode/ 16 | 17 | .idea/* 18 | .vscode/* 19 | 20 | scripts/analysis/*.png 21 | 22 | # Prerequisites 23 | *.d 24 | 25 | # Compiled Object files 26 | *.slo 27 | *.lo 28 | *.o 29 | *.obj 30 | 31 | # Precompiled Headers 32 | *.gch 33 | *.pch 34 | 35 | # Compiled Dynamic libraries 36 | *.so 37 | *.dylib 38 | *.dll 39 | 40 | # Fortran module files 41 | *.mod 42 | *.smod 43 | 44 | # Compiled Static libraries 45 | *.lai 46 | *.la 47 | *.a 48 | *.lib 49 | 50 | # Executables 51 | *.exe 52 | *.out 53 | *.app 54 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.0) #3.0.2) 2 | project(gseg_benchmark) 3 | 4 | add_compile_options(-std=c++17) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | 7 | set(CMAKE_CXX_STANDARD 14) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | roslaunch 16 | cv_bridge 17 | dynamic_reconfigure 18 | pcl_conversions 19 | pcl_ros 20 | geometry_msgs 21 | sensor_msgs 22 | message_generation 23 | velodyne_pointcloud 24 | ) 25 | 26 | add_message_files( 27 | FILES 28 | node.msg 29 | ) 30 | 31 | generate_messages( 32 | DEPENDENCIES 33 | std_msgs 34 | geometry_msgs 35 | sensor_msgs 36 | ) 37 | 38 | find_package(PCL 1.7 REQUIRED) 39 | find_package(Boost 1.54 REQUIRED) 40 | 41 | catkin_package( 42 | INCLUDE_DIRS 43 | LIBRARIES 44 | CATKIN_DEPENDS roscpp rospy std_msgs 45 | ) 46 | 47 | include_directories( 48 | ${catkin_INCLUDE_DIRS} 49 | ${PCL_INCLUDE_DIRS} 50 | include 51 | ) 52 | link_directories(${PCL_LIBRARY_DIRS}) 53 | add_definitions(${PCL_DEFINITIONS}) 54 | 55 | add_executable(classviz src/utils/viz_each_class.cpp include/common.hpp) 56 | target_link_libraries(classviz ${catkin_LIBRARIES}) 57 | add_dependencies(classviz gseg_benchmark_generate_messages_cpp) 58 | 59 | add_executable(estimateviz src/utils/viz_estimates.cpp include/common.hpp) 60 | target_link_libraries(estimateviz ${catkin_LIBRARIES}) 61 | add_dependencies(estimateviz gseg_benchmark_generate_messages_cpp) 62 | 63 | set(alg_source 64 | include/common.hpp 65 | include/gpf/groundplanefit.hpp 66 | include/r_gpf/r_gpf.hpp 67 | include/ransac/ransac_gpf.hpp 68 | include/patchwork/patchwork.hpp 69 | include/gpregression/GaussianFloorSegmentation.h 70 | include/cascadedseg/cascaded_groundseg.hpp 71 | include/linefit/bin.cc 72 | include/linefit/ground_segmentation.cc 73 | include/linefit/segment.cc 74 | ) 75 | 76 | add_executable(benchmark src/main_rosbag.cpp ${alg_source}) 77 | target_link_libraries(benchmark ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 78 | add_dependencies(benchmark gseg_benchmark_generate_messages_cpp) 79 | 80 | add_executable(benchmark_offline src/main_offline.cpp ${alg_source}) 81 | target_link_libraries(benchmark_offline ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 82 | add_dependencies(benchmark_offline gseg_benchmark_generate_messages_cpp) 83 | 84 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Ground Segmentation Benchmark 2 | 3 | 4 | This repository contains various Ground Segmentation baseline methods. Currently, 7 projects are organized for *SemanticKITTI dataset*: 5 | 6 | * [GPF](https://github.com/VincentCheungM/Run_based_segmentation) (Ground Plane Fitting) 7 | * [CascadedSeg](https://github.com/n-patiphon/cascaded_ground_seg) 8 | * [R-GPF](https://github.com/LimHyungTae/ERASOR) (Region-wise GPF) 9 | * [LineFit](https://github.com/lorenwel/linefit_ground_segmentation) 10 | * [Mono plane estimation by RANSAC](https://github.com/jafrado/qhulltest) 11 | * [Patchwork](https://github.com/LimHyungTae/patchwork) (ver.1) 12 | * [Gaussian Floor Segmentation](https://github.com/SmallMunich/FloorSegmentation/tree/master/Gaussian_process_based_Real-time_Ground_Segmentation_for_Autonomous_Land_Vehicles) 13 | 14 | The repository consists of C++ and ROS. But, for python users, we also **provide all the previously extracted ground label files**. Please check the [explanations below](#If-you-are-not-familiar-with-ROS/C++...). 15 | 16 | --- 17 | 18 | ## Citation 19 | 20 | If our open sources have been helpful, please cite the below papers published by our research group: 21 | ``` 22 | @inproceedings{oh2022travel, 23 | title={{TRAVEL: Traversable ground and above-ground object segmentation using graph representation of 3D LiDAR scans}}, 24 | author={Oh, Minho and Jung, Euigon and Lim, Hyungtae and Song, Wonho and Hu, Sumin and Lee, Eungchang Mason and Park, Junghee and Kim, Jaekyung and Lee, Jangwoo and Myung, Hyun}, 25 | booktitle={IEEE Robotics and Automation Letters}, 26 | year={2022}, 27 | note={{Submitted}} 28 | } 29 | ``` 30 | 31 | ``` 32 | @inproceedings{lee2022patchworkpp, 33 | title={{Patchwork++: Fast and robust ground segmentation solving partial under-segmentation using 3D point cloud}}, 34 | author={Lee, Seungjae and Lim, Hyungtae and Myung, Hyun}, 35 | booktitle={Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst.}, 36 | year={2022}, 37 | note={{Submitted}} 38 | } 39 | ``` 40 | 41 | ``` 42 | @article{lim2021patchwork, 43 | title={Patchwork: Concentric Zone-based Region-wise Ground Segmentation with Ground Likelihood Estimation Using a 3D LiDAR Sensor}, 44 | author={Lim, Hyungtae and Oh, Minho and Myung, Hyun}, 45 | journal={IEEE Robot. Autom. Lett.}, 46 | volume={6}, 47 | number={4}, 48 | pages={6458--6465}, 49 | year={2021}, 50 | } 51 | ``` 52 | 53 | ## Contents 54 | 55 | 0. [Description](#Description) 56 | 1. [Requirements](#Requirements) 57 | 2. [Preparing DataSet](#Preparing-DataSet) 58 | 3. [Getting Started](#Getting-Started) 59 | 4. [Python visualization / Provided result files](#If-you-are-not-familiar-with-ROS/C++...) 60 | 61 | 62 | ## Description 63 | This benchmark provides: 64 | ### Performance Calculation 65 | * The benchmark calculates the performance of each method and save the results as *csv* files. 66 | * The output files contain `frame index - time taken - Precision - Recall - TP - FP - FN - TF` values. 67 | * Two versions are to be saved: considering vegetation / not considering vegetation. 68 | 69 | ![Image text](config/materials/seq00_results.png) 70 | 71 | ### RVIZ 72 | * It visualizes the ground segmentation result on RVIZ. 73 | ![Image text](config/materials/gpf_rviz.png) 74 | * green: *True Positive* 75 | * blue: *False Negative* 76 | * red: *False Positive* 77 | 78 | 79 | ## Requirements 80 | 81 | ### Test Environment 82 | The code wass tested successfully at 83 | * Linux 18.04 LTS 84 | * ROS Melodic 85 | * Ptyhon 3.6.9 86 | 87 | ### Settings 88 | 89 | * Install [ROS](http://wiki.ros.org/melodic/Installation) on a machine 90 | * Install [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) (For visualization of GLE of Patchwork) 91 | 92 | ``` 93 | sudo apt update 94 | sudo apt-get install ros-melodic-jsk-recognition 95 | sudo apt-get install ros-melodic-jsk-common-msgs 96 | sudo apt-get install ros-melodic-jsk-rviz-plugins 97 | ``` 98 | * Install [PCL](https://pointclouds.org/downloads/) 99 | ``` 100 | sudo apt-get install libpcl-dev 101 | ``` 102 | 103 | ### Install Package 104 | * Clone our package with [catkin tools](https://catkin-tools.readthedocs.io/en/latest/) 105 | ```asm 106 | $ cd ~/catkin_ws/src 107 | $ git clone git@github.com:url-kaist/Ground-Segmentation-Benchmark.git 108 | $ catkin build gseg_benchmark 109 | ``` 110 | 111 | ## Preparing Dataset 112 | 113 | ### Offline KITTI dataset 114 | 1. Download [SemanticKITTI](http://www.semantic-kitti.org/dataset.html#download) Odometry dataset including Velodyne point clouds, calibration data, and label data. 115 | 2. Set `data_path` parameter in [shellscripts/common.sh](#Set-Parameters-ofBenchmark) for your machine. 116 | 117 | The `data_path` consists of `velodyne` folder and `labels` folder as follows: 118 | ``` 119 | ${data_path} 120 | |___00 121 | |___labels 122 | | |___000000.label 123 | | |___000001.label 124 | | |___ ... 125 | |___velodyne 126 | |___000000.bin 127 | |___000001.bin 128 | |___ ... 129 | |___01 130 | |___labels 131 | | |___ ... 132 | |___velodyne 133 | |___ ... 134 | ``` 135 | 136 | ## Getting Started 137 | 138 | ### Set Parameters of Benchmark 139 | * Set parameters about dataset path, running method, saving csv output files in `shellscripts/common.sh`. 140 | * Make directories to load [SemanticKITTI](#Offline-KITTI-dataset) dataset and save output files and apply them in rosparam setting. 141 | 142 | ``` 143 | rosparam set /data_path "/home/user/data/SemanticKITTI/" # absolute path of downloaded KITTI dataset. It must include '/' at the end part 144 | rosparam set /stop_for_each_frame false # set as 'true' to make it stop every frame 145 | rosparam set /init_idx 0 # index of first frame to run 146 | rosparam set /save_csv_file true # set as 'false' if csv output files are not needed 147 | rosparam set /save_pcd_flag false # set as 'false' if csv output files are not needed 148 | rosparam set /output_path "/data/" # reltive path of output files to be generated 149 | ``` 150 | 151 | ### Run Ground Segmentation Algorithms 152 | 153 | * Start roscore: 154 | ```asm 155 | $ roscore 156 | ``` 157 | * Open a new terminal and launch node with specification of algorithm and data sequence: 158 | ```asm 159 | $ roslaunch gseg_benchmark gseg_benchmark.launch alg:=${name of algorithm} seq:=${sequence} 160 | ``` 161 | For example, 162 | ```asm 163 | $ roslaunch gseg_benchmark gseg_benchmark.launch alg:=patchwork seq:=05 164 | ``` 165 | 166 | * There are 7 algorithms provided: `gpf`, `cascaded_gseg`, `r_gpf`, `linefit`, `ransac`, `patchwork`, `gaussian` 167 | * The examples of `seq` are 00, 01, ..., 10 168 | * If you do not set `seq` or set as `seq:=all`, then the csv output files of all datasets from "00" to "10" will be saved automatically. 169 | * Use this command to run all algorithm for all sequences. 170 | ```asm 171 | $ roslaunch gseg_benchmark gseg_benchmark.launch alg:=all seq:=all 172 | ``` 173 | 174 | * Rviz result will be shown automatically. 175 | 176 | ## If you are not familiar with ROS/C++... 177 | 178 | ### Provided Result Files 179 | We provide csv files of binary estimated results of sequences from "00" to "10". The value `1` denotes the corresponding point is estimated as the ground, whereas `0` denotes the point is considered as the non-ground. 180 | 181 | The previously extracted ground labels can be downloaded via the below commands: 182 | ``` 183 | wget https://urserver.kaist.ac.kr/publicdata/GroundSegBenchmark/cascaded_gseg_ground_labels.zip 184 | wget https://urserver.kaist.ac.kr/publicdata/GroundSegBenchmark/gpf_ground_labels.zip 185 | wget https://urserver.kaist.ac.kr/publicdata/GroundSegBenchmark/gpregression_ground_labels.zip 186 | wget https://urserver.kaist.ac.kr/publicdata/GroundSegBenchmark/linefit_ground_labels.zip 187 | wget https://urserver.kaist.ac.kr/publicdata/GroundSegBenchmark/patchwork_ground_labels.zip 188 | wget https://urserver.kaist.ac.kr/publicdata/GroundSegBenchmark/ransac_ground_labels.zip 189 | wget https://urserver.kaist.ac.kr/publicdata/GroundSegBenchmark/r_gpf_ground_labels.zip 190 | ``` 191 | 192 | ### Visualization with Python 193 | We provide Python code to visualize estimated results in binary form. 194 | 195 | * Install [python3](https://www.python.org/downloads/) 196 | * Install [open3d](http://www.open3d.org/docs/release/getting_started.html) 197 | ``` 198 | pip install open3d 199 | ``` 200 | * Set parameters in `src/utils/viz_all_frames.py` or `src/utils/viz_one_frame.py`. 201 | ``` 202 | alg = "patchwork" 203 | seq = "04" 204 | kittiraw_dir = "/home/user/data/SemanticKITTI/" # absolute path of KITTI dataset folder 205 | label_csv_dir = "/home/user/data/" # absolute path of directory to save csv files 206 | frame_num ="000010" # needed only in viz_one_frame.py 207 | ``` 208 | * Run python code 209 | ```asm 210 | $ cd ~/catkin_ws/src/Ground-Segmentation-Benchmark/src/utils 211 | $ python3 viz_one_frame.py 212 | $ python3 viz_all_frames.py 213 | ``` 214 | ![Image text](config/materials/open3d.png) 215 | * green: *ground*, black: *non-ground* 216 | 217 | --- 218 | ## Contributors 219 | 220 | * Hyungtae Lim: `shapelim at kaist dot ac dot kr` 221 | * Jeewon Kim (as a research intern @[URL](https://urobot.kaist.ac.kr/)): `ddarong2000 at kaist dot ac dot kr` 222 | 223 | ## Errors 224 | If the following error occurs in flann 225 | ``` 226 | /usr/include/flann/util/serialization.h:35:14: error: ‘class std::unordered_map >’ has no member named ‘serialize’ 227 | ``` 228 | then open header file 229 | ``` 230 | sudo gedit /usr/include/flann/util/serialization.h 231 | ``` 232 | and change all terms of "map" into "unordered_map". 233 | 234 | 235 | 236 | ## License 237 | Creative Commons License
This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. 238 | 239 | 240 | ### Copyright 241 | - All codes on this page are copyrighted by KAIST and published under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 License. You must attribute the work in the manner specified by the author. You may not use the work for commercial purposes, and you may only distribute the resulting work under the same license if you alter, transform, or create the work. 242 | -------------------------------------------------------------------------------- /config/materials/gpf_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/config/materials/gpf_rviz.png -------------------------------------------------------------------------------- /config/materials/open3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/config/materials/open3d.png -------------------------------------------------------------------------------- /config/materials/seq00_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/config/materials/seq00_results.png -------------------------------------------------------------------------------- /config/params.yaml: -------------------------------------------------------------------------------- 1 | save_flag: true 2 | sensor_height: 1.723 3 | 4 | gpf: 5 | num_iter: 3 6 | num_lpr: 20 7 | th_seeds: 0.6 # 1.2 when multiple- 0.6 8 | th_dist: 0.2 # 0.3 when multiple- 0.2 9 | 10 | 11 | r_gpf: 12 | num_iter: 3 13 | num_lpr: 10 14 | num_min_pts: 9 15 | th_seeds: 0.4 16 | th_dist: 0.12 17 | max_r: 80.0 18 | num_rings: 32 # 35 19 | num_sectors: 108 # 20 20 | 21 | ransac: 22 | th_dist: 0.15 23 | margin: 0.3 24 | 25 | patchwork: 26 | mode: "czm" # [czm] 27 | verbose: true 28 | visualize: true # Ground Likelihood Estimation is visualized 29 | num_iter: 3 30 | num_lpr: 20 31 | num_min_pts: 10 32 | th_seeds: 0.5 33 | th_dist: 0.125 34 | max_r: 80.0 35 | min_r: 2.7 36 | uprightness_thr: 0.707 # 45: 0.707 / 60: 0.866 37 | 38 | adaptive_seed_selection_margin: -1.1 39 | uniform: 40 | num_rings: 16 41 | num_sectors: 54 42 | czm: 43 | num_zones: 4 44 | num_sectors_each_zone: [16, 32 ,54, 32] 45 | mum_rings_each_zone: [2, 4, 4, 4] 46 | elevation_thresholds: [-1.2, -0.9984, -0.851, -0.605] # For elevation. The size should be equal to flatness_thresholds vector 47 | flatness_thresholds: [0.0, 0.000125, 0.000185, 0.000185] # For flatness. The size should be equal to elevation_thresholds vector 48 | 49 | 50 | cascaded_gseg: 51 | sensor_model: 64 52 | max_slope: 5.0 53 | vertical_thres: 0.50 54 | remove_floor: true 55 | plane_dis_thres: 0.08 56 | n_section: 4 57 | plane_height_thres: 5.0 58 | plane_ang_thres: 16.0 59 | 60 | linefit: 61 | n_threads: 4 # number of threads to use. 62 | 63 | r_min: 0.5 # minimum point distance. 64 | r_max: 50 # maximum point distance. 65 | n_bins: 120 # number of radial bins. 66 | n_segments: 360 # number of radial segments. 67 | 68 | max_dist_to_line: 0.05 # maximum vertical distance of point to line to be considered ground. 69 | 70 | sensor_height: 1.723 # sensor height above ground. 71 | max_slope: 0.3 # maximum slope of a ground line. 72 | max_fit_error: 0.05 # maximum error of a point during line fit. 73 | long_threshold: 1.0 # distance between points after which they are considered far from each other. 74 | max_long_height: 0.1 # maximum height change to previous point in long line. 75 | max_start_height: 0.2 # maximum difference to estimated ground height to start a new line. 76 | line_search_angle: 0.1 # how far to search in angular direction to find a line [rad]. 77 | 78 | latch: false # latch output topics or not 79 | visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING 80 | 81 | gpregression: 82 | rmax: 100 83 | max_bin_points: 200 84 | num_seed_points: 10 85 | num_bins_a: 72 86 | num_bins_l: 200 87 | p_l: 30.0 88 | p_sf: 1 89 | p_sn: 0.3 90 | p_tmodel: 5 91 | p_tdata: 5 92 | p_tg: 0.3 93 | robot_height: 1.723 94 | max_seed_range: 50 95 | max_seed_height: 3 -------------------------------------------------------------------------------- /include/linefit/bin.cc: -------------------------------------------------------------------------------- 1 | #include "linefit/bin.h" 2 | 3 | #include 4 | 5 | Bin::Bin() : min_z(std::numeric_limits::max()), has_point_(false) {} 6 | 7 | Bin::Bin(const Bin& bin) : min_z(std::numeric_limits::max()), 8 | has_point_(false) {} 9 | 10 | void Bin::addPoint(const pcl::PointXYZ& point) { 11 | const double d = sqrt(point.x * point.x + point.y * point.y); 12 | addPoint(d, point.z); 13 | } 14 | 15 | void Bin::addPoint(const double& d, const double& z) { 16 | has_point_ = true; 17 | if (z < min_z) { 18 | min_z = z; 19 | min_z_range = d; 20 | } 21 | } 22 | 23 | Bin::MinZPoint Bin::getMinZPoint() { 24 | MinZPoint point; 25 | 26 | if (has_point_) { 27 | point.z = min_z; 28 | point.d = min_z_range; 29 | } 30 | 31 | return point; 32 | } 33 | -------------------------------------------------------------------------------- /include/linefit/bin.h: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_SEGMENTATION_BIN_H_ 2 | #define GROUND_SEGMENTATION_BIN_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | class Bin { 10 | public: 11 | struct MinZPoint { 12 | MinZPoint() : z(0), d(0) {} 13 | MinZPoint(const double& d, const double& z) : z(z), d(d) {} 14 | bool operator==(const MinZPoint& comp) {return z == comp.z && d == comp.d;} 15 | 16 | double z; 17 | double d; 18 | }; 19 | 20 | private: 21 | 22 | std::atomic has_point_; 23 | std::atomic min_z; 24 | std::atomic min_z_range; 25 | 26 | public: 27 | 28 | Bin(); 29 | 30 | /// \brief Fake copy constructor to allow vector > initialization. 31 | Bin(const Bin& segment); 32 | 33 | void addPoint(const pcl::PointXYZ& point); 34 | 35 | void addPoint(const double& d, const double& z); 36 | 37 | MinZPoint getMinZPoint(); 38 | 39 | inline bool hasPoint() {return has_point_;} 40 | 41 | }; 42 | 43 | #endif /* GROUND_SEGMENTATION_BIN_H_ */ 44 | -------------------------------------------------------------------------------- /include/linefit/ground_segmentation.cc: -------------------------------------------------------------------------------- 1 | #include "linefit/ground_segmentation.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | void GroundSegmentation::visualizePointCloud(const PointCloud::ConstPtr& cloud, 10 | const std::string& id) { 11 | viewer_->addPointCloud(cloud, id, 0); 12 | } 13 | 14 | void GroundSegmentation::visualizeLines(const std::list& lines) { 15 | size_t counter = 0; 16 | for (auto it = lines.begin(); it != lines.end(); ++it) { 17 | viewer_->addLine(it->first, it->second, std::to_string(counter++)); 18 | } 19 | } 20 | 21 | void GroundSegmentation::visualize(const std::list& lines, 22 | const PointCloud::ConstPtr& min_cloud, 23 | const PointCloud::ConstPtr& ground_cloud, 24 | const PointCloud::ConstPtr& obstacle_cloud) { 25 | viewer_->setBackgroundColor (0, 0, 0); 26 | viewer_->addCoordinateSystem (1.0); 27 | viewer_->initCameraParameters (); 28 | viewer_->setCameraPosition(-2.0, 0, 2.0, 1.0, 0, 0); 29 | visualizePointCloud(min_cloud, "min_cloud"); 30 | viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 31 | 0.0f, 1.0f, 0.0f, 32 | "min_cloud"); 33 | viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 34 | 2.0f, 35 | "min_cloud"); 36 | visualizePointCloud(ground_cloud, "ground_cloud"); 37 | viewer_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 38 | 1.0f, 0.0f, 0.0f, 39 | "ground_cloud"); 40 | visualizePointCloud(obstacle_cloud, "obstacle_cloud"); 41 | visualizeLines(lines); 42 | while (!viewer_->wasStopped ()){ 43 | viewer_->spinOnce (100); 44 | boost::this_thread::sleep (boost::posix_time::microseconds (100000)); 45 | } 46 | } 47 | 48 | GroundSegmentation::GroundSegmentation(const GroundSegmentationParams& params) : 49 | params_(params), 50 | segments_(params.n_segments, Segment(params.n_bins, 51 | params.max_slope, 52 | params.max_error_square, 53 | params.long_threshold, 54 | params.max_long_height, 55 | params.max_start_height, 56 | params.sensor_height)) { 57 | if (params.visualize) viewer_ = std::make_shared("3D Viewer"); 58 | } 59 | 60 | void GroundSegmentation::segment(const PointCloud& cloud, std::vector* segmentation) { 61 | // std::cout << "Segmenting cloud with " << cloud.size() << " points...\n"; 62 | std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now(); 63 | segmentation->clear(); 64 | segmentation->resize(cloud.size(), 0); 65 | bin_index_.resize(cloud.size()); 66 | segment_coordinates_.resize(cloud.size()); 67 | 68 | insertPoints(cloud); 69 | std::list lines; 70 | if (params_.visualize) { 71 | getLines(&lines); 72 | } 73 | else { 74 | getLines(NULL); 75 | } 76 | assignCluster(segmentation); 77 | 78 | if (params_.visualize) { 79 | // Visualize. 80 | PointCloud::Ptr obstacle_cloud(new PointCloud()); 81 | // Get cloud of ground points. 82 | PointCloud::Ptr ground_cloud(new PointCloud()); 83 | for (size_t i = 0; i < cloud.size(); ++i) { 84 | if (segmentation->at(i) == 1) ground_cloud->push_back(cloud[i]); 85 | else obstacle_cloud->push_back(cloud[i]); 86 | } 87 | PointCloud::Ptr min_cloud(new PointCloud()); 88 | getMinZPointCloud(min_cloud.get()); 89 | visualize(lines, min_cloud, ground_cloud, obstacle_cloud); 90 | } 91 | std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); 92 | std::chrono::duration fp_ms = end - start; 93 | // std::cout << "Done! Took " << fp_ms.count() << "ms\n"; 94 | } 95 | 96 | void GroundSegmentation::getLines(std::list *lines) { 97 | std::mutex line_mutex; 98 | std::vector thread_vec(params_.n_threads); 99 | unsigned int i; 100 | for (i = 0; i < params_.n_threads; ++i) { 101 | const unsigned int start_index = params_.n_segments / params_.n_threads * i; 102 | const unsigned int end_index = params_.n_segments / params_.n_threads * (i+1); 103 | thread_vec[i] = std::thread(&GroundSegmentation::lineFitThread, this, 104 | start_index, end_index, lines, &line_mutex); 105 | } 106 | for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) { 107 | it->join(); 108 | } 109 | } 110 | 111 | void GroundSegmentation::lineFitThread(const unsigned int start_index, 112 | const unsigned int end_index, 113 | std::list *lines, std::mutex* lines_mutex) { 114 | const bool visualize = lines; 115 | const double seg_step = 2*M_PI / params_.n_segments; 116 | double angle = -M_PI + seg_step/2 + seg_step * start_index; 117 | for (unsigned int i = start_index; i < end_index; ++i) { 118 | segments_[i].fitSegmentLines(); 119 | // Convert lines to 3d if we want to. 120 | if (visualize) { 121 | std::list segment_lines; 122 | segments_[i].getLines(&segment_lines); 123 | for (auto line_iter = segment_lines.begin(); line_iter != segment_lines.end(); ++line_iter) { 124 | const pcl::PointXYZ start = minZPointTo3d(line_iter->first, angle); 125 | const pcl::PointXYZ end = minZPointTo3d(line_iter->second, angle); 126 | lines_mutex->lock(); 127 | lines->emplace_back(start, end); 128 | lines_mutex->unlock(); 129 | } 130 | 131 | angle += seg_step; 132 | } 133 | } 134 | } 135 | 136 | void GroundSegmentation::getMinZPointCloud(PointCloud* cloud) { 137 | const double seg_step = 2*M_PI / params_.n_segments; 138 | double angle = -M_PI + seg_step/2; 139 | for (auto seg_iter = segments_.begin(); seg_iter != segments_.end(); ++seg_iter) { 140 | for (auto bin_iter = seg_iter->begin(); bin_iter != seg_iter->end(); ++bin_iter) { 141 | const pcl::PointXYZ min = minZPointTo3d(bin_iter->getMinZPoint(), angle); 142 | cloud->push_back(min); 143 | } 144 | 145 | angle += seg_step; 146 | } 147 | } 148 | 149 | pcl::PointXYZ GroundSegmentation::minZPointTo3d(const Bin::MinZPoint &min_z_point, 150 | const double &angle) { 151 | pcl::PointXYZ point; 152 | point.x = cos(angle) * min_z_point.d; 153 | point.y = sin(angle) * min_z_point.d; 154 | point.z = min_z_point.z; 155 | return point; 156 | } 157 | 158 | void GroundSegmentation::assignCluster(std::vector* segmentation) { 159 | std::vector thread_vec(params_.n_threads); 160 | const size_t cloud_size = segmentation->size(); 161 | for (unsigned int i = 0; i < params_.n_threads; ++i) { 162 | const unsigned int start_index = cloud_size / params_.n_threads * i; 163 | const unsigned int end_index = cloud_size / params_.n_threads * (i+1); 164 | thread_vec[i] = std::thread(&GroundSegmentation::assignClusterThread, this, 165 | start_index, end_index, segmentation); 166 | } 167 | for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) { 168 | it->join(); 169 | } 170 | } 171 | 172 | void GroundSegmentation::assignClusterThread(const unsigned int &start_index, 173 | const unsigned int &end_index, 174 | std::vector *segmentation) { 175 | const double segment_step = 2*M_PI/params_.n_segments; 176 | for (unsigned int i = start_index; i < end_index; ++i) { 177 | Bin::MinZPoint point_2d = segment_coordinates_[i]; 178 | const int segment_index = bin_index_[i].first; 179 | if (segment_index >= 0) { 180 | double dist = segments_[segment_index].verticalDistanceToLine(point_2d.d, point_2d.z); 181 | // Search neighboring segments. 182 | int steps = 1; 183 | while (dist == -1 && steps * segment_step < params_.line_search_angle) { 184 | // Fix indices that are out of bounds. 185 | int index_1 = segment_index + steps; 186 | while (index_1 >= params_.n_segments) index_1 -= params_.n_segments; 187 | int index_2 = segment_index - steps; 188 | while (index_2 < 0) index_2 += params_.n_segments; 189 | // Get distance to neighboring lines. 190 | const double dist_1 = segments_[index_1].verticalDistanceToLine(point_2d.d, point_2d.z); 191 | const double dist_2 = segments_[index_2].verticalDistanceToLine(point_2d.d, point_2d.z); 192 | // Select larger distance if both segments return a valid distance. 193 | if (dist_1 > dist) { 194 | dist = dist_1; 195 | } 196 | if (dist_2 > dist) { 197 | dist = dist_2; 198 | } 199 | ++steps; 200 | } 201 | if (dist < params_.max_dist_to_line && dist != -1) { 202 | segmentation->at(i) = 1; 203 | } 204 | } 205 | } 206 | } 207 | 208 | void GroundSegmentation::getMinZPoints(PointCloud* out_cloud) { 209 | const double seg_step = 2*M_PI / params_.n_segments; 210 | const double bin_step = (sqrt(params_.r_max_square) - sqrt(params_.r_min_square)) 211 | / params_.n_bins; 212 | const double r_min = sqrt(params_.r_min_square); 213 | double angle = -M_PI + seg_step/2; 214 | for (auto seg_iter = segments_.begin(); seg_iter != segments_.end(); ++seg_iter) { 215 | double dist = r_min + bin_step/2; 216 | for (auto bin_iter = seg_iter->begin(); bin_iter != seg_iter->end(); ++bin_iter) { 217 | pcl::PointXYZ point; 218 | if (bin_iter->hasPoint()) { 219 | Bin::MinZPoint min_z_point(bin_iter->getMinZPoint()); 220 | point.x = cos(angle) * min_z_point.d; 221 | point.y = sin(angle) * min_z_point.d; 222 | point.z = min_z_point.z; 223 | 224 | out_cloud->push_back(point); 225 | } 226 | dist += bin_step; 227 | } 228 | angle += seg_step; 229 | } 230 | } 231 | 232 | void GroundSegmentation::insertPoints(const PointCloud& cloud) { 233 | std::vector threads(params_.n_threads); 234 | const size_t points_per_thread = cloud.size() / params_.n_threads; 235 | // Launch threads. 236 | for (unsigned int i = 0; i < params_.n_threads - 1; ++i) { 237 | const size_t start_index = i * points_per_thread; 238 | const size_t end_index = (i+1) * points_per_thread; 239 | threads[i] = std::thread(&GroundSegmentation::insertionThread, this, 240 | cloud, start_index, end_index); 241 | } 242 | // Launch last thread which might have more points than others. 243 | const size_t start_index = (params_.n_threads - 1) * points_per_thread; 244 | const size_t end_index = cloud.size(); 245 | threads[params_.n_threads - 1] = 246 | std::thread(&GroundSegmentation::insertionThread, this, cloud, start_index, end_index); 247 | // Wait for threads to finish. 248 | for (auto it = threads.begin(); it != threads.end(); ++it) { 249 | it->join(); 250 | } 251 | } 252 | 253 | void GroundSegmentation::insertionThread(const PointCloud& cloud, 254 | const size_t start_index, 255 | const size_t end_index) { 256 | const double segment_step = 2*M_PI / params_.n_segments; 257 | const double bin_step = (sqrt(params_.r_max_square) - sqrt(params_.r_min_square)) 258 | / params_.n_bins; 259 | const double r_min = sqrt(params_.r_min_square); 260 | for (unsigned int i = start_index; i < end_index; ++i) { 261 | pcl::PointXYZ point(cloud[i]); 262 | const double range_square = point.x * point.x + point.y * point.y; 263 | const double range = sqrt(range_square); 264 | if (range_square < params_.r_max_square && range_square > params_.r_min_square) { 265 | const double angle = std::atan2(point.y, point.x); 266 | const unsigned int bin_index = (range - r_min) / bin_step; 267 | const unsigned int segment_index = (angle + M_PI) / segment_step; 268 | const unsigned int segment_index_clamped = segment_index == params_.n_segments ? 0 : segment_index; 269 | segments_[segment_index_clamped][bin_index].addPoint(range, point.z); 270 | bin_index_[i] = std::make_pair(segment_index_clamped, bin_index); 271 | } 272 | else { 273 | bin_index_[i] = std::make_pair(-1, -1); 274 | } 275 | segment_coordinates_[i] = Bin::MinZPoint(range, point.z); 276 | } 277 | } 278 | -------------------------------------------------------------------------------- /include/linefit/ground_segmentation.h: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_SEGMENTATION_H_ 2 | #define GROUND_SEGMENTATION_H_ 3 | 4 | /* 5 | * This code is from below github page: 6 | * https://github.com/lorenwel/linefit_ground_segmentation 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "linefit/segment.h" 17 | 18 | struct GroundSegmentationParams { 19 | GroundSegmentationParams() : 20 | visualize(false), 21 | r_min_square(0.3 * 0.3), 22 | r_max_square(20*20), 23 | n_bins(30), 24 | n_segments(180), 25 | max_dist_to_line(0.15), 26 | max_slope(1), 27 | n_threads(4), 28 | max_error_square(0.01), 29 | long_threshold(2.0), 30 | max_long_height(0.1), 31 | max_start_height(0.2), 32 | sensor_height(0.2), 33 | line_search_angle(0.2) {} 34 | 35 | // Visualize estimated ground. 36 | bool visualize; 37 | // Minimum range of segmentation. 38 | double r_min_square; 39 | // Maximum range of segmentation. 40 | double r_max_square; 41 | // Number of radial bins. 42 | int n_bins; 43 | // Number of angular segments. 44 | int n_segments; 45 | // Maximum distance to a ground line to be classified as ground. 46 | double max_dist_to_line; 47 | // Max slope to be considered ground line. 48 | double max_slope; 49 | // Max error for line fit. 50 | double max_error_square; 51 | // Distance at which points are considered far from each other. 52 | double long_threshold; 53 | // Maximum slope for 54 | double max_long_height; 55 | // Maximum heigh of starting line to be labelled ground. 56 | double max_start_height; 57 | // Height of sensor above ground. 58 | double sensor_height; 59 | // How far to search for a line in angular direction [rad]. 60 | double line_search_angle; 61 | // Number of threads. 62 | int n_threads; 63 | }; 64 | 65 | typedef pcl::PointCloud PointCloud; 66 | 67 | typedef std::pair PointLine; 68 | 69 | class GroundSegmentation { 70 | 71 | const GroundSegmentationParams params_; 72 | 73 | // Access with segments_[segment][bin]. 74 | std::vector segments_; 75 | 76 | // Bin index of every point. 77 | std::vector > bin_index_; 78 | 79 | // 2D coordinates (d, z) of every point in its respective segment. 80 | std::vector segment_coordinates_; 81 | 82 | // Visualizer. 83 | std::shared_ptr viewer_; 84 | 85 | void assignCluster(std::vector* segmentation); 86 | 87 | void assignClusterThread(const unsigned int& start_index, 88 | const unsigned int& end_index, 89 | std::vector* segmentation); 90 | 91 | void insertPoints(const PointCloud& cloud); 92 | 93 | void insertionThread(const PointCloud& cloud, 94 | const size_t start_index, 95 | const size_t end_index); 96 | 97 | void getMinZPoints(PointCloud* out_cloud); 98 | 99 | void getLines(std::list* lines); 100 | 101 | void lineFitThread(const unsigned int start_index, const unsigned int end_index, 102 | std::list *lines, std::mutex* lines_mutex); 103 | 104 | pcl::PointXYZ minZPointTo3d(const Bin::MinZPoint& min_z_point, const double& angle); 105 | 106 | void getMinZPointCloud(PointCloud* cloud); 107 | 108 | void visualizePointCloud(const PointCloud::ConstPtr& cloud, 109 | const std::string& id = "point_cloud"); 110 | 111 | void visualizeLines(const std::list& lines); 112 | 113 | void visualize(const std::list& lines, const PointCloud::ConstPtr& cloud, const PointCloud::ConstPtr& ground_cloud, const PointCloud::ConstPtr& obstacle_cloud); 114 | 115 | public: 116 | 117 | GroundSegmentation(const GroundSegmentationParams& params = GroundSegmentationParams()); 118 | 119 | void segment(const PointCloud& cloud, std::vector* segmentation); 120 | 121 | }; 122 | 123 | #endif // GROUND_SEGMENTATION_H_ 124 | -------------------------------------------------------------------------------- /include/linefit/segment.cc: -------------------------------------------------------------------------------- 1 | #include "linefit/segment.h" 2 | 3 | Segment::Segment(const unsigned int& n_bins, 4 | const double& max_slope, 5 | const double& max_error, 6 | const double& long_threshold, 7 | const double& max_long_height, 8 | const double& max_start_height, 9 | const double& sensor_height) : 10 | bins_(n_bins), 11 | max_slope_(max_slope), 12 | max_error_(max_error), 13 | long_threshold_(long_threshold), 14 | max_long_height_(max_long_height), 15 | max_start_height_(max_start_height), 16 | sensor_height_(sensor_height){} 17 | 18 | void Segment::fitSegmentLines() { 19 | // Find first point. 20 | auto line_start = bins_.begin(); 21 | while (!line_start->hasPoint()) { 22 | ++line_start; 23 | // Stop if we reached last point. 24 | if (line_start == bins_.end()) return; 25 | } 26 | // Fill lines. 27 | bool is_long_line = false; 28 | double cur_ground_height = -sensor_height_; 29 | std::list current_line_points(1, line_start->getMinZPoint()); 30 | LocalLine cur_line = std::make_pair(0,0); 31 | for (auto line_iter = line_start+1; line_iter != bins_.end(); ++line_iter) { 32 | if (line_iter->hasPoint()) { 33 | Bin::MinZPoint cur_point = line_iter->getMinZPoint(); 34 | if (cur_point.d - current_line_points.back().d > long_threshold_) is_long_line = true; 35 | if (current_line_points.size() >= 2) { 36 | // Get expected z value to possibly reject far away points. 37 | double expected_z = std::numeric_limits::max(); 38 | if (is_long_line && current_line_points.size() > 2) { 39 | expected_z = cur_line.first * cur_point.d + cur_line.second; 40 | } 41 | current_line_points.push_back(cur_point); 42 | cur_line = fitLocalLine(current_line_points); 43 | const double error = getMaxError(current_line_points, cur_line); 44 | // Check if not a good line. 45 | if (error > max_error_ || 46 | std::fabs(cur_line.first) > max_slope_ || 47 | is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_) { 48 | // Add line until previous point as ground. 49 | current_line_points.pop_back(); 50 | // Don't let lines with 2 base points through. 51 | if (current_line_points.size() >= 3) { 52 | const LocalLine new_line = fitLocalLine(current_line_points); 53 | lines_.push_back(localLineToLine(new_line, current_line_points)); 54 | cur_ground_height = new_line.first * current_line_points.back().d + new_line.second; 55 | } 56 | // Start new line. 57 | is_long_line = false; 58 | current_line_points.erase(current_line_points.begin(), --current_line_points.end()); 59 | --line_iter; 60 | } 61 | // Good line, continue. 62 | else { } 63 | } 64 | else { 65 | // Not enough points. 66 | if (cur_point.d - current_line_points.back().d < long_threshold_ && 67 | std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_) { 68 | // Add point if valid. 69 | current_line_points.push_back(cur_point); 70 | } 71 | else { 72 | // Start new line. 73 | current_line_points.clear(); 74 | current_line_points.push_back(cur_point); 75 | } 76 | } 77 | } 78 | } 79 | // Add last line. 80 | if (current_line_points.size() > 2) { 81 | const LocalLine new_line = fitLocalLine(current_line_points); 82 | lines_.push_back(localLineToLine(new_line, current_line_points)); 83 | } 84 | } 85 | 86 | Segment::Line Segment::localLineToLine(const LocalLine& local_line, 87 | const std::list& line_points) { 88 | Line line; 89 | const double first_d = line_points.front().d; 90 | const double second_d = line_points.back().d; 91 | const double first_z = local_line.first * first_d + local_line.second; 92 | const double second_z = local_line.first * second_d + local_line.second; 93 | line.first.z = first_z; 94 | line.first.d = first_d; 95 | line.second.z = second_z; 96 | line.second.d = second_d; 97 | return line; 98 | } 99 | 100 | double Segment::verticalDistanceToLine(const double &d, const double &z) { 101 | static const double kMargin = 0.1; 102 | double distance = -1; 103 | for (auto it = lines_.begin(); it != lines_.end(); ++it) { 104 | if (it->first.d - kMargin < d && it->second.d + kMargin > d) { 105 | const double delta_z = it->second.z - it->first.z; 106 | const double delta_d = it->second.d - it->first.d; 107 | const double expected_z = (d - it->first.d)/delta_d *delta_z + it->first.z; 108 | distance = std::fabs(z - expected_z); 109 | } 110 | } 111 | return distance; 112 | } 113 | 114 | double Segment::getMeanError(const std::list &points, const LocalLine &line) { 115 | double error_sum = 0; 116 | for (auto it = points.begin(); it != points.end(); ++it) { 117 | const double residual = (line.first * it->d + line.second) - it->z; 118 | error_sum += residual * residual; 119 | } 120 | return error_sum/points.size(); 121 | } 122 | 123 | double Segment::getMaxError(const std::list &points, const LocalLine &line) { 124 | double max_error = 0; 125 | for (auto it = points.begin(); it != points.end(); ++it) { 126 | const double residual = (line.first * it->d + line.second) - it->z; 127 | const double error = residual * residual; 128 | if (error > max_error) max_error = error; 129 | } 130 | return max_error; 131 | } 132 | 133 | Segment::LocalLine Segment::fitLocalLine(const std::list &points) { 134 | const unsigned int n_points = points.size(); 135 | Eigen::MatrixXd X(n_points, 2); 136 | Eigen::VectorXd Y(n_points); 137 | unsigned int counter = 0; 138 | for (auto iter = points.begin(); iter != points.end(); ++iter) { 139 | X(counter, 0) = iter->d; 140 | X(counter, 1) = 1; 141 | Y(counter) = iter->z; 142 | ++counter; 143 | } 144 | Eigen::VectorXd result = X.colPivHouseholderQr().solve(Y); 145 | LocalLine line_result; 146 | line_result.first = result(0); 147 | line_result.second = result(1); 148 | return line_result; 149 | } 150 | 151 | bool Segment::getLines(std::list *lines) { 152 | if (lines_.empty()) { 153 | return false; 154 | } 155 | else { 156 | *lines = lines_; 157 | return true; 158 | } 159 | } 160 | -------------------------------------------------------------------------------- /include/linefit/segment.h: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_SEGMENTATION_SEGMENT_H_ 2 | #define GROUND_SEGMENTATION_SEGMENT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "linefit/bin.h" 8 | 9 | class Segment { 10 | public: 11 | typedef std::pair Line; 12 | 13 | typedef std::pair LocalLine; 14 | 15 | private: 16 | // Parameters. Description in GroundSegmentation. 17 | const double max_slope_; 18 | const double max_error_; 19 | const double long_threshold_; 20 | const double max_long_height_; 21 | const double max_start_height_; 22 | const double sensor_height_; 23 | 24 | std::vector bins_; 25 | 26 | std::list lines_; 27 | 28 | LocalLine fitLocalLine(const std::list& points); 29 | 30 | double getMeanError(const std::list& points, const LocalLine& line); 31 | 32 | double getMaxError(const std::list& points, const LocalLine& line); 33 | 34 | Line localLineToLine(const LocalLine& local_line, const std::list& line_points); 35 | 36 | 37 | public: 38 | 39 | Segment(const unsigned int& n_bins, 40 | const double& max_slope, 41 | const double& max_error, 42 | const double& long_threshold, 43 | const double& max_long_height, 44 | const double& max_start_height, 45 | const double& sensor_height); 46 | 47 | double verticalDistanceToLine(const double& d, const double &z); 48 | 49 | void fitSegmentLines(); 50 | 51 | inline Bin& operator[](const size_t& index) { 52 | return bins_[index]; 53 | } 54 | 55 | inline std::vector::iterator begin() { 56 | return bins_.begin(); 57 | } 58 | 59 | inline std::vector::iterator end() { 60 | return bins_.end(); 61 | } 62 | 63 | bool getLines(std::list* lines); 64 | 65 | }; 66 | 67 | #endif /* GROUND_SEGMENTATION_SEGMENT_H_ */ 68 | -------------------------------------------------------------------------------- /include/ransac/ransac_gpf.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RANSAC_GPF_H 2 | #define RANSAC_GPF_H 3 | 4 | #include "../common.hpp" 5 | 6 | class RansacGPF { 7 | public: 8 | RansacGPF() {}; 9 | 10 | RansacGPF(ros::NodeHandle *nh) : node_handle_(*nh) { 11 | // Init ROS related 12 | ROS_INFO("Inititalizing Ransac GPF..."); 13 | 14 | node_handle_.param("/sensor_height", sensor_height_, 1.723); 15 | 16 | // RANSAC PARAMETERS 17 | node_handle_.param("/ransac/th_dist", ransac_th_dist_, 0.01); 18 | 19 | ROS_INFO("Sensor Height: %f", sensor_height_); 20 | ROS_INFO("RANSAC dist threshold: %f", ransac_th_dist_); 21 | node_handle_.param("/ransac/margin", ransac_margin_, 0.3); 22 | ROS_INFO("RANSAC dist margin: %f", ransac_margin_); 23 | 24 | // Optional 25 | seg.setOptimizeCoefficients(true); 26 | // Mandatory 27 | seg.setModelType(pcl::SACMODEL_PLANE); 28 | seg.setMethodType(pcl::SAC_RANSAC); 29 | seg.setDistanceThreshold(ransac_th_dist_); 30 | 31 | std::cout << "Target mode: \033[1;32m" << mode_ << "\033[0m" << std::endl; 32 | PlaneViz = node_handle_.advertise("/gpf/plane", 100); 33 | } 34 | 35 | void estimate_ground( 36 | pcl::PointCloud &cloudIn, 37 | pcl::PointCloud &cloudOut, 38 | pcl::PointCloud &cloudNonground, 39 | double &time_taken); 40 | void estimate_ground( 41 | pcl::PointCloud &cloudIn, 42 | vector &labels); 43 | 44 | geometry_msgs::PolygonStamped set_plane_polygon(const Eigen::Vector3f &normal_v, const float &d); 45 | 46 | private: 47 | ros::NodeHandle node_handle_; 48 | double sensor_height_; 49 | int num_iter_; 50 | int num_lpr_; 51 | double th_seeds_; 52 | double th_dist_; 53 | std::string mode_; 54 | // For RANSAC 55 | pcl::SACSegmentation seg; 56 | double ransac_th_dist_; 57 | double ransac_margin_; 58 | 59 | ros::Publisher PlaneViz; 60 | 61 | // void extract_ground_Ceres(const pcl::PointCloud& cloudIn, 62 | // pcl::PointCloud& dst, pcl::PointCloud& outlier); 63 | void extract_ground_RANSAC( 64 | const pcl::PointCloud &cloudIn, 65 | pcl::PointCloud &dst, pcl::PointCloud &outlier); 66 | 67 | // Model parameter for ground plane fitting 68 | // The ground plane model is: ax+by+cz+d=0 69 | // Here normal:=[a,b,c], d=d 70 | // th_dist_d_ = threshold_dist - d 71 | float d_; 72 | MatrixXf normal_; 73 | float th_dist_d_; 74 | }; 75 | 76 | /* 77 | @brief Velodyne pointcloud callback function. The main GPF pipeline is here. 78 | PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud 79 | ->error points removal -> extract ground seeds -> ground plane fit mainloop 80 | */ 81 | void RansacGPF::estimate_ground( 82 | pcl::PointCloud &cloudIn, 83 | pcl::PointCloud &cloudOut, 84 | pcl::PointCloud &cloudNonground, 85 | double &time_taken) { 86 | 87 | auto start = chrono::high_resolution_clock::now(); 88 | 89 | extract_ground_RANSAC(cloudIn, cloudOut, cloudNonground); 90 | 91 | auto end = chrono::high_resolution_clock::now(); 92 | time_taken = static_cast(chrono::duration_cast(end - start).count()) / 1000000.0; 93 | 94 | // Visualization of plane 95 | jsk_recognition_msgs::PolygonArray plane_marker; 96 | plane_marker.header.frame_id = "/map"; 97 | plane_marker.header.stamp = ros::Time::now(); 98 | 99 | auto polygons = set_plane_polygon(normal_, d_); 100 | polygons.header = plane_marker.header; 101 | plane_marker.polygons.push_back(polygons); 102 | plane_marker.likelihood.push_back(BLUE_COLOR); 103 | PlaneViz.publish(plane_marker); 104 | } 105 | 106 | void RansacGPF::estimate_ground( 107 | pcl::PointCloud &cloudIn, 108 | vector &labels) { 109 | 110 | pcl::PointCloud cloudOut; 111 | pcl::PointCloud cloudNonground; 112 | int ground = 0; 113 | if (!labels.empty()) labels.clear(); 114 | 115 | auto start = chrono::high_resolution_clock::now(); 116 | extract_ground_RANSAC(cloudIn, cloudOut, cloudNonground); 117 | 118 | pcl::KdTreeFLANN kdtree; 119 | std::vector idxes; 120 | std::vector sqr_dists; 121 | 122 | auto cloudGround = boost::make_shared>(cloudOut); 123 | kdtree.setInputCloud(cloudGround); 124 | 125 | double epsilon = 0.0000001; 126 | 127 | for (int i = 0; i &cloudIn, 140 | pcl::PointCloud &dst, pcl::PointCloud &outlier) { 141 | pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); 142 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices); 143 | // Create the segmentation object 144 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 145 | *cloud = cloudIn; 146 | seg.setInputCloud(cloud); 147 | seg.segment(*inliers, *coefficients); 148 | 149 | static Eigen::Vector3f abc(3); 150 | abc(0) = coefficients->values[0]; 151 | abc(1) = coefficients->values[1]; 152 | abc(2) = coefficients->values[2]; 153 | d_ = coefficients->values[3]; 154 | 155 | dst.points.clear(); 156 | outlier.points.clear(); 157 | int outlier_idx = 0; 158 | 159 | static Eigen::Vector3f xyz_point; 160 | // Note that inliers are sorted 161 | for (int i = 0; i < inliers->indices.size(); ++i) { 162 | auto inlier_idx = inliers->indices[i]; 163 | dst.points.push_back(cloud->points[inliers->indices[i]]); 164 | 165 | // Fill outliers! 166 | while (outlier_idx < inlier_idx) { 167 | xyz_point(0) = cloud->points[outlier_idx].x; 168 | xyz_point(1) = cloud->points[outlier_idx].y; 169 | xyz_point(2) = cloud->points[outlier_idx].z; 170 | if (abc.dot(xyz_point) + d_ < ransac_margin_) { 171 | dst.points.push_back(cloud->points[outlier_idx]); 172 | } else { 173 | outlier.points.push_back(cloud->points[outlier_idx]); 174 | } 175 | ++outlier_idx; 176 | } 177 | ++outlier_idx; 178 | } 179 | normal_ = abc; 180 | } 181 | 182 | geometry_msgs::PolygonStamped RansacGPF::set_plane_polygon(const Eigen::Vector3f &normal_v, const float &d) { 183 | // using (50, 50), (50, -50), (-50, 50), (-50, -50) 184 | const static vector xs = {50, 50, -50, -50}; 185 | const static vector ys = {50, -50, -50, 50}; 186 | geometry_msgs::PolygonStamped polygons; 187 | // Set point of polygon. Start from RL and ccw 188 | geometry_msgs::Point32 point; 189 | 190 | for (uint8_t i = 0; i < 4; ++i) { 191 | float x = xs[i]; 192 | float y = ys[i]; 193 | point.x = x; 194 | point.y = y; 195 | // ax + by + cz = d; 196 | // => z = -(ax + by + d)/c 197 | point.z = -1 * (normal_v(0) * x + normal_v(1) * y + d) / normal_v(2); 198 | polygons.polygon.points.push_back(point); 199 | } 200 | return polygons; 201 | } 202 | 203 | 204 | #endif 205 | -------------------------------------------------------------------------------- /launch/deprecated/cascaded_gseg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "cascaded_gseg" # Select among [gpf, r_gpf] 5 | "00" 6 | true 7 | true 8 | "/data/" 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/deprecated/gpf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "gpf" 5 | "multiple" 6 | "10" 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/deprecated/gpregression.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "gpregression" 5 | "00" 6 | true 7 | true 8 | "/data/" 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/deprecated/ground_semgentation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | $(arg target_alg) 8 | $(arg target_seq) 9 | false 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/deprecated/patchwork.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "patchwork" 5 | "10" 6 | false 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/deprecated/r_gpf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "r_gpf" # Select among [gpf, r_gpf] 5 | "05" 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/deprecated/ransac_gpf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | "cascaded_gseg" 9 | "02" 10 | 3299 11 | "/data/SemanticKITTI/sequences" 12 | "/data/patchwork_dev/" 13 | true 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/deprecated/simple.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "simple" # Select among [gpf, r_gpf, simple] 5 | "10" 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/deprecated/tro_cascaded_gseg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | "cascaded_gseg" # Select among [gpf, r_gpf] 5 | "00" 6 | true 7 | "/data/SemanticKITTI/sequences" 8 | "/data/" 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/gseg_benchmark.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /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 | gseg_benchmark 4 | 1.0.0 5 | The ground segmentation benchmark in SemanticKITTI dataset 6 | 7 | Hyungtae Lim 8 | 9 | 10 | 11 | 12 | 13 | GPLv3 14 | catkin 15 | message_generation 16 | libpcl-all 17 | roscpp 18 | rospy 19 | std_msgs 20 | nav_msgs 21 | sensor_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | nav_msgs 26 | roslaunch 27 | libpcl-all-dev 28 | message_generation 29 | sensor_msgs 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /rviz/basic.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /src1 10 | Splitter Ratio: 0.5 11 | Tree Height: 755 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: src 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 10 60 | Min Value: -10 61 | Value: true 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud2 65 | Color: 0; 0; 255 66 | Color Transformer: FlatColor 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Min Color: 0; 0; 0 72 | Name: FN 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 3 77 | Size (m): 0.20000000298023224 78 | Style: Flat Squares 79 | Topic: /benchmark/FN 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | - Alpha: 1 85 | Autocompute Intensity Bounds: true 86 | Autocompute Value Bounds: 87 | Max Value: 10 88 | Min Value: -10 89 | Value: true 90 | Axis: Z 91 | Channel Name: intensity 92 | Class: rviz/PointCloud2 93 | Color: 255; 0; 0 94 | Color Transformer: FlatColor 95 | Decay Time: 0 96 | Enabled: true 97 | Invert Rainbow: false 98 | Max Color: 255; 255; 255 99 | Min Color: 0; 0; 0 100 | Name: FP 101 | Position Transformer: XYZ 102 | Queue Size: 10 103 | Selectable: true 104 | Size (Pixels): 3 105 | Size (m): 0.20000000298023224 106 | Style: Flat Squares 107 | Topic: /benchmark/FP 108 | Unreliable: false 109 | Use Fixed Frame: true 110 | Use rainbow: true 111 | Value: true 112 | - Alpha: 1 113 | Autocompute Intensity Bounds: true 114 | Autocompute Value Bounds: 115 | Max Value: 10 116 | Min Value: -10 117 | Value: true 118 | Axis: Z 119 | Channel Name: intensity 120 | Class: rviz/PointCloud2 121 | Color: 25; 255; 0 122 | Color Transformer: FlatColor 123 | Decay Time: 0 124 | Enabled: true 125 | Invert Rainbow: false 126 | Max Color: 255; 255; 255 127 | Min Color: 0; 0; 0 128 | Name: TP 129 | Position Transformer: XYZ 130 | Queue Size: 10 131 | Selectable: true 132 | Size (Pixels): 3 133 | Size (m): 0.20000000298023224 134 | Style: Flat Squares 135 | Topic: /benchmark/TP 136 | Unreliable: false 137 | Use Fixed Frame: true 138 | Use rainbow: true 139 | Value: true 140 | - Alpha: 1 141 | Autocompute Intensity Bounds: true 142 | Autocompute Value Bounds: 143 | Max Value: 10 144 | Min Value: -10 145 | Value: true 146 | Axis: Z 147 | Channel Name: intensity 148 | Class: rviz/PointCloud2 149 | Color: 255; 255; 255 150 | Color Transformer: FlatColor 151 | Decay Time: 0 152 | Enabled: true 153 | Invert Rainbow: false 154 | Max Color: 255; 255; 255 155 | Min Color: 0; 0; 0 156 | Name: src 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 3 161 | Size (m): 0.019999999552965164 162 | Style: Flat Squares 163 | Topic: /benchmark/cloud 164 | Unreliable: false 165 | Use Fixed Frame: true 166 | Use rainbow: true 167 | Value: true 168 | Enabled: true 169 | Global Options: 170 | Background Color: 48; 48; 48 171 | Default Light: true 172 | Fixed Frame: map 173 | Frame Rate: 30 174 | Name: root 175 | Tools: 176 | - Class: rviz/Interact 177 | Hide Inactive Objects: true 178 | - Class: rviz/MoveCamera 179 | - Class: rviz/Select 180 | - Class: rviz/FocusCamera 181 | - Class: rviz/Measure 182 | - Class: rviz/SetInitialPose 183 | Theta std deviation: 0.2617993950843811 184 | Topic: /initialpose 185 | X std deviation: 0.5 186 | Y std deviation: 0.5 187 | - Class: rviz/SetGoal 188 | Topic: /move_base_simple/goal 189 | - Class: rviz/PublishPoint 190 | Single click: true 191 | Topic: /clicked_point 192 | Value: true 193 | Views: 194 | Current: 195 | Class: rviz/Orbit 196 | Distance: 60.421165466308594 197 | Enable Stereo Rendering: 198 | Stereo Eye Separation: 0.05999999865889549 199 | Stereo Focal Distance: 1 200 | Swap Stereo Eyes: false 201 | Value: false 202 | Focal Point: 203 | X: -1.2681165933609009 204 | Y: 1.9304155111312866 205 | Z: 0.5047271847724915 206 | Focal Shape Fixed Size: true 207 | Focal Shape Size: 0.05000000074505806 208 | Invert Z Axis: false 209 | Name: Current View 210 | Near Clip Distance: 0.009999999776482582 211 | Pitch: 0.8803988099098206 212 | Target Frame: 213 | Value: Orbit (rviz) 214 | Yaw: 4.993581295013428 215 | Saved: ~ 216 | Window Geometry: 217 | Displays: 218 | collapsed: false 219 | Height: 1052 220 | Hide Left Dock: false 221 | Hide Right Dock: false 222 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000037efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000037e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 223 | Selection: 224 | collapsed: false 225 | Time: 226 | collapsed: false 227 | Tool Properties: 228 | collapsed: false 229 | Views: 230 | collapsed: false 231 | Width: 1920 232 | X: 0 233 | Y: 0 234 | -------------------------------------------------------------------------------- /rviz/cascaded_gseg.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /src1 11 | - /plane_viz1 12 | - /TP1 13 | - /TN1 14 | - /FP1 15 | Splitter Ratio: 0.5764706134796143 16 | Tree Height: 743 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: src 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 5 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: true 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 30 59 | Reference Frame: 60 | Value: true 61 | - Alpha: 1 62 | Autocompute Intensity Bounds: false 63 | Autocompute Value Bounds: 64 | Max Value: 10 65 | Min Value: -10 66 | Value: true 67 | Axis: Z 68 | Channel Name: intensity 69 | Class: rviz/PointCloud2 70 | Color: 255; 255; 255 71 | Color Transformer: FlatColor 72 | Decay Time: 0 73 | Enabled: true 74 | Invert Rainbow: false 75 | Max Color: 255; 255; 255 76 | Max Intensity: 0.8999999761581421 77 | Min Color: 0; 0; 0 78 | Min Intensity: 0 79 | Name: src 80 | Position Transformer: XYZ 81 | Queue Size: 10 82 | Selectable: true 83 | Size (Pixels): 3 84 | Size (m): 0.019999999552965164 85 | Style: Flat Squares 86 | Topic: /benchmark/cloud 87 | Unreliable: false 88 | Use Fixed Frame: true 89 | Use rainbow: true 90 | Value: true 91 | - Alpha: 0.30000001192092896 92 | Class: jsk_rviz_plugin/PolygonArray 93 | Color: 25; 255; 0 94 | Enabled: true 95 | Name: plane_viz 96 | Topic: /gpf/plane 97 | Unreliable: false 98 | Value: true 99 | coloring: Liekelihood 100 | enable lighting: false 101 | normal length: 1 102 | only border: false 103 | show normal: true 104 | - Alpha: 1 105 | Autocompute Intensity Bounds: true 106 | Autocompute Value Bounds: 107 | Max Value: 10 108 | Min Value: -10 109 | Value: true 110 | Axis: Z 111 | Channel Name: intensity 112 | Class: rviz/PointCloud2 113 | Color: 25; 255; 25 114 | Color Transformer: FlatColor 115 | Decay Time: 0 116 | Enabled: true 117 | Invert Rainbow: false 118 | Max Color: 255; 255; 255 119 | Max Intensity: 0.8799999952316284 120 | Min Color: 0; 0; 0 121 | Min Intensity: 0 122 | Name: TP 123 | Position Transformer: XYZ 124 | Queue Size: 10 125 | Selectable: true 126 | Size (Pixels): 3 127 | Size (m): 0.15000000596046448 128 | Style: Flat Squares 129 | Topic: /benchmark/TP 130 | Unreliable: false 131 | Use Fixed Frame: true 132 | Use rainbow: true 133 | Value: true 134 | - Alpha: 1 135 | Autocompute Intensity Bounds: true 136 | Autocompute Value Bounds: 137 | Max Value: 10 138 | Min Value: -10 139 | Value: true 140 | Axis: Z 141 | Channel Name: intensity 142 | Class: rviz/PointCloud2 143 | Color: 25; 25; 255 144 | Color Transformer: FlatColor 145 | Decay Time: 0 146 | Enabled: true 147 | Invert Rainbow: false 148 | Max Color: 25; 25; 255 149 | Max Intensity: 0.6899999976158142 150 | Min Color: 0; 0; 0 151 | Min Intensity: 0 152 | Name: TN 153 | Position Transformer: XYZ 154 | Queue Size: 10 155 | Selectable: true 156 | Size (Pixels): 3 157 | Size (m): 0.15000000596046448 158 | Style: Flat Squares 159 | Topic: /benchmark/FN 160 | Unreliable: false 161 | Use Fixed Frame: true 162 | Use rainbow: true 163 | Value: true 164 | - Alpha: 1 165 | Autocompute Intensity Bounds: true 166 | Autocompute Value Bounds: 167 | Max Value: 10 168 | Min Value: -10 169 | Value: true 170 | Axis: Z 171 | Channel Name: intensity 172 | Class: rviz/PointCloud2 173 | Color: 255; 25; 25 174 | Color Transformer: FlatColor 175 | Decay Time: 0 176 | Enabled: true 177 | Invert Rainbow: false 178 | Max Color: 255; 255; 255 179 | Max Intensity: 0.9900000095367432 180 | Min Color: 0; 0; 0 181 | Min Intensity: 0 182 | Name: FP 183 | Position Transformer: XYZ 184 | Queue Size: 10 185 | Selectable: true 186 | Size (Pixels): 3 187 | Size (m): 0.20000000298023224 188 | Style: Flat Squares 189 | Topic: /benchmark/FP 190 | Unreliable: false 191 | Use Fixed Frame: true 192 | Use rainbow: true 193 | Value: true 194 | - Class: rviz/Marker 195 | Enabled: false 196 | Marker Topic: /precision 197 | Name: Marker 198 | Namespaces: 199 | {} 200 | Queue Size: 100 201 | Value: false 202 | - Class: rviz/Marker 203 | Enabled: false 204 | Marker Topic: /recall 205 | Name: Marker 206 | Namespaces: 207 | {} 208 | Queue Size: 100 209 | Value: false 210 | Enabled: true 211 | Global Options: 212 | Background Color: 48; 48; 48 213 | Default Light: true 214 | Fixed Frame: map 215 | Frame Rate: 30 216 | Name: root 217 | Tools: 218 | - Class: rviz/Interact 219 | Hide Inactive Objects: true 220 | - Class: rviz/MoveCamera 221 | - Class: rviz/Select 222 | - Class: rviz/FocusCamera 223 | - Class: rviz/Measure 224 | - Class: rviz/SetInitialPose 225 | Theta std deviation: 0.2617993950843811 226 | Topic: /initialpose 227 | X std deviation: 0.5 228 | Y std deviation: 0.5 229 | - Class: rviz/SetGoal 230 | Topic: /move_base_simple/goal 231 | - Class: rviz/PublishPoint 232 | Single click: true 233 | Topic: /clicked_point 234 | Value: true 235 | Views: 236 | Current: 237 | Class: rviz/Orbit 238 | Distance: 80.30448913574219 239 | Enable Stereo Rendering: 240 | Stereo Eye Separation: 0.05999999865889549 241 | Stereo Focal Distance: 1 242 | Swap Stereo Eyes: false 243 | Value: false 244 | Focal Point: 245 | X: -13.322132110595703 246 | Y: 0.5092552900314331 247 | Z: 32.7205696105957 248 | Focal Shape Fixed Size: true 249 | Focal Shape Size: 0.05000000074505806 250 | Invert Z Axis: false 251 | Name: Current View 252 | Near Clip Distance: 0.009999999776482582 253 | Pitch: 1.4097964763641357 254 | Target Frame: 255 | Value: Orbit (rviz) 256 | Yaw: 3.1380186080932617 257 | Saved: ~ 258 | Window Geometry: 259 | Displays: 260 | collapsed: false 261 | Height: 1052 262 | Hide Left Dock: false 263 | Hide Right Dock: false 264 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000372fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000372000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000372000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004afc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 265 | Selection: 266 | collapsed: false 267 | Time: 268 | collapsed: false 269 | Tool Properties: 270 | collapsed: false 271 | Views: 272 | collapsed: false 273 | Width: 1920 274 | X: 1920 275 | Y: 0 276 | -------------------------------------------------------------------------------- /rviz/classviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /vegetation1 10 | - /building1 11 | - /fense1 12 | Splitter Ratio: 0.5 13 | Tree Height: 483 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: road 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: false 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: false 58 | - Alpha: 1 59 | Autocompute Intensity Bounds: true 60 | Autocompute Value Bounds: 61 | Max Value: 10 62 | Min Value: -10 63 | Value: true 64 | Axis: Z 65 | Channel Name: intensity 66 | Class: rviz/PointCloud2 67 | Color: 25; 25; 255 68 | Color Transformer: FlatColor 69 | Decay Time: 0 70 | Enabled: true 71 | Invert Rainbow: false 72 | Max Color: 255; 255; 255 73 | Max Intensity: 0.7099999785423279 74 | Min Color: 0; 0; 0 75 | Min Intensity: 0.10999999940395355 76 | Name: lane 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.05000000074505806 82 | Style: Flat Squares 83 | Topic: /lane 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: true 88 | - Alpha: 1 89 | Autocompute Intensity Bounds: true 90 | Autocompute Value Bounds: 91 | Max Value: 10 92 | Min Value: -10 93 | Value: true 94 | Axis: Z 95 | Channel Name: intensity 96 | Class: rviz/PointCloud2 97 | Color: 255; 25; 25 98 | Color Transformer: FlatColor 99 | Decay Time: 0 100 | Enabled: true 101 | Invert Rainbow: false 102 | Max Color: 255; 25; 25 103 | Max Intensity: -999999 104 | Min Color: 0; 0; 0 105 | Min Intensity: 999999 106 | Name: other_ground 107 | Position Transformer: XYZ 108 | Queue Size: 10 109 | Selectable: true 110 | Size (Pixels): 3 111 | Size (m): 0.05000000074505806 112 | Style: Flat Squares 113 | Topic: /other_ground 114 | Unreliable: false 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/PointCloud2 127 | Color: 25; 255; 255 128 | Color Transformer: FlatColor 129 | Decay Time: 0 130 | Enabled: true 131 | Invert Rainbow: false 132 | Max Color: 255; 255; 255 133 | Max Intensity: 0.8100000023841858 134 | Min Color: 0; 0; 0 135 | Min Intensity: 0 136 | Name: parking 137 | Position Transformer: XYZ 138 | Queue Size: 10 139 | Selectable: true 140 | Size (Pixels): 3 141 | Size (m): 0.05000000074505806 142 | Style: Flat Squares 143 | Topic: /parking 144 | Unreliable: false 145 | Use Fixed Frame: true 146 | Use rainbow: true 147 | Value: true 148 | - Alpha: 1 149 | Autocompute Intensity Bounds: true 150 | Autocompute Value Bounds: 151 | Max Value: 10 152 | Min Value: -10 153 | Value: true 154 | Axis: Z 155 | Channel Name: intensity 156 | Class: rviz/PointCloud2 157 | Color: 155; 155; 155 158 | Color Transformer: FlatColor 159 | Decay Time: 0 160 | Enabled: true 161 | Invert Rainbow: false 162 | Max Color: 255; 255; 255 163 | Max Intensity: 0.6899999976158142 164 | Min Color: 0; 0; 0 165 | Min Intensity: 0 166 | Name: road 167 | Position Transformer: XYZ 168 | Queue Size: 10 169 | Selectable: true 170 | Size (Pixels): 3 171 | Size (m): 0.05000000074505806 172 | Style: Flat Squares 173 | Topic: /road 174 | Unreliable: false 175 | Use Fixed Frame: true 176 | Use rainbow: true 177 | Value: true 178 | - Alpha: 1 179 | Autocompute Intensity Bounds: true 180 | Autocompute Value Bounds: 181 | Max Value: 10 182 | Min Value: -10 183 | Value: true 184 | Axis: Z 185 | Channel Name: intensity 186 | Class: rviz/PointCloud2 187 | Color: 255; 255; 255 188 | Color Transformer: FlatColor 189 | Decay Time: 0 190 | Enabled: true 191 | Invert Rainbow: false 192 | Max Color: 255; 255; 255 193 | Max Intensity: 0.6100000143051147 194 | Min Color: 0; 0; 0 195 | Min Intensity: 0 196 | Name: sidewalk 197 | Position Transformer: XYZ 198 | Queue Size: 10 199 | Selectable: true 200 | Size (Pixels): 3 201 | Size (m): 0.05000000074505806 202 | Style: Flat Squares 203 | Topic: /sidewalk 204 | Unreliable: false 205 | Use Fixed Frame: true 206 | Use rainbow: true 207 | Value: true 208 | - Alpha: 1 209 | Autocompute Intensity Bounds: true 210 | Autocompute Value Bounds: 211 | Max Value: 10 212 | Min Value: -10 213 | Value: true 214 | Axis: Z 215 | Channel Name: intensity 216 | Class: rviz/PointCloud2 217 | Color: 25; 255; 25 218 | Color Transformer: FlatColor 219 | Decay Time: 0 220 | Enabled: true 221 | Invert Rainbow: false 222 | Max Color: 255; 255; 255 223 | Max Intensity: 0.5799999833106995 224 | Min Color: 0; 0; 0 225 | Min Intensity: 0 226 | Name: terrain 227 | Position Transformer: XYZ 228 | Queue Size: 10 229 | Selectable: true 230 | Size (Pixels): 3 231 | Size (m): 0.05000000074505806 232 | Style: Flat Squares 233 | Topic: /terrain 234 | Unreliable: false 235 | Use Fixed Frame: true 236 | Use rainbow: true 237 | Value: true 238 | - Alpha: 1 239 | Autocompute Intensity Bounds: true 240 | Autocompute Value Bounds: 241 | Max Value: 10 242 | Min Value: -10 243 | Value: true 244 | Axis: Z 245 | Channel Name: intensity 246 | Class: rviz/PointCloud2 247 | Color: 205; 15; 155 248 | Color Transformer: FlatColor 249 | Decay Time: 0 250 | Enabled: true 251 | Invert Rainbow: false 252 | Max Color: 255; 255; 255 253 | Max Intensity: 0.6299999952316284 254 | Min Color: 0; 0; 0 255 | Min Intensity: 0 256 | Name: unlabeled 257 | Position Transformer: XYZ 258 | Queue Size: 10 259 | Selectable: true 260 | Size (Pixels): 3 261 | Size (m): 0.05000000074505806 262 | Style: Flat Squares 263 | Topic: /unlabeled 264 | Unreliable: false 265 | Use Fixed Frame: true 266 | Use rainbow: true 267 | Value: true 268 | - Alpha: 1 269 | Autocompute Intensity Bounds: true 270 | Autocompute Value Bounds: 271 | Max Value: 10 272 | Min Value: -10 273 | Value: true 274 | Axis: Z 275 | Channel Name: intensity 276 | Class: rviz/PointCloud2 277 | Color: 125; 70; 255 278 | Color Transformer: FlatColor 279 | Decay Time: 0 280 | Enabled: true 281 | Invert Rainbow: false 282 | Max Color: 255; 255; 255 283 | Max Intensity: -999999 284 | Min Color: 0; 0; 0 285 | Min Intensity: 999999 286 | Name: outlier 287 | Position Transformer: XYZ 288 | Queue Size: 10 289 | Selectable: true 290 | Size (Pixels): 3 291 | Size (m): 0.05000000074505806 292 | Style: Flat Squares 293 | Topic: /outlier 294 | Unreliable: false 295 | Use Fixed Frame: true 296 | Use rainbow: true 297 | Value: true 298 | - Alpha: 1 299 | Autocompute Intensity Bounds: true 300 | Autocompute Value Bounds: 301 | Max Value: 10 302 | Min Value: -10 303 | Value: true 304 | Axis: Z 305 | Channel Name: intensity 306 | Class: rviz/PointCloud2 307 | Color: 255; 255; 0 308 | Color Transformer: FlatColor 309 | Decay Time: 0 310 | Enabled: true 311 | Invert Rainbow: false 312 | Max Color: 255; 255; 255 313 | Max Intensity: 0.7400000095367432 314 | Min Color: 0; 0; 0 315 | Min Intensity: 0 316 | Name: vegetation 317 | Position Transformer: XYZ 318 | Queue Size: 10 319 | Selectable: true 320 | Size (Pixels): 3 321 | Size (m): 0.05000000074505806 322 | Style: Flat Squares 323 | Topic: /vegetation 324 | Unreliable: false 325 | Use Fixed Frame: true 326 | Use rainbow: true 327 | Value: true 328 | - Alpha: 1 329 | Autocompute Intensity Bounds: true 330 | Autocompute Value Bounds: 331 | Max Value: 10 332 | Min Value: -10 333 | Value: true 334 | Axis: Z 335 | Channel Name: intensity 336 | Class: rviz/PointCloud2 337 | Color: 125; 5; 5 338 | Color Transformer: FlatColor 339 | Decay Time: 0 340 | Enabled: true 341 | Invert Rainbow: false 342 | Max Color: 255; 255; 255 343 | Max Intensity: 0.9900000095367432 344 | Min Color: 0; 0; 0 345 | Min Intensity: 0 346 | Name: building 347 | Position Transformer: XYZ 348 | Queue Size: 10 349 | Selectable: true 350 | Size (Pixels): 3 351 | Size (m): 0.05000000074505806 352 | Style: Flat Squares 353 | Topic: /building 354 | Unreliable: false 355 | Use Fixed Frame: true 356 | Use rainbow: true 357 | Value: true 358 | - Alpha: 1 359 | Autocompute Intensity Bounds: true 360 | Autocompute Value Bounds: 361 | Max Value: 10 362 | Min Value: -10 363 | Value: true 364 | Axis: Z 365 | Channel Name: intensity 366 | Class: rviz/PointCloud2 367 | Color: 255; 125; 5 368 | Color Transformer: FlatColor 369 | Decay Time: 0 370 | Enabled: true 371 | Invert Rainbow: false 372 | Max Color: 255; 255; 255 373 | Max Intensity: 0.7799999713897705 374 | Min Color: 0; 0; 0 375 | Min Intensity: 0 376 | Name: fense 377 | Position Transformer: XYZ 378 | Queue Size: 10 379 | Selectable: true 380 | Size (Pixels): 3 381 | Size (m): 0.05000000074505806 382 | Style: Flat Squares 383 | Topic: /fense 384 | Unreliable: false 385 | Use Fixed Frame: true 386 | Use rainbow: true 387 | Value: true 388 | Enabled: true 389 | Global Options: 390 | Background Color: 48; 48; 48 391 | Default Light: true 392 | Fixed Frame: map 393 | Frame Rate: 30 394 | Name: root 395 | Tools: 396 | - Class: rviz/Interact 397 | Hide Inactive Objects: true 398 | - Class: rviz/MoveCamera 399 | - Class: rviz/Select 400 | - Class: rviz/FocusCamera 401 | - Class: rviz/Measure 402 | - Class: rviz/SetInitialPose 403 | Theta std deviation: 0.2617993950843811 404 | Topic: /initialpose 405 | X std deviation: 0.5 406 | Y std deviation: 0.5 407 | - Class: rviz/SetGoal 408 | Topic: /move_base_simple/goal 409 | - Class: rviz/PublishPoint 410 | Single click: true 411 | Topic: /clicked_point 412 | Value: true 413 | Views: 414 | Current: 415 | Class: rviz/Orbit 416 | Distance: 32.219337463378906 417 | Enable Stereo Rendering: 418 | Stereo Eye Separation: 0.05999999865889549 419 | Stereo Focal Distance: 1 420 | Swap Stereo Eyes: false 421 | Value: false 422 | Focal Point: 423 | X: -2.8311290740966797 424 | Y: 0.16472376883029938 425 | Z: 2.9878454208374023 426 | Focal Shape Fixed Size: true 427 | Focal Shape Size: 0.05000000074505806 428 | Invert Z Axis: false 429 | Name: Current View 430 | Near Clip Distance: 0.009999999776482582 431 | Pitch: 0.3897974491119385 432 | Target Frame: 433 | Value: Orbit (rviz) 434 | Yaw: 2.9653851985931396 435 | Saved: ~ 436 | Window Geometry: 437 | Displays: 438 | collapsed: false 439 | Height: 780 440 | Hide Left Dock: false 441 | Hide Right Dock: false 442 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000026efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000026e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000026e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000032f0000026e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 443 | Selection: 444 | collapsed: false 445 | Time: 446 | collapsed: false 447 | Tool Properties: 448 | collapsed: false 449 | Views: 450 | collapsed: false 451 | Width: 1440 452 | X: 2046 453 | Y: 125 454 | -------------------------------------------------------------------------------- /rviz/estimateviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /cas_FN1 10 | - /cas_FP1 11 | - /cas_TP1 12 | Splitter Ratio: 0.5 13 | Tree Height: 483 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: cas_FN 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: false 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: false 58 | - Alpha: 1 59 | Autocompute Intensity Bounds: true 60 | Autocompute Value Bounds: 61 | Max Value: 10 62 | Min Value: -10 63 | Value: true 64 | Axis: Z 65 | Channel Name: intensity 66 | Class: rviz/PointCloud2 67 | Color: 2; 2; 255 68 | Color Transformer: FlatColor 69 | Decay Time: 0 70 | Enabled: true 71 | Invert Rainbow: false 72 | Max Color: 255; 255; 255 73 | Max Intensity: 0 74 | Min Color: 0; 0; 0 75 | Min Intensity: 0 76 | Name: cas_FN 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.10000000149011612 82 | Style: Flat Squares 83 | Topic: /cascade/FN 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: true 88 | - Alpha: 1 89 | Autocompute Intensity Bounds: true 90 | Autocompute Value Bounds: 91 | Max Value: 10 92 | Min Value: -10 93 | Value: true 94 | Axis: Z 95 | Channel Name: intensity 96 | Class: rviz/PointCloud2 97 | Color: 255; 2; 2 98 | Color Transformer: FlatColor 99 | Decay Time: 0 100 | Enabled: true 101 | Invert Rainbow: false 102 | Max Color: 255; 255; 255 103 | Max Intensity: 1 104 | Min Color: 0; 0; 0 105 | Min Intensity: 1 106 | Name: cas_FP 107 | Position Transformer: XYZ 108 | Queue Size: 10 109 | Selectable: true 110 | Size (Pixels): 3 111 | Size (m): 0.10000000149011612 112 | Style: Flat Squares 113 | Topic: /cascade/FP 114 | Unreliable: false 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/PointCloud2 127 | Color: 2; 255; 2 128 | Color Transformer: FlatColor 129 | Decay Time: 0 130 | Enabled: true 131 | Invert Rainbow: false 132 | Max Color: 255; 255; 255 133 | Max Intensity: 3 134 | Min Color: 0; 0; 0 135 | Min Intensity: 3 136 | Name: cas_TP 137 | Position Transformer: XYZ 138 | Queue Size: 10 139 | Selectable: true 140 | Size (Pixels): 3 141 | Size (m): 0.10000000149011612 142 | Style: Flat Squares 143 | Topic: /cascade/TP 144 | Unreliable: false 145 | Use Fixed Frame: true 146 | Use rainbow: true 147 | Value: true 148 | Enabled: true 149 | Global Options: 150 | Background Color: 48; 48; 48 151 | Default Light: true 152 | Fixed Frame: map 153 | Frame Rate: 30 154 | Name: root 155 | Tools: 156 | - Class: rviz/Interact 157 | Hide Inactive Objects: true 158 | - Class: rviz/MoveCamera 159 | - Class: rviz/Select 160 | - Class: rviz/FocusCamera 161 | - Class: rviz/Measure 162 | - Class: rviz/SetInitialPose 163 | Theta std deviation: 0.2617993950843811 164 | Topic: /initialpose 165 | X std deviation: 0.5 166 | Y std deviation: 0.5 167 | - Class: rviz/SetGoal 168 | Topic: /move_base_simple/goal 169 | - Class: rviz/PublishPoint 170 | Single click: true 171 | Topic: /clicked_point 172 | Value: true 173 | Views: 174 | Current: 175 | Class: rviz/Orbit 176 | Distance: 112.79371643066406 177 | Enable Stereo Rendering: 178 | Stereo Eye Separation: 0.05999999865889549 179 | Stereo Focal Distance: 1 180 | Swap Stereo Eyes: false 181 | Value: false 182 | Focal Point: 183 | X: -0.9473841786384583 184 | Y: 1.4703583717346191 185 | Z: 2.9902374744415283 186 | Focal Shape Fixed Size: true 187 | Focal Shape Size: 0.05000000074505806 188 | Invert Z Axis: false 189 | Name: Current View 190 | Near Clip Distance: 0.009999999776482582 191 | Pitch: 1.5697963237762451 192 | Target Frame: 193 | Value: Orbit (rviz) 194 | Yaw: 3.1103875637054443 195 | Saved: ~ 196 | Window Geometry: 197 | Displays: 198 | collapsed: false 199 | Height: 780 200 | Hide Left Dock: false 201 | Hide Right Dock: false 202 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000026efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000026e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000026e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000032f0000026e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 203 | Selection: 204 | collapsed: false 205 | Time: 206 | collapsed: false 207 | Tool Properties: 208 | collapsed: false 209 | Views: 210 | collapsed: false 211 | Width: 1440 212 | X: 2046 213 | Y: 125 214 | -------------------------------------------------------------------------------- /rviz/fig_02_3300.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Axes1 10 | Splitter Ratio: 0.5 11 | Tree Height: 581 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: TP 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: false 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 10 60 | Min Value: -10 61 | Value: true 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud2 65 | Color: 0; 0; 255 66 | Color Transformer: FlatColor 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Min Color: 0; 0; 0 72 | Name: FN 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 3 77 | Size (m): 0.20000000298023224 78 | Style: Flat Squares 79 | Topic: /benchmark/FN 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | - Alpha: 1 85 | Autocompute Intensity Bounds: true 86 | Autocompute Value Bounds: 87 | Max Value: 10 88 | Min Value: -10 89 | Value: true 90 | Axis: Z 91 | Channel Name: intensity 92 | Class: rviz/PointCloud2 93 | Color: 255; 0; 0 94 | Color Transformer: FlatColor 95 | Decay Time: 0 96 | Enabled: true 97 | Invert Rainbow: false 98 | Max Color: 255; 255; 255 99 | Min Color: 0; 0; 0 100 | Name: FP 101 | Position Transformer: XYZ 102 | Queue Size: 10 103 | Selectable: true 104 | Size (Pixels): 3 105 | Size (m): 0.20000000298023224 106 | Style: Flat Squares 107 | Topic: /benchmark/FP 108 | Unreliable: false 109 | Use Fixed Frame: true 110 | Use rainbow: true 111 | Value: true 112 | - Alpha: 1 113 | Autocompute Intensity Bounds: true 114 | Autocompute Value Bounds: 115 | Max Value: 10 116 | Min Value: -10 117 | Value: true 118 | Axis: Z 119 | Channel Name: intensity 120 | Class: rviz/PointCloud2 121 | Color: 25; 255; 0 122 | Color Transformer: FlatColor 123 | Decay Time: 0 124 | Enabled: true 125 | Invert Rainbow: false 126 | Max Color: 255; 255; 255 127 | Min Color: 0; 0; 0 128 | Name: TP 129 | Position Transformer: XYZ 130 | Queue Size: 10 131 | Selectable: true 132 | Size (Pixels): 3 133 | Size (m): 0.20000000298023224 134 | Style: Flat Squares 135 | Topic: /benchmark/TP 136 | Unreliable: false 137 | Use Fixed Frame: true 138 | Use rainbow: true 139 | Value: true 140 | - Alpha: 1 141 | Autocompute Intensity Bounds: true 142 | Autocompute Value Bounds: 143 | Max Value: 10 144 | Min Value: -10 145 | Value: true 146 | Axis: Z 147 | Channel Name: intensity 148 | Class: rviz/PointCloud2 149 | Color: 255; 255; 255 150 | Color Transformer: FlatColor 151 | Decay Time: 0 152 | Enabled: false 153 | Invert Rainbow: false 154 | Max Color: 255; 255; 255 155 | Min Color: 0; 0; 0 156 | Name: src 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 3 161 | Size (m): 0.019999999552965164 162 | Style: Flat Squares 163 | Topic: /benchmark/cloud 164 | Unreliable: false 165 | Use Fixed Frame: true 166 | Use rainbow: true 167 | Value: false 168 | - Class: rviz/Axes 169 | Enabled: true 170 | Length: 2 171 | Name: Axes 172 | Radius: 0.30000001192092896 173 | Reference Frame: 174 | Value: true 175 | Enabled: true 176 | Global Options: 177 | Background Color: 255; 255; 255 178 | Default Light: true 179 | Fixed Frame: map 180 | Frame Rate: 30 181 | Name: root 182 | Tools: 183 | - Class: rviz/Interact 184 | Hide Inactive Objects: true 185 | - Class: rviz/MoveCamera 186 | - Class: rviz/Select 187 | - Class: rviz/FocusCamera 188 | - Class: rviz/Measure 189 | - Class: rviz/SetInitialPose 190 | Theta std deviation: 0.2617993950843811 191 | Topic: /initialpose 192 | X std deviation: 0.5 193 | Y std deviation: 0.5 194 | - Class: rviz/SetGoal 195 | Topic: /move_base_simple/goal 196 | - Class: rviz/PublishPoint 197 | Single click: true 198 | Topic: /clicked_point 199 | Value: true 200 | Views: 201 | Current: 202 | Class: rviz/Orbit 203 | Distance: 38.854373931884766 204 | Enable Stereo Rendering: 205 | Stereo Eye Separation: 0.05999999865889549 206 | Stereo Focal Distance: 1 207 | Swap Stereo Eyes: false 208 | Value: false 209 | Focal Point: 210 | X: 0 211 | Y: 0 212 | Z: 0 213 | Focal Shape Fixed Size: false 214 | Focal Shape Size: 0.05000000074505806 215 | Invert Z Axis: false 216 | Name: Current View 217 | Near Clip Distance: 0.009999999776482582 218 | Pitch: 0.9753980040550232 219 | Target Frame: 220 | Value: Orbit (rviz) 221 | Yaw: 1.1453979015350342 222 | Saved: ~ 223 | Window Geometry: 224 | Displays: 225 | collapsed: false 226 | Height: 1052 227 | Hide Left Dock: false 228 | Hide Right Dock: false 229 | QMainWindow State: 000000ff00000000fd000000040000000000000215000002d0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002d0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002d0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000780000000ecfc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000045f000002d000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 230 | Selection: 231 | collapsed: false 232 | Time: 233 | collapsed: false 234 | Tool Properties: 235 | collapsed: false 236 | Views: 237 | collapsed: false 238 | Width: 1920 239 | X: 0 240 | Y: 0 241 | -------------------------------------------------------------------------------- /rviz/ground.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /plane_viz1 11 | Splitter Ratio: 0.5 12 | Tree Height: 755 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: src 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 5 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 20 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Autocompute Intensity Bounds: false 59 | Autocompute Value Bounds: 60 | Max Value: 10 61 | Min Value: -10 62 | Value: true 63 | Axis: Z 64 | Channel Name: intensity 65 | Class: rviz/PointCloud2 66 | Color: 255; 255; 255 67 | Color Transformer: FlatColor 68 | Decay Time: 0 69 | Enabled: true 70 | Invert Rainbow: false 71 | Max Color: 255; 255; 255 72 | Max Intensity: 0.8999999761581421 73 | Min Color: 0; 0; 0 74 | Min Intensity: 0 75 | Name: src 76 | Position Transformer: XYZ 77 | Queue Size: 10 78 | Selectable: true 79 | Size (Pixels): 3 80 | Size (m): 0.05999999865889549 81 | Style: Flat Squares 82 | Topic: /benchmark/cloud 83 | Unreliable: false 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/PointCloud2 96 | Color: 15; 255; 100 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.9900000095367432 103 | Min Color: 0; 0; 0 104 | Min Intensity: 0 105 | Name: plane 106 | Position Transformer: XYZ 107 | Queue Size: 10 108 | Selectable: true 109 | Size (Pixels): 3 110 | Size (m): 0.15000000596046448 111 | Style: Flat Squares 112 | Topic: /benchmark/ground_estimate 113 | Unreliable: false 114 | Use Fixed Frame: true 115 | Use rainbow: true 116 | Value: true 117 | - Alpha: 0.30000001192092896 118 | Class: jsk_rviz_plugin/PolygonArray 119 | Color: 25; 255; 0 120 | Enabled: true 121 | Name: plane_viz 122 | Topic: /gpf/plane 123 | Unreliable: false 124 | Value: true 125 | coloring: Auto 126 | enable lighting: true 127 | normal length: 0.20000000298023224 128 | only border: false 129 | show normal: true 130 | Enabled: true 131 | Global Options: 132 | Background Color: 48; 48; 48 133 | Default Light: true 134 | Fixed Frame: map 135 | Frame Rate: 30 136 | Name: root 137 | Tools: 138 | - Class: rviz/Interact 139 | Hide Inactive Objects: true 140 | - Class: rviz/MoveCamera 141 | - Class: rviz/Select 142 | - Class: rviz/FocusCamera 143 | - Class: rviz/Measure 144 | - Class: rviz/SetInitialPose 145 | Theta std deviation: 0.2617993950843811 146 | Topic: /initialpose 147 | X std deviation: 0.5 148 | Y std deviation: 0.5 149 | - Class: rviz/SetGoal 150 | Topic: /move_base_simple/goal 151 | - Class: rviz/PublishPoint 152 | Single click: true 153 | Topic: /clicked_point 154 | Value: true 155 | Views: 156 | Current: 157 | Class: rviz/Orbit 158 | Distance: 92.17261505126953 159 | Enable Stereo Rendering: 160 | Stereo Eye Separation: 0.05999999865889549 161 | Stereo Focal Distance: 1 162 | Swap Stereo Eyes: false 163 | Value: false 164 | Focal Point: 165 | X: 4.563323974609375 166 | Y: 4.161128520965576 167 | Z: -1.909702181816101 168 | Focal Shape Fixed Size: true 169 | Focal Shape Size: 0.05000000074505806 170 | Invert Z Axis: false 171 | Name: Current View 172 | Near Clip Distance: 0.009999999776482582 173 | Pitch: 0.9147995114326477 174 | Target Frame: 175 | Value: Orbit (rviz) 176 | Yaw: 2.153083324432373 177 | Saved: ~ 178 | Window Geometry: 179 | Displays: 180 | collapsed: false 181 | Height: 1052 182 | Hide Left Dock: false 183 | Hide Right Dock: false 184 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000037efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000037e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 185 | Selection: 186 | collapsed: false 187 | Time: 188 | collapsed: false 189 | Tool Properties: 190 | collapsed: false 191 | Views: 192 | collapsed: false 193 | Width: 1920 194 | X: 1920 195 | Y: 0 196 | -------------------------------------------------------------------------------- /rviz/ground4r_gpf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /TP1 11 | - /FN1 12 | - /FP1 13 | - /Axes1 14 | - /revert_pc1 15 | Splitter Ratio: 0.6044568419456482 16 | Tree Height: 467 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: TP 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 5 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: true 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 30 59 | Reference Frame: 60 | Value: true 61 | - Alpha: 1 62 | Autocompute Intensity Bounds: false 63 | Autocompute Value Bounds: 64 | Max Value: 10 65 | Min Value: -10 66 | Value: true 67 | Axis: Z 68 | Channel Name: intensity 69 | Class: rviz/PointCloud2 70 | Color: 255; 255; 255 71 | Color Transformer: FlatColor 72 | Decay Time: 0 73 | Enabled: false 74 | Invert Rainbow: false 75 | Max Color: 255; 255; 255 76 | Max Intensity: 0.8999999761581421 77 | Min Color: 0; 0; 0 78 | Min Intensity: 0 79 | Name: src 80 | Position Transformer: XYZ 81 | Queue Size: 10 82 | Selectable: true 83 | Size (Pixels): 3 84 | Size (m): 0.019999999552965164 85 | Style: Flat Squares 86 | Topic: /benchmark/cloud 87 | Unreliable: false 88 | Use Fixed Frame: true 89 | Use rainbow: true 90 | Value: false 91 | - Alpha: 0.6000000238418579 92 | Class: jsk_rviz_plugin/PolygonArray 93 | Color: 25; 255; 0 94 | Enabled: false 95 | Name: plane_viz 96 | Topic: /gpf/plane 97 | Unreliable: false 98 | Value: false 99 | coloring: Liekelihood 100 | enable lighting: false 101 | normal length: 1 102 | only border: false 103 | show normal: false 104 | - Alpha: 1 105 | Autocompute Intensity Bounds: true 106 | Autocompute Value Bounds: 107 | Max Value: 10 108 | Min Value: -10 109 | Value: true 110 | Axis: Z 111 | Channel Name: intensity 112 | Class: rviz/PointCloud2 113 | Color: 25; 255; 25 114 | Color Transformer: FlatColor 115 | Decay Time: 0 116 | Enabled: true 117 | Invert Rainbow: false 118 | Max Color: 255; 255; 255 119 | Max Intensity: 0.8799999952316284 120 | Min Color: 0; 0; 0 121 | Min Intensity: 0 122 | Name: TP 123 | Position Transformer: XYZ 124 | Queue Size: 10 125 | Selectable: true 126 | Size (Pixels): 3 127 | Size (m): 0.20000000298023224 128 | Style: Flat Squares 129 | Topic: /benchmark/TP 130 | Unreliable: false 131 | Use Fixed Frame: true 132 | Use rainbow: true 133 | Value: true 134 | - Alpha: 1 135 | Autocompute Intensity Bounds: true 136 | Autocompute Value Bounds: 137 | Max Value: 10 138 | Min Value: -10 139 | Value: true 140 | Axis: Z 141 | Channel Name: intensity 142 | Class: rviz/PointCloud2 143 | Color: 25; 25; 255 144 | Color Transformer: FlatColor 145 | Decay Time: 0 146 | Enabled: true 147 | Invert Rainbow: false 148 | Max Color: 25; 25; 255 149 | Max Intensity: 0.6899999976158142 150 | Min Color: 0; 0; 0 151 | Min Intensity: 0 152 | Name: FN 153 | Position Transformer: XYZ 154 | Queue Size: 10 155 | Selectable: true 156 | Size (Pixels): 3 157 | Size (m): 0.20000000298023224 158 | Style: Flat Squares 159 | Topic: /benchmark/FN 160 | Unreliable: false 161 | Use Fixed Frame: true 162 | Use rainbow: true 163 | Value: true 164 | - Alpha: 1 165 | Autocompute Intensity Bounds: true 166 | Autocompute Value Bounds: 167 | Max Value: 10 168 | Min Value: -10 169 | Value: true 170 | Axis: Z 171 | Channel Name: intensity 172 | Class: rviz/PointCloud2 173 | Color: 255; 25; 25 174 | Color Transformer: FlatColor 175 | Decay Time: 0 176 | Enabled: true 177 | Invert Rainbow: false 178 | Max Color: 255; 255; 255 179 | Max Intensity: 0.9900000095367432 180 | Min Color: 0; 0; 0 181 | Min Intensity: 0 182 | Name: FP 183 | Position Transformer: XYZ 184 | Queue Size: 10 185 | Selectable: true 186 | Size (Pixels): 3 187 | Size (m): 0.20000000298023224 188 | Style: Flat Squares 189 | Topic: /benchmark/FP 190 | Unreliable: false 191 | Use Fixed Frame: true 192 | Use rainbow: true 193 | Value: true 194 | - Class: rviz/Marker 195 | Enabled: false 196 | Marker Topic: /precision 197 | Name: Marker 198 | Namespaces: 199 | {} 200 | Queue Size: 100 201 | Value: false 202 | - Class: rviz/Marker 203 | Enabled: false 204 | Marker Topic: /recall 205 | Name: Marker 206 | Namespaces: 207 | {} 208 | Queue Size: 100 209 | Value: false 210 | - Class: rviz/Axes 211 | Enabled: true 212 | Length: 1.5 213 | Name: Axes 214 | Radius: 0.15000000596046448 215 | Reference Frame: 216 | Value: true 217 | - Alpha: 1 218 | Autocompute Intensity Bounds: true 219 | Autocompute Value Bounds: 220 | Max Value: 10 221 | Min Value: -10 222 | Value: true 223 | Axis: Z 224 | Channel Name: intensity 225 | Class: rviz/PointCloud2 226 | Color: 25; 255; 255 227 | Color Transformer: FlatColor 228 | Decay Time: 0 229 | Enabled: true 230 | Invert Rainbow: false 231 | Max Color: 255; 255; 255 232 | Max Intensity: -999999 233 | Min Color: 0; 0; 0 234 | Min Intensity: 999999 235 | Name: revert_pc 236 | Position Transformer: XYZ 237 | Queue Size: 10 238 | Selectable: true 239 | Size (Pixels): 3 240 | Size (m): 0.20000000298023224 241 | Style: Flat Squares 242 | Topic: /revert_pc 243 | Unreliable: false 244 | Use Fixed Frame: true 245 | Use rainbow: true 246 | Value: true 247 | - Alpha: 1 248 | Autocompute Intensity Bounds: true 249 | Autocompute Value Bounds: 250 | Max Value: 10 251 | Min Value: -10 252 | Value: true 253 | Axis: Z 254 | Channel Name: intensity 255 | Class: rviz/PointCloud2 256 | Color: 255; 25; 255 257 | Color Transformer: FlatColor 258 | Decay Time: 0 259 | Enabled: false 260 | Invert Rainbow: false 261 | Max Color: 255; 255; 255 262 | Max Intensity: 0.8100000023841858 263 | Min Color: 0; 0; 0 264 | Min Intensity: 0 265 | Name: reject_pc 266 | Position Transformer: XYZ 267 | Queue Size: 10 268 | Selectable: true 269 | Size (Pixels): 3 270 | Size (m): 0.15000000596046448 271 | Style: Flat Squares 272 | Topic: /reject_pc 273 | Unreliable: false 274 | Use Fixed Frame: true 275 | Use rainbow: true 276 | Value: false 277 | Enabled: true 278 | Global Options: 279 | Background Color: 48; 48; 48 280 | Default Light: true 281 | Fixed Frame: map 282 | Frame Rate: 30 283 | Name: root 284 | Tools: 285 | - Class: rviz/Interact 286 | Hide Inactive Objects: true 287 | - Class: rviz/MoveCamera 288 | - Class: rviz/Select 289 | - Class: rviz/FocusCamera 290 | - Class: rviz/Measure 291 | - Class: rviz/SetInitialPose 292 | Theta std deviation: 0.2617993950843811 293 | Topic: /initialpose 294 | X std deviation: 0.5 295 | Y std deviation: 0.5 296 | - Class: rviz/SetGoal 297 | Topic: /move_base_simple/goal 298 | - Class: rviz/PublishPoint 299 | Single click: true 300 | Topic: /clicked_point 301 | Value: true 302 | Views: 303 | Current: 304 | Class: rviz/Orbit 305 | Distance: 41.25093460083008 306 | Enable Stereo Rendering: 307 | Stereo Eye Separation: 0.05999999865889549 308 | Stereo Focal Distance: 1 309 | Swap Stereo Eyes: false 310 | Value: false 311 | Focal Point: 312 | X: -1.5914632081985474 313 | Y: -2.2983741760253906 314 | Z: 0.7079330682754517 315 | Focal Shape Fixed Size: true 316 | Focal Shape Size: 0.05000000074505806 317 | Invert Z Axis: false 318 | Name: Current View 319 | Near Clip Distance: 0.009999999776482582 320 | Pitch: 1.5397964715957642 321 | Target Frame: 322 | Value: Orbit (rviz) 323 | Yaw: 3.136617660522461 324 | Saved: ~ 325 | Window Geometry: 326 | Displays: 327 | collapsed: false 328 | Height: 759 329 | Hide Left Dock: false 330 | Hide Right Dock: true 331 | QMainWindow State: 000000ff00000000fd0000000400000000000001c10000025efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000372000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056d00000039fc0100000002fb0000000800540069006d006501000000000000056d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003a60000025e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 332 | Selection: 333 | collapsed: false 334 | Time: 335 | collapsed: false 336 | Tool Properties: 337 | collapsed: false 338 | Views: 339 | collapsed: true 340 | Width: 1389 341 | X: 2071 342 | Y: 87 343 | -------------------------------------------------------------------------------- /rviz/kitti_mapgen.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /PointCloud21/Autocompute Value Bounds1 11 | Splitter Ratio: 0.5 12 | Tree Height: 549 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: PointCloud2 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Autocompute Intensity Bounds: false 59 | Autocompute Value Bounds: 60 | Max Value: 1.5499999523162842 61 | Min Value: 0.10610000044107437 62 | Value: false 63 | Axis: Z 64 | Channel Name: intensity 65 | Class: rviz/PointCloud2 66 | Color: 255; 255; 255 67 | Color Transformer: Intensity 68 | Decay Time: 0 69 | Enabled: true 70 | Invert Rainbow: false 71 | Max Color: 255; 255; 255 72 | Max Intensity: 0.5 73 | Min Color: 0; 0; 0 74 | Min Intensity: 0 75 | Name: PointCloud2 76 | Position Transformer: XYZ 77 | Queue Size: 10 78 | Selectable: true 79 | Size (Pixels): 3 80 | Size (m): 0.10000000149011612 81 | Style: Flat Squares 82 | Topic: /mapgen/map 83 | Unreliable: false 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | - Alpha: 1 88 | Buffer Length: 1 89 | Class: rviz/Path 90 | Color: 25; 255; 0 91 | Enabled: true 92 | Head Diameter: 0.30000001192092896 93 | Head Length: 0.20000000298023224 94 | Length: 0.30000001192092896 95 | Line Style: Lines 96 | Line Width: 0.029999999329447746 97 | Name: Path 98 | Offset: 99 | X: 0 100 | Y: 0 101 | Z: 0 102 | Pose Color: 255; 85; 255 103 | Pose Style: None 104 | Radius: 0.029999999329447746 105 | Shaft Diameter: 0.10000000149011612 106 | Shaft Length: 0.10000000149011612 107 | Topic: /path 108 | Unreliable: false 109 | Value: true 110 | - Class: rviz/Axes 111 | Enabled: true 112 | Length: 1 113 | Name: Axes 114 | Radius: 0.10000000149011612 115 | Reference Frame: 116 | Value: true 117 | Enabled: true 118 | Global Options: 119 | Background Color: 48; 48; 48 120 | Default Light: true 121 | Fixed Frame: map 122 | Frame Rate: 30 123 | Name: root 124 | Tools: 125 | - Class: rviz/Interact 126 | Hide Inactive Objects: true 127 | - Class: rviz/MoveCamera 128 | - Class: rviz/Select 129 | - Class: rviz/FocusCamera 130 | - Class: rviz/Measure 131 | - Class: rviz/SetInitialPose 132 | Theta std deviation: 0.2617993950843811 133 | Topic: /initialpose 134 | X std deviation: 0.5 135 | Y std deviation: 0.5 136 | - Class: rviz/SetGoal 137 | Topic: /move_base_simple/goal 138 | - Class: rviz/PublishPoint 139 | Single click: true 140 | Topic: /clicked_point 141 | Value: true 142 | Views: 143 | Current: 144 | Class: rviz/Orbit 145 | Distance: 211.60079956054688 146 | Enable Stereo Rendering: 147 | Stereo Eye Separation: 0.05999999865889549 148 | Stereo Focal Distance: 1 149 | Swap Stereo Eyes: false 150 | Value: false 151 | Focal Point: 152 | X: 51.64692306518555 153 | Y: -6.151813507080078 154 | Z: 12.050065994262695 155 | Focal Shape Fixed Size: false 156 | Focal Shape Size: 0.05000000074505806 157 | Invert Z Axis: false 158 | Name: Current View 159 | Near Clip Distance: 0.009999999776482582 160 | Pitch: 1.424796462059021 161 | Target Frame: 162 | Value: Orbit (rviz) 163 | Yaw: 1.9204024076461792 164 | Saved: ~ 165 | Window Geometry: 166 | Displays: 167 | collapsed: false 168 | Height: 846 169 | Hide Left Dock: false 170 | Hide Right Dock: false 171 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 172 | Selection: 173 | collapsed: false 174 | Time: 175 | collapsed: false 176 | Tool Properties: 177 | collapsed: false 178 | Views: 179 | collapsed: false 180 | Width: 1200 181 | X: 2017 182 | Y: 79 183 | -------------------------------------------------------------------------------- /rviz/patchwork_viz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PolygonArray1 10 | Splitter Ratio: 0.5 11 | Tree Height: 728 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Class: jsk_rviz_plugin/PolygonArray 58 | Color: 25; 255; 0 59 | Enabled: true 60 | Name: PolygonArray 61 | Topic: /gpf/plane 62 | Unreliable: false 63 | Value: true 64 | coloring: Auto 65 | enable lighting: true 66 | normal length: 0.10000000149011612 67 | only border: false 68 | show normal: false 69 | Enabled: true 70 | Global Options: 71 | Background Color: 48; 48; 48 72 | Default Light: true 73 | Fixed Frame: map 74 | Frame Rate: 30 75 | Name: root 76 | Tools: 77 | - Class: rviz/Interact 78 | Hide Inactive Objects: true 79 | - Class: rviz/MoveCamera 80 | - Class: rviz/Select 81 | - Class: rviz/FocusCamera 82 | - Class: rviz/Measure 83 | - Class: rviz/SetInitialPose 84 | Theta std deviation: 0.2617993950843811 85 | Topic: /initialpose 86 | X std deviation: 0.5 87 | Y std deviation: 0.5 88 | - Class: rviz/SetGoal 89 | Topic: /move_base_simple/goal 90 | - Class: rviz/PublishPoint 91 | Single click: true 92 | Topic: /clicked_point 93 | Value: true 94 | Views: 95 | Current: 96 | Class: rviz/Orbit 97 | Distance: 210.1780242919922 98 | Enable Stereo Rendering: 99 | Stereo Eye Separation: 0.05999999865889549 100 | Stereo Focal Distance: 1 101 | Swap Stereo Eyes: false 102 | Value: false 103 | Focal Point: 104 | X: -3.4474682807922363 105 | Y: 3.967628002166748 106 | Z: -0.0036148999352008104 107 | Focal Shape Fixed Size: true 108 | Focal Shape Size: 0.05000000074505806 109 | Invert Z Axis: false 110 | Name: Current View 111 | Near Clip Distance: 0.009999999776482582 112 | Pitch: 1.5697963237762451 113 | Target Frame: 114 | Value: Orbit (rviz) 115 | Yaw: 3.0985796451568604 116 | Saved: ~ 117 | Window Geometry: 118 | Displays: 119 | collapsed: false 120 | Height: 1025 121 | Hide Left Dock: false 122 | Hide Right Dock: false 123 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 124 | Selection: 125 | collapsed: false 126 | Time: 127 | collapsed: false 128 | Tool Properties: 129 | collapsed: false 130 | Views: 131 | collapsed: false 132 | Width: 1853 133 | X: 67 134 | Y: 27 135 | -------------------------------------------------------------------------------- /rviz/sotacomparison.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /plane_viz1 11 | - /FP1 12 | - /Axes1 13 | - /revert_pc1 14 | Splitter Ratio: 0.6044568419456482 15 | Tree Height: 760 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: revert_pc 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Cell Size: 5 44 | Class: rviz/Grid 45 | Color: 160; 160; 164 46 | Enabled: false 47 | Line Style: 48 | Line Width: 0.029999999329447746 49 | Value: Lines 50 | Name: Grid 51 | Normal Cell Count: 0 52 | Offset: 53 | X: 0 54 | Y: 0 55 | Z: 0 56 | Plane: XY 57 | Plane Cell Count: 30 58 | Reference Frame: 59 | Value: false 60 | - Alpha: 1 61 | Autocompute Intensity Bounds: false 62 | Autocompute Value Bounds: 63 | Max Value: 10 64 | Min Value: -10 65 | Value: true 66 | Axis: Z 67 | Channel Name: intensity 68 | Class: rviz/PointCloud2 69 | Color: 255; 255; 255 70 | Color Transformer: FlatColor 71 | Decay Time: 0 72 | Enabled: true 73 | Invert Rainbow: false 74 | Max Color: 255; 255; 255 75 | Max Intensity: 0.8999999761581421 76 | Min Color: 0; 0; 0 77 | Min Intensity: 0 78 | Name: src 79 | Position Transformer: XYZ 80 | Queue Size: 10 81 | Selectable: true 82 | Size (Pixels): 3 83 | Size (m): 0.019999999552965164 84 | Style: Flat Squares 85 | Topic: /benchmark/cloud 86 | Unreliable: false 87 | Use Fixed Frame: true 88 | Use rainbow: true 89 | Value: true 90 | - Alpha: 0.6000000238418579 91 | Class: jsk_rviz_plugin/PolygonArray 92 | Color: 25; 255; 0 93 | Enabled: true 94 | Name: plane_viz 95 | Topic: /gpf/plane 96 | Unreliable: false 97 | Value: true 98 | coloring: Liekelihood 99 | enable lighting: false 100 | normal length: 1 101 | only border: false 102 | show normal: false 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 10 107 | Min Value: -10 108 | Value: true 109 | Axis: Z 110 | Channel Name: intensity 111 | Class: rviz/PointCloud2 112 | Color: 25; 255; 25 113 | Color Transformer: FlatColor 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Max Intensity: 0.8799999952316284 119 | Min Color: 0; 0; 0 120 | Min Intensity: 0 121 | Name: TP 122 | Position Transformer: XYZ 123 | Queue Size: 10 124 | Selectable: true 125 | Size (Pixels): 3 126 | Size (m): 0.07000000029802322 127 | Style: Flat Squares 128 | Topic: /benchmark/TP 129 | Unreliable: false 130 | Use Fixed Frame: true 131 | Use rainbow: true 132 | Value: true 133 | - Alpha: 1 134 | Autocompute Intensity Bounds: true 135 | Autocompute Value Bounds: 136 | Max Value: 10 137 | Min Value: -10 138 | Value: true 139 | Axis: Z 140 | Channel Name: intensity 141 | Class: rviz/PointCloud2 142 | Color: 25; 25; 255 143 | Color Transformer: FlatColor 144 | Decay Time: 0 145 | Enabled: true 146 | Invert Rainbow: false 147 | Max Color: 25; 25; 255 148 | Max Intensity: 0.6899999976158142 149 | Min Color: 0; 0; 0 150 | Min Intensity: 0 151 | Name: FN 152 | Position Transformer: XYZ 153 | Queue Size: 10 154 | Selectable: true 155 | Size (Pixels): 3 156 | Size (m): 0.10000000149011612 157 | Style: Flat Squares 158 | Topic: /benchmark/FN 159 | Unreliable: false 160 | Use Fixed Frame: true 161 | Use rainbow: true 162 | Value: true 163 | - Alpha: 1 164 | Autocompute Intensity Bounds: true 165 | Autocompute Value Bounds: 166 | Max Value: 10 167 | Min Value: -10 168 | Value: true 169 | Axis: Z 170 | Channel Name: intensity 171 | Class: rviz/PointCloud2 172 | Color: 255; 25; 25 173 | Color Transformer: FlatColor 174 | Decay Time: 0 175 | Enabled: true 176 | Invert Rainbow: false 177 | Max Color: 255; 255; 255 178 | Max Intensity: 0.9900000095367432 179 | Min Color: 0; 0; 0 180 | Min Intensity: 0 181 | Name: FP 182 | Position Transformer: XYZ 183 | Queue Size: 10 184 | Selectable: true 185 | Size (Pixels): 3 186 | Size (m): 0.15000000596046448 187 | Style: Flat Squares 188 | Topic: /benchmark/FP 189 | Unreliable: false 190 | Use Fixed Frame: true 191 | Use rainbow: true 192 | Value: true 193 | - Class: rviz/Marker 194 | Enabled: false 195 | Marker Topic: /precision 196 | Name: Marker 197 | Namespaces: 198 | {} 199 | Queue Size: 100 200 | Value: false 201 | - Class: rviz/Marker 202 | Enabled: false 203 | Marker Topic: /recall 204 | Name: Marker 205 | Namespaces: 206 | {} 207 | Queue Size: 100 208 | Value: false 209 | - Class: rviz/Axes 210 | Enabled: true 211 | Length: 1.5 212 | Name: Axes 213 | Radius: 0.15000000596046448 214 | Reference Frame: 215 | Value: true 216 | - Alpha: 1 217 | Autocompute Intensity Bounds: true 218 | Autocompute Value Bounds: 219 | Max Value: 10 220 | Min Value: -10 221 | Value: true 222 | Axis: Z 223 | Channel Name: intensity 224 | Class: rviz/PointCloud2 225 | Color: 25; 255; 255 226 | Color Transformer: FlatColor 227 | Decay Time: 0 228 | Enabled: true 229 | Invert Rainbow: false 230 | Max Color: 255; 255; 255 231 | Max Intensity: -999999 232 | Min Color: 0; 0; 0 233 | Min Intensity: 999999 234 | Name: revert_pc 235 | Position Transformer: XYZ 236 | Queue Size: 10 237 | Selectable: true 238 | Size (Pixels): 3 239 | Size (m): 0.20000000298023224 240 | Style: Flat Squares 241 | Topic: /revert_pc 242 | Unreliable: false 243 | Use Fixed Frame: true 244 | Use rainbow: true 245 | Value: true 246 | - Alpha: 1 247 | Autocompute Intensity Bounds: true 248 | Autocompute Value Bounds: 249 | Max Value: 10 250 | Min Value: -10 251 | Value: true 252 | Axis: Z 253 | Channel Name: intensity 254 | Class: rviz/PointCloud2 255 | Color: 255; 25; 255 256 | Color Transformer: FlatColor 257 | Decay Time: 0 258 | Enabled: false 259 | Invert Rainbow: false 260 | Max Color: 255; 255; 255 261 | Max Intensity: 0.8100000023841858 262 | Min Color: 0; 0; 0 263 | Min Intensity: 0 264 | Name: reject_pc 265 | Position Transformer: XYZ 266 | Queue Size: 10 267 | Selectable: true 268 | Size (Pixels): 3 269 | Size (m): 0.15000000596046448 270 | Style: Flat Squares 271 | Topic: /reject_pc 272 | Unreliable: false 273 | Use Fixed Frame: true 274 | Use rainbow: true 275 | Value: false 276 | Enabled: true 277 | Global Options: 278 | Background Color: 48; 48; 48 279 | Default Light: true 280 | Fixed Frame: map 281 | Frame Rate: 30 282 | Name: root 283 | Tools: 284 | - Class: rviz/Interact 285 | Hide Inactive Objects: true 286 | - Class: rviz/MoveCamera 287 | - Class: rviz/Select 288 | - Class: rviz/FocusCamera 289 | - Class: rviz/Measure 290 | - Class: rviz/SetInitialPose 291 | Theta std deviation: 0.2617993950843811 292 | Topic: /initialpose 293 | X std deviation: 0.5 294 | Y std deviation: 0.5 295 | - Class: rviz/SetGoal 296 | Topic: /move_base_simple/goal 297 | - Class: rviz/PublishPoint 298 | Single click: true 299 | Topic: /clicked_point 300 | Value: true 301 | Views: 302 | Current: 303 | Class: rviz/Orbit 304 | Distance: 34.03683853149414 305 | Enable Stereo Rendering: 306 | Stereo Eye Separation: 0.05999999865889549 307 | Stereo Focal Distance: 1 308 | Swap Stereo Eyes: false 309 | Value: false 310 | Focal Point: 311 | X: 3.641408681869507 312 | Y: -0.3012377619743347 313 | Z: 1.3567605018615723 314 | Focal Shape Fixed Size: true 315 | Focal Shape Size: 0.05000000074505806 316 | Invert Z Axis: false 317 | Name: Current View 318 | Near Clip Distance: 0.009999999776482582 319 | Pitch: 0.9347970485687256 320 | Target Frame: 321 | Value: Orbit (rviz) 322 | Yaw: 5.3316216468811035 323 | Saved: ~ 324 | Window Geometry: 325 | Displays: 326 | collapsed: false 327 | Height: 1052 328 | Hide Left Dock: false 329 | Hide Right Dock: true 330 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000383fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000383000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000372000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000038300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 331 | Selection: 332 | collapsed: false 333 | Time: 334 | collapsed: false 335 | Tool Properties: 336 | collapsed: false 337 | Views: 338 | collapsed: true 339 | Width: 1920 340 | X: 1920 341 | Y: 0 342 | -------------------------------------------------------------------------------- /rviz/vizmap.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /PointCloud21 11 | - /PointCloud22 12 | - /PointCloud23 13 | Splitter Ratio: 0.6282420754432678 14 | Tree Height: 718 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | - /Orbit1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: PointCloud2 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Class: rviz/Axes 43 | Enabled: false 44 | Length: 1 45 | Name: Axes 46 | Radius: 0.10000000149011612 47 | Reference Frame: 48 | Value: false 49 | - Alpha: 0.5 50 | Cell Size: 10 51 | Class: rviz/Grid 52 | Color: 160; 160; 164 53 | Enabled: false 54 | Line Style: 55 | Line Width: 0.029999999329447746 56 | Value: Lines 57 | Name: Grid 58 | Normal Cell Count: 0 59 | Offset: 60 | X: 0 61 | Y: 0 62 | Z: 0 63 | Plane: XY 64 | Plane Cell Count: 10 65 | Reference Frame: 66 | Value: false 67 | - Alpha: 1 68 | Autocompute Intensity Bounds: true 69 | Autocompute Value Bounds: 70 | Max Value: 10 71 | Min Value: -10 72 | Value: true 73 | Axis: Z 74 | Channel Name: intensity 75 | Class: rviz/PointCloud2 76 | Color: 25; 25; 255 77 | Color Transformer: FlatColor 78 | Decay Time: 0 79 | Enabled: true 80 | Invert Rainbow: false 81 | Max Color: 255; 255; 255 82 | Max Intensity: 0.32999998331069946 83 | Min Color: 0; 0; 0 84 | Min Intensity: 0 85 | Name: PointCloud2 86 | Position Transformer: XYZ 87 | Queue Size: 10 88 | Selectable: true 89 | Size (Pixels): 3 90 | Size (m): 0.15000000596046448 91 | Style: Flat Squares 92 | Topic: /map/thr1 93 | Unreliable: false 94 | Use Fixed Frame: true 95 | Use rainbow: true 96 | Value: true 97 | - Alpha: 1 98 | Autocompute Intensity Bounds: true 99 | Autocompute Value Bounds: 100 | Max Value: 10 101 | Min Value: -10 102 | Value: true 103 | Axis: Z 104 | Channel Name: intensity 105 | Class: rviz/PointCloud2 106 | Color: 25; 255; 25 107 | Color Transformer: FlatColor 108 | Decay Time: 0 109 | Enabled: true 110 | Invert Rainbow: false 111 | Max Color: 255; 255; 255 112 | Max Intensity: 0.6599999666213989 113 | Min Color: 0; 0; 0 114 | Min Intensity: 0.33000001311302185 115 | Name: PointCloud2 116 | Position Transformer: XYZ 117 | Queue Size: 10 118 | Selectable: true 119 | Size (Pixels): 3 120 | Size (m): 0.15000000596046448 121 | Style: Flat Squares 122 | Topic: /map/thr2 123 | Unreliable: false 124 | Use Fixed Frame: true 125 | Use rainbow: true 126 | Value: true 127 | - Alpha: 1 128 | Autocompute Intensity Bounds: true 129 | Autocompute Value Bounds: 130 | Max Value: 10 131 | Min Value: -10 132 | Value: true 133 | Axis: Z 134 | Channel Name: intensity 135 | Class: rviz/PointCloud2 136 | Color: 255; 55; 25 137 | Color Transformer: FlatColor 138 | Decay Time: 0 139 | Enabled: true 140 | Invert Rainbow: false 141 | Max Color: 255; 255; 255 142 | Max Intensity: 0.9900000095367432 143 | Min Color: 0; 0; 0 144 | Min Intensity: 0.6600000262260437 145 | Name: PointCloud2 146 | Position Transformer: XYZ 147 | Queue Size: 10 148 | Selectable: true 149 | Size (Pixels): 3 150 | Size (m): 0.15000000596046448 151 | Style: Flat Squares 152 | Topic: /map/thr3 153 | Unreliable: false 154 | Use Fixed Frame: true 155 | Use rainbow: true 156 | Value: true 157 | Enabled: true 158 | Global Options: 159 | Background Color: 48; 48; 48 160 | Default Light: true 161 | Fixed Frame: map 162 | Frame Rate: 30 163 | Name: root 164 | Tools: 165 | - Class: rviz/Interact 166 | Hide Inactive Objects: true 167 | - Class: rviz/MoveCamera 168 | - Class: rviz/Select 169 | - Class: rviz/FocusCamera 170 | - Class: rviz/Measure 171 | - Class: rviz/SetInitialPose 172 | Theta std deviation: 0.2617993950843811 173 | Topic: /initialpose 174 | X std deviation: 0.5 175 | Y std deviation: 0.5 176 | - Class: rviz/SetGoal 177 | Topic: /move_base_simple/goal 178 | - Class: rviz/PublishPoint 179 | Single click: true 180 | Topic: /clicked_point 181 | Value: true 182 | Views: 183 | Current: 184 | Class: rviz/XYOrbit 185 | Distance: 179.86541748046875 186 | Enable Stereo Rendering: 187 | Stereo Eye Separation: 0.05999999865889549 188 | Stereo Focal Distance: 1 189 | Swap Stereo Eyes: false 190 | Value: false 191 | Focal Point: 192 | X: -23.397851943969727 193 | Y: 10.218847274780273 194 | Z: 2.6963902200805023e-5 195 | Focal Shape Fixed Size: true 196 | Focal Shape Size: 0.05000000074505806 197 | Invert Z Axis: false 198 | Name: Current View 199 | Near Clip Distance: 0.009999999776482582 200 | Pitch: 1.059796929359436 201 | Target Frame: 202 | Value: XYOrbit (rviz) 203 | Yaw: 1.8903496265411377 204 | Saved: 205 | - Class: rviz/Orbit 206 | Distance: 5.025652885437012 207 | Enable Stereo Rendering: 208 | Stereo Eye Separation: 0.05999999865889549 209 | Stereo Focal Distance: 1 210 | Swap Stereo Eyes: false 211 | Value: false 212 | Focal Point: 213 | X: -0.10060713440179825 214 | Y: 0.7826277017593384 215 | Z: 1.6387885808944702 216 | Focal Shape Fixed Size: true 217 | Focal Shape Size: 0.05000000074505806 218 | Invert Z Axis: false 219 | Name: Orbit 220 | Near Clip Distance: 0.009999999776482582 221 | Pitch: 1.5597963333129883 222 | Target Frame: 223 | Value: Orbit (rviz) 224 | Yaw: 0.0053986357524991035 225 | Window Geometry: 226 | Displays: 227 | collapsed: false 228 | Height: 1052 229 | Hide Left Dock: false 230 | Hide Right Dock: true 231 | QMainWindow State: 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 232 | Selection: 233 | collapsed: false 234 | Time: 235 | collapsed: false 236 | Tool Properties: 237 | collapsed: false 238 | Views: 239 | collapsed: true 240 | Width: 1920 241 | X: 1920 242 | Y: 0 243 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/README.md: -------------------------------------------------------------------------------- 1 | # Rosbag generation 2 | 3 | ## How to use 4 | 5 | ### kitti2node.py 6 | 0. build를 통해 nonplanar_gpf를 컴파일 해야함. 그래야 nonplanar_gpf.msg를 import 할 수 있음 7 | 1. kitti2node의 344~345 input path와 output path를 지정 8 | 2. 만약 일부분의 stamp만 필요하면 `kitti2node.py`의 381을 True로 지정하고 원하는 `initial_stamp`와 `final_stamp`를 지정함 9 | 3. 2에서 `init_stamp`가 하나 더 있는 이유는 이게 너무 0.0초에 node가 생성돼서 씹히는 경우가 생겨서 그럼 (이해 안 되면 임형태한테 물어보길...) 10 | 11 | `frame_range = [init_stamp] + range(init_stamp, final_stamp, interval)`에 해당하는 줄을 말함 12 | 13 | 4. 아래 명령어 실행 14 | 15 | ``` 16 | python kitti2imnode.py -t None -r None -s $sequence$ --kitti_type "odom_noimg" 17 | ``` 18 | 19 | 예시: 00번을 ros node로 만들 때 20 | 21 | ``` 22 | python kitti2imnode.py -t None -r None -s 00 --kitti_type "odom_noimg" 23 | ``` 24 | 25 | ### plot_z_variation.py 26 | 27 | 각 sequence 별 z 분석 (viz 폴더에 결과들 있음!) 28 | 29 | # Descriptions 30 | 31 | 사실 이 코드는 이미지도 bag에 넣을 수 있는데, 용량이 너무 커지므로 여기서는 lidar(pointcloud)와 pose만 다룸 32 | 33 | 34 | * SuMa의 pose를 /home/shapelim/hdd2/kitti_semantic/dataset/sequences/00/poses.txt에 여기 넣어둬야 함! 35 | 36 | * label은 원래 uint32인데, intensity에 넣기 위해서 float 32로 변경해서 넣어줌. C++ 에서 uint32로 되돌려서 파싱 필요 37 | 38 | np.uint32 <-> uint32_t in c++ 39 | np.float32 <-> float in c++ 40 | 41 | Originally, 42 | 43 | CAM2LIDAR = np.array([[-1.857739385241e-03, -9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03], 44 | [- 6.481465826011e-03, 8.051860151134e-03, - 9.999466081774e-01, - 7.337429464231e-02], 45 | [9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01], 46 | [0, 0, 0, 1]]) 47 | 48 | 바닥 기준이라면 49 | 50 | CAM2BASE = np.array([[-1.857739385241e-03, -9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03], 51 | [- 6.481465826011e-03, 8.051860151134e-03, - 9.999466081774e-01, 1.65], 52 | [9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01], 53 | [0, 0, 0, 1]]) 54 | 55 | ***sensor height***는 1.723m 로 잡으면 됨 56 | 57 | 58 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/debug_tf.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import imSLAM 3 | from kitti2bag import inv 4 | import pypcd 5 | labelname = "/media/shapelim/UX960 NVMe/kitti_semantic/dataset/sequences/00/labels/000000.label" 6 | labels = np.fromfile(labelname, dtype=np.uint32) 7 | print(labels.shape) 8 | pc = np.fromfile("/media/shapelim/UX960 NVMe/kitti_semantic/dataset/sequences/00/velodyne/000000.bin", dtype=np.float32).reshape(-1, 4) 9 | 10 | print(labels.astype(np.float32).shape) 11 | 12 | CAM2LIDAR = np.array([[-1.857739385241e-03, -9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03], 13 | [- 6.481465826011e-03, 8.051860151134e-03, - 9.999466081774e-01, - 7.337429464231e-02], 14 | [9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01], 15 | [0, 0, 0, 1]]) 16 | 17 | CAM2BASE = np.array([[-1.857739385241e-03, -9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03], 18 | [- 6.481465826011e-03, 8.051860151134e-03, - 9.999466081774e-01, 1.65], 19 | [9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01], 20 | [0, 0, 0, 1]]) 21 | 22 | LiDAR2BASE = np.array([[0., 0., 0., 0.], 23 | [0., 0., 0., 0.], 24 | [0., 0., 0., -1.73], 25 | [0., 0., 0., 1.]]) 26 | 27 | 28 | # print(np.linalg.inv(CAM2LIDAR)) 29 | print(inv(CAM2LIDAR)) 30 | test = np.array([[1], 31 | [0], 32 | [0], 33 | [1]]) 34 | print(np.matmul(inv(CAM2LIDAR), test)) 35 | 36 | # print(np.matmul(CAM2LIDAR, LiDAR2BASE)) 37 | # self.T_w_cam0 = [] 38 | # with open(pose_file, 'r') as f: 39 | # for line in f.readlines(): 40 | # T = np.fromstring(line, dtype=float, sep=' ') 41 | # T = T.reshape(3, 4) 42 | # T = np.vstack((T, [0, 0, 0, 1])) 43 | # self.T_w_cam0.append(T) 44 | 45 | # scan = (np.fromfile(pc, dtype=np.float32)).reshape(-1, 4) 46 | 47 | 48 | # print(labels.shape) 49 | # print(pc.shape[0]/4) 50 | # print(labels[:10]) 51 | # print("=============") 52 | # # print(np.amax(labels)) 53 | # # print(np.amin(labels)) 54 | # print(np.unique(labels)) 55 | # 56 | # 57 | label = np.fromfile(labelname, dtype=np.uint32) 58 | # label = label.reshape((-1)) 59 | # if not isinstance(label, np.ndarray): 60 | # raise TypeError("Label should be numpy array") 61 | # 62 | # # only fill in attribute if the right size 63 | sem_label = label & 0xFFFF # semantic label in lower half 64 | inst_label = label >> 16 # instance id in upper half 65 | print(sem_label.shape) 66 | print(inst_label.shape) 67 | # 68 | 69 | # sanity check 70 | # assert ((sem_label + (inst_label << 16) == label).all()) 71 | # print(np.unique(sem_label)) 72 | # print(np.unique(inst_label)) 73 | # 74 | # print(np.unique(sem_label).shape[0]) 75 | # print(np.unique(inst_label).shape[0]) 76 | # 77 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/gen_rosbag.sh: -------------------------------------------------------------------------------- 1 | python kitti2node.py -t None -r None -s 01 --kitti_type "odom_noimg" 2 | python kitti2node.py -t None -r None -s 02 --kitti_type "odom_noimg" 3 | python kitti2node.py -t None -r None -s 03 --kitti_type "odom_noimg" 4 | python kitti2node.py -t None -r None -s 04 --kitti_type "odom_noimg" 5 | python kitti2node.py -t None -r None -s 05 --kitti_type "odom_noimg" 6 | python kitti2node.py -t None -r None -s 06 --kitti_type "odom_noimg" 7 | python kitti2node.py -t None -r None -s 07 --kitti_type "odom_noimg" 8 | python kitti2node.py -t None -r None -s 08 --kitti_type "odom_noimg" 9 | python kitti2node.py -t None -r None -s 09 --kitti_type "odom_noimg" 10 | python kitti2node.py -t None -r None -s 10 --kitti_type "odom_noimg" 11 | 12 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/kitti2bag.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/scripts/semantickitti2bag/kitti2bag.pyc -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/__init__.py: -------------------------------------------------------------------------------- 1 | """Tools for working with KITTI data.""" 2 | 3 | from pykitti.odometry import odometry 4 | from pykitti.raw import raw 5 | 6 | __author__ = "Lee Clement" 7 | __email__ = "lee.clement@robotics.utias.utoronto.ca" 8 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/scripts/semantickitti2bag/pykitti/__init__.pyc -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/odometry.py: -------------------------------------------------------------------------------- 1 | """Provides 'odometry', which loads and parses odometry benchmark data.""" 2 | 3 | import datetime as dt 4 | import glob 5 | import os 6 | from collections import namedtuple 7 | 8 | import numpy as np 9 | 10 | import pykitti.utils as utils 11 | 12 | __author__ = "Lee Clement" 13 | __email__ = "lee.clement@robotics.utias.utoronto.ca" 14 | 15 | 16 | class odometry: 17 | """Load and parse odometry benchmark data into a usable format.""" 18 | 19 | def __init__(self, base_path, sequence, frame_range=None): 20 | """Set the path.""" 21 | self.sequence = sequence 22 | self.sequence_path = os.path.join(base_path, 'sequences', sequence) 23 | self.pose_path = os.path.join(base_path, 'poses_gt') 24 | self.frame_range = frame_range 25 | 26 | def load_calib(self): 27 | """Load and compute intrinsic and extrinsic calibration parameters.""" 28 | # We'll build the calibration parameters as a dictionary, then 29 | # convert it to a namedtuple to prevent it from being modified later 30 | data = {} 31 | 32 | # Load the calibration file 33 | calib_filepath = os.path.join(self.sequence_path, 'calib.txt') 34 | filedata = utils.read_calib_file(calib_filepath) 35 | 36 | # Create 3x4 projection matrices 37 | P_rect_00 = np.reshape(filedata['P0'], (3, 4)) 38 | P_rect_10 = np.reshape(filedata['P1'], (3, 4)) 39 | P_rect_20 = np.reshape(filedata['P2'], (3, 4)) 40 | P_rect_30 = np.reshape(filedata['P3'], (3, 4)) 41 | 42 | # Compute the rectified extrinsics from cam0 to camN 43 | T1 = np.eye(4) 44 | T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0] 45 | T2 = np.eye(4) 46 | T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0] 47 | T3 = np.eye(4) 48 | T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0] 49 | 50 | # Compute the velodyne to rectified camera coordinate transforms 51 | data['T_cam0_velo'] = np.reshape(filedata['Tr'], (3, 4)) 52 | data['T_cam0_velo'] = np.vstack([data['T_cam0_velo'], [0, 0, 0, 1]]) 53 | self.T_cam0_velo = data['T_cam0_velo']; 54 | data['T_cam1_velo'] = T1.dot(data['T_cam0_velo']) 55 | data['T_cam2_velo'] = T2.dot(data['T_cam0_velo']) 56 | data['T_cam3_velo'] = T3.dot(data['T_cam0_velo']) 57 | 58 | # Compute the camera intrinsics 59 | data['K_cam0'] = P_rect_00[0:3, 0:3] 60 | data['K_cam1'] = P_rect_10[0:3, 0:3] 61 | data['K_cam2'] = P_rect_20[0:3, 0:3] 62 | data['K_cam3'] = P_rect_30[0:3, 0:3] 63 | 64 | # Compute the stereo baselines in meters by projecting the origin of 65 | # each camera frame into the velodyne frame and computing the distances 66 | # between them 67 | p_cam = np.array([0, 0, 0, 1]) 68 | p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam) 69 | p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam) 70 | p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam) 71 | p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam) 72 | 73 | data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0) # gray baseline 74 | data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2) # rgb baseline 75 | 76 | self.calib = namedtuple('CalibData', data.keys())(*data.values()) 77 | 78 | def load_timestamps(self): 79 | """Load timestamps from file.""" 80 | print('Loading timestamps for sequence ' + self.sequence + '...') 81 | 82 | timestamp_file = os.path.join(self.sequence_path, 'times.txt') 83 | 84 | # Read and parse the timestamps 85 | self.timestamps = [] 86 | with open(timestamp_file, 'r') as f: 87 | for line in f.readlines(): 88 | t = dt.timedelta(seconds=float(line)) 89 | self.timestamps.append(t) 90 | 91 | # Subselect the chosen range of frames, if any 92 | if self.frame_range: 93 | self.timestamps = [self.timestamps[i] for i in self.frame_range] 94 | 95 | print('Found ' + str(len(self.timestamps)) + ' timestamps...') 96 | 97 | print('done.') 98 | 99 | def load_poses(self): 100 | """Load ground truth poses from file.""" 101 | print('Loading GT poses for sequence ' + self.sequence + '...') 102 | 103 | pose_file = os.path.join(self.pose_path, self.sequence + '.txt') 104 | print(pose_file) 105 | 106 | # Read and parse the poses 107 | try: 108 | self.T_w_cam0 = [] 109 | with open(pose_file, 'r') as f: 110 | for line in f.readlines(): 111 | T = np.fromstring(line, dtype=float, sep=' ') 112 | T = T.reshape(3, 4) 113 | T = np.vstack((T, [0, 0, 0, 1])) 114 | self.T_w_cam0.append(T) 115 | if self.frame_range: 116 | self.T_w_cam0 = [self.T_w_cam0[i] for i in self.frame_range] 117 | print('done.') 118 | 119 | 120 | except IOError: 121 | print('Ground truth poses are not avaialble for sequence ' + 122 | self.sequence + '.') 123 | 124 | def load_gray(self, **kwargs): 125 | """Load monochrome stereo images from file. 126 | 127 | Setting imformat='cv2' will convert the images to uint8 for 128 | easy use with OpenCV. 129 | """ 130 | print('Loading monochrome images from sequence ' + 131 | self.sequence + '...') 132 | 133 | imL_path = os.path.join(self.sequence_path, 'image_0', '*.png') 134 | imR_path = os.path.join(self.sequence_path, 'image_1', '*.png') 135 | 136 | imL_files = sorted(glob.glob(imL_path)) 137 | imR_files = sorted(glob.glob(imR_path)) 138 | 139 | # Subselect the chosen range of frames, if any 140 | if self.frame_range: 141 | imL_files = [imL_files[i] for i in self.frame_range] 142 | imR_files = [imR_files[i] for i in self.frame_range] 143 | 144 | print('Found ' + str(len(imL_files)) + ' image pairs...') 145 | 146 | self.gray = utils.load_stereo_pairs(imL_files, imR_files, **kwargs) 147 | 148 | print('done.') 149 | 150 | def load_rgb(self, **kwargs): 151 | """Load RGB stereo images from file. 152 | 153 | Setting imformat='cv2' will convert the images to uint8 and BGR for 154 | easy use with OpenCV. 155 | """ 156 | print('Loading color images from sequence ' + 157 | self.sequence + '...') 158 | 159 | imL_path = os.path.join(self.sequence_path, 'image_2', '*.png') 160 | imR_path = os.path.join(self.sequence_path, 'image_3', '*.png') 161 | 162 | imL_files = sorted(glob.glob(imL_path)) 163 | imR_files = sorted(glob.glob(imR_path)) 164 | 165 | # Subselect the chosen range of frames, if any 166 | if self.frame_range: 167 | imL_files = [imL_files[i] for i in self.frame_range] 168 | imR_files = [imR_files[i] for i in self.frame_range] 169 | 170 | print('Found ' + str(len(imL_files)) + ' image pairs...') 171 | 172 | self.rgb = utils.load_stereo_pairs(imL_files, imR_files, **kwargs) 173 | 174 | print('done.') 175 | 176 | def load_velo(self): 177 | """Load velodyne [x,y,z,reflectance] scan data from binary files.""" 178 | # Find all the Velodyne files 179 | velo_path = os.path.join(self.sequence_path, 'velodyne', '*.bin') 180 | velo_files = sorted(glob.glob(velo_path)) 181 | 182 | # Subselect the chosen range of frames, if any 183 | if self.frame_range: 184 | velo_files = [velo_files[i] for i in self.frame_range] 185 | 186 | print('Found ' + str(len(velo_files)) + ' Velodyne scans...') 187 | 188 | # Read the Velodyne scans. Each point is [x,y,z,reflectance] 189 | self.velo = utils.load_velo_scans(velo_files) 190 | 191 | print('done.') 192 | 193 | def load_label(self): 194 | """Load velodyne label from binary files.""" 195 | # Find all the label files 196 | label_path = os.path.join(self.sequence_path, 'labels', '*.label') 197 | label_files = sorted(glob.glob(label_path)) 198 | 199 | # Subselect the chosen range of frames, if any 200 | if self.frame_range: 201 | label_files = [label_files[i] for i in self.frame_range] 202 | 203 | print('Found ' + str(len(label_files)) + ' labels of scans...') 204 | 205 | self.label = utils.load_labels(label_files) 206 | 207 | print('done.') 208 | 209 | if __name__ == "__main__": 210 | 211 | abs_path = "/media/shapelim/UX960NVMe1/kitti_semantic/dataset/sequences/01" 212 | output_path = "/media/shapelim/UX960NVMe1/changgi_intensity/01" 213 | label_path = os.path.join(abs_path, 'labels', '*.label') 214 | label_files = sorted(glob.glob(label_path))[:10] 215 | print('Found ' + str(len(label_files)) + ' labels of scans...') 216 | 217 | label = utils.load_labels(label_files) 218 | 219 | velo_path = os.path.join(abs_path, 'velodyne', '*.bin') 220 | velo_files = sorted(glob.glob(velo_path))[:10] 221 | 222 | velo = utils.load_velo_scans(velo_files) 223 | print("Done") 224 | import pandas as pd 225 | count = 0 226 | for (label_data, scan_data) in zip(label, velo): 227 | if (count%10 == 0): 228 | print("On conducting ", count, "th operation...") 229 | sem_label = label_data & 0xFFFF 230 | inst_id = label_data >> 16 231 | is_traffic = (sem_label == 81) 232 | 233 | xyz = scan_data[:, :3] 234 | range = np.reshape(np.sqrt(np.sum(np.square(xyz), axis=1)), (-1, 1)) 235 | intensity = scan_data[:, 3:] 236 | print(range.shape, intensity.shape) 237 | 238 | traffic_inten = np.reshape(intensity[is_traffic], (-1, 1)) 239 | traffic_range = np.reshape(range[is_traffic], (-1, 1)) 240 | traffic_id = np.reshape(inst_id[is_traffic], (-1, 1)) 241 | traffic_info = np.concatenate([traffic_inten, traffic_range, traffic_id], axis=1) 242 | nontraffic_inten = np.reshape(intensity[np.logical_not(is_traffic)], (-1,1)) 243 | 244 | # print(traffic_inten.shape) 245 | # print(nontraffic_inten.shape) 246 | df_traffic = pd.DataFrame(traffic_inten) 247 | df_nontraffic = pd.DataFrame(nontraffic_inten) 248 | t_fname = output_path + "/traffic_sign/" + str(count).zfill(4) + ".csv" 249 | nt_fname = output_path + "/nontraffic_sign/" + str(count).zfill(4) + ".csv" 250 | df_traffic.to_csv(t_fname, index=False) 251 | df_nontraffic.to_csv(nt_fname, index=False) 252 | 253 | count += 1 254 | 255 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/odometry.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/scripts/semantickitti2bag/pykitti/odometry.pyc -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/raw.py: -------------------------------------------------------------------------------- 1 | """Provides 'raw', which loads and parses raw KITTI data.""" 2 | 3 | import datetime as dt 4 | import glob 5 | import os 6 | from collections import namedtuple 7 | 8 | import numpy as np 9 | 10 | import pykitti.utils as utils 11 | 12 | __author__ = "Lee Clement" 13 | __email__ = "lee.clement@robotics.utias.utoronto.ca" 14 | 15 | 16 | class raw: 17 | """Load and parse raw data into a usable format.""" 18 | 19 | def __init__(self, base_path, date, drive, frame_range=None): 20 | """Set the path.""" 21 | self.drive = date + '_drive_' + drive + '_sync' 22 | self.calib_path = os.path.join(base_path, date) 23 | self.data_path = os.path.join(base_path, date, self.drive) 24 | self.frame_range = frame_range 25 | 26 | def _load_calib_rigid(self, filename): 27 | """Read a rigid transform calibration file as a numpy.array.""" 28 | filepath = os.path.join(self.calib_path, filename) 29 | data = utils.read_calib_file(filepath) 30 | return utils.transform_from_rot_trans(data['R'], data['T']) 31 | 32 | def _load_calib_cam_to_cam(self, velo_to_cam_file, cam_to_cam_file): 33 | # We'll return the camera calibration as a dictionary 34 | data = {} 35 | 36 | # Load the rigid transformation from velodyne coordinates 37 | # to unrectified cam0 coordinates 38 | T_cam0unrect_velo = self._load_calib_rigid(velo_to_cam_file) 39 | 40 | # Load and parse the cam-to-cam calibration data 41 | cam_to_cam_filepath = os.path.join(self.calib_path, cam_to_cam_file) 42 | filedata = utils.read_calib_file(cam_to_cam_filepath) 43 | 44 | # Create 3x4 projection matrices 45 | P_rect_00 = np.reshape(filedata['P_rect_00'], (3, 4)) 46 | P_rect_10 = np.reshape(filedata['P_rect_01'], (3, 4)) 47 | P_rect_20 = np.reshape(filedata['P_rect_02'], (3, 4)) 48 | P_rect_30 = np.reshape(filedata['P_rect_03'], (3, 4)) 49 | 50 | # Create 4x4 matrix from the rectifying rotation matrix 51 | R_rect_00 = np.eye(4) 52 | R_rect_00[0:3, 0:3] = np.reshape(filedata['R_rect_00'], (3, 3)) 53 | 54 | # Compute the rectified extrinsics from cam0 to camN 55 | T0 = np.eye(4) 56 | T0[0, 3] = P_rect_00[0, 3] / P_rect_00[0, 0] 57 | T1 = np.eye(4) 58 | T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0] 59 | T2 = np.eye(4) 60 | T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0] 61 | T3 = np.eye(4) 62 | T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0] 63 | 64 | # Compute the velodyne to rectified camera coordinate transforms 65 | data['T_cam0_velo'] = T0.dot(R_rect_00.dot(T_cam0unrect_velo)) 66 | data['T_cam1_velo'] = T1.dot(R_rect_00.dot(T_cam0unrect_velo)) 67 | data['T_cam2_velo'] = T2.dot(R_rect_00.dot(T_cam0unrect_velo)) 68 | data['T_cam3_velo'] = T3.dot(R_rect_00.dot(T_cam0unrect_velo)) 69 | 70 | # Compute the camera intrinsics 71 | data['K_cam0'] = P_rect_00[0:3, 0:3] 72 | data['K_cam1'] = P_rect_10[0:3, 0:3] 73 | data['K_cam2'] = P_rect_20[0:3, 0:3] 74 | data['K_cam3'] = P_rect_30[0:3, 0:3] 75 | 76 | # Compute the stereo baselines in meters by projecting the origin of 77 | # each camera frame into the velodyne frame and computing the distances 78 | # between them 79 | p_cam = np.array([0, 0, 0, 1]) 80 | p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam) 81 | p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam) 82 | p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam) 83 | p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam) 84 | 85 | data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0) # gray baseline 86 | data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2) # rgb baseline 87 | 88 | return data 89 | 90 | def load_calib(self): 91 | """Load and compute intrinsic and extrinsic calibration parameters.""" 92 | # We'll build the calibration parameters as a dictionary, then 93 | # convert it to a namedtuple to prevent it from being modified later 94 | data = {} 95 | 96 | # Load the rigid transformation from velodyne to IMU 97 | data['T_velo_imu'] = self._load_calib_rigid('calib_imu_to_velo.txt') 98 | 99 | # Load the camera intrinsics and extrinsics 100 | data.update(self._load_calib_cam_to_cam( 101 | 'calib_velo_to_cam.txt', 'calib_cam_to_cam.txt')) 102 | 103 | # Pre-compute the IMU to rectified camera coordinate transforms 104 | data['T_cam0_imu'] = data['T_cam0_velo'].dot(data['T_velo_imu']) 105 | data['T_cam1_imu'] = data['T_cam1_velo'].dot(data['T_velo_imu']) 106 | data['T_cam2_imu'] = data['T_cam2_velo'].dot(data['T_velo_imu']) 107 | data['T_cam3_imu'] = data['T_cam3_velo'].dot(data['T_velo_imu']) 108 | 109 | self.calib = namedtuple('CalibData', data.keys())(*data.values()) 110 | 111 | def load_timestamps(self): 112 | """Load timestamps from file.""" 113 | print('Loading OXTS timestamps from ' + self.drive + '...') 114 | 115 | timestamp_file = os.path.join( 116 | self.data_path, 'oxts', 'timestamps.txt') 117 | 118 | # Read and parse the timestamps 119 | self.timestamps = [] 120 | with open(timestamp_file, 'r') as f: 121 | for line in f.readlines(): 122 | # NB: datetime only supports microseconds, but KITTI timestamps 123 | # give nanoseconds, so need to truncate last 4 characters to 124 | # get rid of \n (counts as 1) and extra 3 digits 125 | t = dt.datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f') 126 | self.timestamps.append(t) 127 | 128 | # Subselect the chosen range of frames, if any 129 | if self.frame_range: 130 | self.timestamps = [self.timestamps[i] for i in self.frame_range] 131 | 132 | print('Found ' + str(len(self.timestamps)) + ' timestamps...') 133 | 134 | print('done.') 135 | 136 | def _poses_from_oxts(self, oxts_packets): 137 | """Helper method to compute SE(3) pose matrices from OXTS packets.""" 138 | er = 6378137. # earth radius (approx.) in meters 139 | 140 | # compute scale from first lat value 141 | scale = np.cos(oxts_packets[0].lat * np.pi / 180.) 142 | 143 | t_0 = [] # initial position 144 | poses = [] # list of poses computed from oxts 145 | for packet in oxts_packets: 146 | # Use a Mercator projection to get the translation vector 147 | tx = scale * packet.lon * np.pi * er / 180. 148 | ty = scale * er * \ 149 | np.log(np.tan((90. + packet.lat) * np.pi / 360.)) 150 | tz = packet.alt 151 | t = np.array([tx, ty, tz]) 152 | 153 | # We want the initial position to be the origin, but keep the ENU 154 | # coordinate system 155 | if len(t_0) == 0: 156 | t_0 = t 157 | 158 | # Use the Euler angles to get the rotation matrix 159 | Rx = utils.rotx(packet.roll) 160 | Ry = utils.roty(packet.pitch) 161 | Rz = utils.rotz(packet.yaw) 162 | R = Rz.dot(Ry.dot(Rx)) 163 | 164 | # Combine the translation and rotation into a homogeneous transform 165 | poses.append(utils.transform_from_rot_trans(R, t - t_0)) 166 | 167 | return poses 168 | 169 | def load_oxts(self): 170 | """Load OXTS data from file.""" 171 | print('Loading OXTS data from ' + self.drive + '...') 172 | 173 | # Find all the data files 174 | oxts_path = os.path.join(self.data_path, 'oxts', 'data', '*.txt') 175 | oxts_files = sorted(glob.glob(oxts_path)) 176 | 177 | # Subselect the chosen range of frames, if any 178 | if self.frame_range: 179 | oxts_files = [oxts_files[i] for i in self.frame_range] 180 | 181 | print('Found ' + str(len(oxts_files)) + ' OXTS measurements...') 182 | 183 | # Extract the data from each OXTS packet 184 | # Per dataformat.txt 185 | OxtsPacket = namedtuple('OxtsPacket', 186 | 'lat, lon, alt, ' + 187 | 'roll, pitch, yaw, ' + 188 | 'vn, ve, vf, vl, vu, ' + 189 | 'ax, ay, az, af, al, au, ' + 190 | 'wx, wy, wz, wf, wl, wu, ' + 191 | 'pos_accuracy, vel_accuracy, ' + 192 | 'navstat, numsats, ' + 193 | 'posmode, velmode, orimode') 194 | 195 | oxts_packets = [] 196 | for filename in oxts_files: 197 | with open(filename, 'r') as f: 198 | for line in f.readlines(): 199 | line = line.split() 200 | # Last five entries are flags and counts 201 | line[:-5] = [float(x) for x in line[:-5]] 202 | line[-5:] = [int(float(x)) for x in line[-5:]] 203 | 204 | data = OxtsPacket(*line) 205 | oxts_packets.append(data) 206 | 207 | # Precompute the IMU poses in the world frame 208 | T_w_imu = self._poses_from_oxts(oxts_packets) 209 | 210 | # Bundle into an easy-to-access structure 211 | OxtsData = namedtuple('OxtsData', 'packet, T_w_imu') 212 | self.oxts = [] 213 | for (p, T) in zip(oxts_packets, T_w_imu): 214 | self.oxts.append(OxtsData(p, T)) 215 | 216 | print('done.') 217 | 218 | def load_gray(self, **kwargs): 219 | """Load monochrome stereo images from file. 220 | 221 | Setting imformat='cv2' will convert the images to uint8 for 222 | easy use with OpenCV. 223 | """ 224 | print('Loading monochrome images from ' + self.drive + '...') 225 | 226 | imL_path = os.path.join(self.data_path, 'image_00', 'data', '*.png') 227 | imR_path = os.path.join(self.data_path, 'image_01', 'data', '*.png') 228 | 229 | imL_files = sorted(glob.glob(imL_path)) 230 | imR_files = sorted(glob.glob(imR_path)) 231 | 232 | # Subselect the chosen range of frames, if any 233 | if self.frame_range: 234 | imL_files = [imL_files[i] for i in self.frame_range] 235 | imR_files = [imR_files[i] for i in self.frame_range] 236 | 237 | print('Found ' + str(len(imL_files)) + ' image pairs...') 238 | 239 | self.gray = utils.load_stereo_pairs(imL_files, imR_files, **kwargs) 240 | 241 | print('done.') 242 | 243 | def load_rgb(self, **kwargs): 244 | """Load RGB stereo images from file. 245 | 246 | Setting imformat='cv2' will convert the images to uint8 and BGR for 247 | easy use with OpenCV. 248 | """ 249 | print('Loading color images from ' + self.drive + '...') 250 | 251 | imL_path = os.path.join(self.data_path, 'image_02', 'data', '*.png') 252 | imR_path = os.path.join(self.data_path, 'image_03', 'data', '*.png') 253 | 254 | imL_files = sorted(glob.glob(imL_path)) 255 | imR_files = sorted(glob.glob(imR_path)) 256 | 257 | # Subselect the chosen range of frames, if any 258 | if self.frame_range: 259 | imL_files = [imL_files[i] for i in self.frame_range] 260 | imR_files = [imR_files[i] for i in self.frame_range] 261 | 262 | print('Found ' + str(len(imL_files)) + ' image pairs...') 263 | 264 | self.rgb = utils.load_stereo_pairs(imL_files, imR_files, **kwargs) 265 | 266 | print('done.') 267 | 268 | def load_velo(self): 269 | """Load velodyne [x,y,z,reflectance] scan data from binary files.""" 270 | # Find all the Velodyne files 271 | velo_path = os.path.join( 272 | self.data_path, 'velodyne_points', 'data', '*.bin') 273 | velo_files = sorted(glob.glob(velo_path)) 274 | 275 | # Subselect the chosen range of frames, if any 276 | if self.frame_range: 277 | velo_files = [velo_files[i] for i in self.frame_range] 278 | 279 | print('Found ' + str(len(velo_files)) + ' Velodyne scans...') 280 | 281 | # Read the Velodyne scans. Each point is [x,y,z,reflectance] 282 | self.velo = utils.load_velo_scans(velo_files) 283 | 284 | print('done.') 285 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/raw.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/scripts/semantickitti2bag/pykitti/raw.pyc -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/utils.py: -------------------------------------------------------------------------------- 1 | """Provides helper methods for loading and parsing KITTI data.""" 2 | 3 | from collections import namedtuple 4 | 5 | import matplotlib.image as mpimg 6 | import numpy as np 7 | from tqdm import tqdm 8 | 9 | __author__ = "Lee Clement" 10 | __email__ = "lee.clement@robotics.utias.utoronto.ca" 11 | 12 | 13 | def rotx(t): 14 | """Rotation about the x-axis.""" 15 | c = np.cos(t) 16 | s = np.sin(t) 17 | return np.array([[1, 0, 0], 18 | [0, c, -s], 19 | [0, s, c]]) 20 | 21 | 22 | def roty(t): 23 | """Rotation about the y-axis.""" 24 | c = np.cos(t) 25 | s = np.sin(t) 26 | return np.array([[c, 0, s], 27 | [0, 1, 0], 28 | [-s, 0, c]]) 29 | 30 | 31 | def rotz(t): 32 | """Rotation about the z-axis.""" 33 | c = np.cos(t) 34 | s = np.sin(t) 35 | return np.array([[c, -s, 0], 36 | [s, c, 0], 37 | [0, 0, 1]]) 38 | 39 | 40 | def transform_from_rot_trans(R, t): 41 | """Transforation matrix from rotation matrix and translation vector.""" 42 | R = R.reshape(3, 3) 43 | t = t.reshape(3, 1) 44 | return np.vstack((np.hstack([R, t]), [0, 0, 0, 1])) 45 | 46 | 47 | def read_calib_file(filepath): 48 | """Read in a calibration file and parse into a dictionary.""" 49 | data = {} 50 | 51 | with open(filepath, 'r') as f: 52 | for line in f.readlines(): 53 | key, value = line.split(':', 1) 54 | # The only non-float values in these files are dates, which 55 | # we don't care about anyway 56 | try: 57 | data[key] = np.array([float(x) for x in value.split()]) 58 | except ValueError: 59 | pass 60 | 61 | return data 62 | 63 | 64 | def load_stereo_pairs(imL_files, imR_files, **kwargs): 65 | """Helper method to read stereo image pairs.""" 66 | StereoPair = namedtuple('StereoPair', 'left, right') 67 | 68 | impairs = [] 69 | for imfiles in zip(imL_files, imR_files): 70 | # Convert to uint8 and BGR for OpenCV if requested 71 | imformat = kwargs.get('format', '') 72 | if imformat is 'cv2': 73 | imL = np.uint8(mpimg.imread(imfiles[0]) * 255) 74 | imR = np.uint8(mpimg.imread(imfiles[1]) * 255) 75 | 76 | # Convert RGB to BGR 77 | if len(imL.shape) > 2: 78 | imL = imL[:, :, ::-1] 79 | imR = imR[:, :, ::-1] 80 | 81 | else: 82 | imL = mpimg.imread(imfiles[0]) 83 | imR = mpimg.imread(imfiles[1]) 84 | 85 | impairs.append(StereoPair(imL, imR)) 86 | 87 | return impairs 88 | 89 | 90 | def load_velo_scans(velo_files): 91 | """Helper method to parse velodyne binary files into a list of scans.""" 92 | scan_list = [] 93 | for i in tqdm(range(len(velo_files))): 94 | filename = velo_files[i] 95 | print("velo: ", filename) 96 | scan = np.fromfile(filename, dtype=np.float32) 97 | scan_list.append(scan.reshape((-1, 4))) 98 | 99 | return scan_list 100 | import csv 101 | 102 | def load_labels(label_files): 103 | """Helper method to parse velodyne binary files into a list of label.""" 104 | label_list = [] 105 | 106 | for i in tqdm(range(len(label_files))): 107 | filename = label_files[i] 108 | print("label: ", filename) 109 | pc_label = np.fromfile(filename, dtype=np.uint32) 110 | 111 | # # # # # Debug # # # # # 112 | # with open("/home/shapelim/hdd2/kitti_semantic/dataset/debug_python/" + str(i)+".csv", "w") as csvfile: 113 | # wr = csv.writer(csvfile) 114 | # semantic_label = pc_label & 0xFFFF 115 | # inst_label = pc_label >> 16 116 | # for j in range(semantic_label.shape[0]): 117 | # wr.writerow([semantic_label[j], inst_label[j]]) 118 | # # # # # # # # # # # # # 119 | 120 | label_list.append(pc_label.reshape((-1, 1))) 121 | return label_list 122 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/pykitti/utils.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/scripts/semantickitti2bag/pykitti/utils.pyc -------------------------------------------------------------------------------- /scripts/semantickitti2bag/scribble.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | from tqdm import tqdm 5 | a= np.array([1, 2]) 6 | b= np.array([1, 43]) 7 | for q, w, z in tqdm(zip(a, b, b)): 8 | print(q, w, z) -------------------------------------------------------------------------------- /scripts/semantickitti2bag/semantic_kitti_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def intensity2labels(intensity_np): 4 | label = intensity_np.astype(np.uint32) 5 | sem_label = label & 0xFFFF # semantic label in lower half 6 | inst_label = label >> 16 # instance id in upper half 7 | return sem_label, inst_label 8 | -------------------------------------------------------------------------------- /scripts/semantickitti2bag/semantic_kitti_utils.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/Ground-Segmentation-Benchmark/af253dfad8d84668137a59295e41599a26558704/scripts/semantickitti2bag/semantic_kitti_utils.pyc -------------------------------------------------------------------------------- /shellscripts/autosave_all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "cascaded_gseg" 7 | 8 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 9 | do 10 | rosparam set /sequence ${seq} 11 | sleep 1 12 | rosrun gseg_benchmark benchmark_offline 13 | done 14 | 15 | rosparam set /algorithm "gpf" 16 | rosparam set /gpf/mode "multiple" # "single" or "multiple" 17 | 18 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 19 | do 20 | rosparam set /sequence ${seq} 21 | sleep 1 22 | rosrun gseg_benchmark benchmark_offline 23 | done 24 | 25 | rosparam set /algorithm "gpregression" 26 | 27 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 28 | do 29 | rosparam set /sequence ${seq} 30 | sleep 1 31 | rosrun gseg_benchmark benchmark_offline 32 | done 33 | 34 | rosparam set /algorithm "linefit" 35 | 36 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 37 | do 38 | rosparam set /sequence ${seq} 39 | sleep 1 40 | rosrun gseg_benchmark benchmark_offline 41 | done 42 | 43 | rosparam set /algorithm "patchwork" 44 | 45 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 46 | do 47 | rosparam set /sequence ${seq} 48 | sleep 1 49 | rosrun gseg_benchmark benchmark_offline 50 | done 51 | 52 | rosparam set /algorithm "r_gpf" 53 | 54 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 55 | do 56 | rosparam set /sequence ${seq} 57 | sleep 1 58 | rosrun gseg_benchmark benchmark_offline 59 | done 60 | 61 | rosparam set /algorithm "ransac" 62 | 63 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 64 | do 65 | rosparam set /sequence ${seq} 66 | sleep 1 67 | rosrun gseg_benchmark benchmark_offline 68 | done 69 | -------------------------------------------------------------------------------- /shellscripts/autosave_cascaded_gseg.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "cascaded_gseg" 7 | 8 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 9 | do 10 | rosparam set /sequence ${seq} 11 | sleep 1 12 | rosrun gseg_benchmark benchmark_offline 13 | done 14 | -------------------------------------------------------------------------------- /shellscripts/autosave_gpf.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "gpf" 7 | rosparam set /gpf/mode "multiple" # "single" or "multiple" 8 | 9 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 10 | do 11 | rosparam set /sequence ${seq} 12 | sleep 1 13 | rosrun gseg_benchmark benchmark_offline 14 | done 15 | -------------------------------------------------------------------------------- /shellscripts/autosave_gpregression.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "gpregression" 7 | 8 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 9 | do 10 | rosparam set /sequence ${seq} 11 | sleep 1 12 | rosrun gseg_benchmark benchmark_offline 13 | done 14 | -------------------------------------------------------------------------------- /shellscripts/autosave_linefit.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "linefit" 7 | 8 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 9 | do 10 | rosparam set /sequence ${seq} 11 | sleep 1 12 | rosrun gseg_benchmark benchmark_offline 13 | done 14 | -------------------------------------------------------------------------------- /shellscripts/autosave_patchwork.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "patchwork" 7 | 8 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 9 | do 10 | rosparam set /sequence ${seq} 11 | sleep 1 12 | rosrun gseg_benchmark benchmark_offline 13 | done 14 | -------------------------------------------------------------------------------- /shellscripts/autosave_r_gpf.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "r_gpf" 7 | 8 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 9 | do 10 | rosparam set /sequence ${seq} 11 | sleep 1 12 | rosrun gseg_benchmark benchmark_offline 13 | done 14 | -------------------------------------------------------------------------------- /shellscripts/autosave_ransac.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | GSEG_BENCHMARK_PATH=$(rospack find gseg_benchmark) 3 | rosparam load $GSEG_BENCHMARK_PATH/config/params.yaml 4 | bash $GSEG_BENCHMARK_PATH/shellscripts/common.sh 5 | 6 | rosparam set /algorithm "ransac" 7 | 8 | for seq in "'00'" "'01'" "'02'" "'03'" "'04'" "'05'" "'06'" "'07'" "'08'" "'09'" "'10'" 9 | do 10 | rosparam set /sequence ${seq} 11 | sleep 1 12 | rosrun gseg_benchmark benchmark_offline 13 | done 14 | -------------------------------------------------------------------------------- /shellscripts/common.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo "Loading common parameters for ground segmentation benchmark..." 3 | 4 | # Use absolute directory path for data_path including HOME path if you are not using SSD 5 | rosparam set /data_path "/home/user/data/SemanticKITTI/" # path of downloaded KITTI dataset. It must include '/' at the end part 6 | 7 | rosparam set /init_idx 0 # index of the first frame to run 8 | 9 | rosparam set /stop_for_each_frame false # set as 'true' to make it stop every frame 10 | 11 | rosparam set /save_csv_file false # set as 'false' if csv output files are not needed 12 | 13 | rosparam set /save_pcd_flag false # set as 'false' if csv output files are not needed 14 | 15 | # Use path relative to HOME directory 16 | rosparam set /output_path "/data/" # path of output files to be generated 17 | 18 | echo "Loading complete!" -------------------------------------------------------------------------------- /src/lib/cvt.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jeewon on 22. 3. 20.. 3 | // 4 | 5 | #ifndef GSEG_BENCHMARK_CVT_H 6 | #define GSEG_BENCHMARK_CVT_H 7 | 8 | #include 9 | 10 | namespace cvt { 11 | template 12 | pcl::PointCloud cloudmsg2cloud(sensor_msgs::PointCloud2 cloudmsg) 13 | { 14 | pcl::PointCloud cloudresult; 15 | pcl::fromROSMsg(cloudmsg,cloudresult); 16 | return cloudresult; 17 | } 18 | template 19 | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id = "map") 20 | { 21 | sensor_msgs::PointCloud2 cloud_ROS; 22 | pcl::toROSMsg(cloud, cloud_ROS); 23 | cloud_ROS.header.frame_id = frame_id; 24 | return cloud_ROS; 25 | } 26 | } 27 | 28 | #endif //GSEG_BENCHMARK_CVT_H 29 | -------------------------------------------------------------------------------- /src/lib/datahandle.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jeewon on 22. 3. 20.. 3 | // 4 | 5 | #ifndef GSEG_BENCHMARK_DATAHANDLE_H 6 | #define GSEG_BENCHMARK_DATAHANDLE_H 7 | 8 | #include 9 | #include 10 | 11 | 12 | namespace datahandle3d { 13 | template 14 | int load_pcd(std::string pcd_name, boost::shared_ptr< pcl::PointCloud< PointT > > dst){ 15 | if (pcl::io::loadPCDFile (pcd_name, *dst) == -1) 16 | { 17 | PCL_ERROR ("Couldn't read file!!! \n"); 18 | return (-1); 19 | } 20 | std::cout << "Loaded " << dst->size () << " data points from " < 2 | // For disable PCL complile lib, to use PointXYZIR 3 | #define PCL_NO_PRECOMPILE 4 | 5 | #include 6 | #include "cascadedseg/cascaded_groundseg.hpp" 7 | #include "gpf/groundplanefit.hpp" 8 | #include "r_gpf/r_gpf.hpp" 9 | #include "ransac/ransac_gpf.hpp" 10 | #include "patchwork/patchwork.hpp" 11 | #include "gpregression/GaussianFloorSegmentation.h" 12 | #include "lib/cvt.h" 13 | 14 | using namespace std; 15 | 16 | ros::Publisher CloudPublisher; 17 | ros::Publisher TPPublisher; 18 | ros::Publisher FPPublisher; 19 | ros::Publisher FNPublisher; 20 | ros::Publisher OutputPublisher; 21 | ros::Publisher PrecisionPublisher; 22 | ros::Publisher RecallPublisher; 23 | 24 | 25 | boost::shared_ptr gpf; 26 | boost::shared_ptr r_gpf; 27 | boost::shared_ptr ransac_gpf; 28 | boost::shared_ptr sp; 29 | boost::shared_ptr cascaded_gseg; 30 | boost::shared_ptr> gpregression; 31 | 32 | std::string output_filename; 33 | std::string acc_filename, pcd_savepath; 34 | string algorithm; 35 | string mode; 36 | string seq; 37 | string output_csvpath; 38 | string w_veg_path, wo_veg_path; 39 | bool save_flag; 40 | bool use_z_thr; 41 | bool save_csv_file; 42 | 43 | void pub_score(std::string mode, double measure) { 44 | static int SCALE = 5; 45 | visualization_msgs::Marker marker; 46 | marker.header.frame_id = "map"; 47 | marker.header.stamp = ros::Time(); 48 | marker.ns = "my_namespace"; 49 | marker.id = 0; 50 | marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 51 | marker.action = visualization_msgs::Marker::ADD; 52 | if (mode == "p") marker.pose.position.x = 28.5; 53 | if (mode == "r") marker.pose.position.x = 25; 54 | marker.pose.position.y = 30; 55 | 56 | marker.pose.position.z = 1; 57 | marker.pose.orientation.x = 0.0; 58 | marker.pose.orientation.y = 0.0; 59 | marker.pose.orientation.z = 0.0; 60 | marker.pose.orientation.w = 1.0; 61 | marker.scale.x = SCALE; 62 | marker.scale.y = SCALE; 63 | marker.scale.z = SCALE; 64 | marker.color.a = 1.0; // Don't forget to set the alpha! 65 | marker.color.r = 0.0; 66 | marker.color.g = 1.0; 67 | marker.color.b = 0.0; 68 | //only if using a MESH_RESOURCE marker type: 69 | marker.text = mode + ": " + std::to_string(measure); 70 | if (mode == "p") PrecisionPublisher.publish(marker); 71 | if (mode == "r") RecallPublisher.publish(marker); 72 | 73 | } 74 | 75 | 76 | void callbackNode(const gseg_benchmark::node::ConstPtr &msg) { 77 | int n = msg->header.seq; 78 | cout << n << "th node come" << endl; 79 | pcl::PointCloud pc_curr = cvt::cloudmsg2cloud(msg->lidar); 80 | pcl::PointCloud pc_ground; 81 | pcl::PointCloud pc_non_ground; 82 | 83 | static double time_taken; 84 | if (algorithm == "gpf") { 85 | cout << "Operating gpf..." << endl; 86 | gpf->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 87 | } else if (algorithm == "r_gpf") { 88 | cout << "Operating r-gpf..." << endl; 89 | r_gpf->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 90 | } else if (algorithm == "ransac") { 91 | cout << "Operating ransac..." << endl; 92 | ransac_gpf->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 93 | } else if (algorithm == "patchwork") { 94 | cout << "Operating patchwork..." << endl; 95 | sp->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 96 | } else if (algorithm == "cascaded_gseg") { 97 | cout << "Operating cascaded_gseg..." << endl; 98 | int num1 = (int) pc_curr.size(); 99 | cascaded_gseg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 100 | pc_curr.points.clear(); 101 | pc_curr = pc_ground + pc_non_ground; 102 | int num2 = (int) pc_curr.size(); 103 | cout << "\033[1;33m" << "point num diff: " << num1 - num2 << "\033[0m" << endl; 104 | } else if (algorithm == "gpregression") { 105 | cout << "Operating gpregression..." <estimate_ground(pc_curr,pc_ground, pc_non_ground, time_taken); 107 | } 108 | // Estimation 109 | static double precision, recall, precision_wo_veg, recall_wo_veg; 110 | static vector TPFNs; // TP, FP, FN, TF order 111 | static vector TPFNs_wo_veg; // TP, FP, FN, TF order 112 | 113 | calculate_precision_recall(pc_curr, pc_ground, precision, recall, TPFNs); 114 | calculate_precision_recall_without_vegetation(pc_curr, pc_ground, precision_wo_veg, recall_wo_veg, TPFNs_wo_veg); 115 | 116 | cout << "\033[1;32m" << n << "th, " << " takes : " << time_taken << " | " << pc_curr.size() << " -> " 117 | << pc_ground.size() 118 | << "\033[0m" << endl; 119 | 120 | cout << "\033[1;32m [W/ Vegi.] P: " << precision << " | R: " << recall << "\033[0m" << endl; 121 | cout << "\033[1;32m [WO Vegi.] P: " << precision_wo_veg << " | R: " << recall_wo_veg << "\033[0m" << endl; 122 | 123 | // - - - - - - - - - - - - - - - - - - - - 124 | // If you want to save precision/recall in a text file, revise this part 125 | // - - - - - - - - - - - - - - - - - - - - 126 | static string w_veg_path = output_csvpath + "w_veg_" + seq + ".csv"; 127 | static string wo_veg_path = output_csvpath + "wo_veg_" + seq + ".csv"; 128 | if (save_csv_file) { 129 | // Save w/ veg 130 | ofstream output(w_veg_path, ios::app); 131 | output << n << "," << time_taken << "," << precision << "," << recall << "," << TPFNs[0] << "," << TPFNs[1] << "," << TPFNs[2] 132 | << "," << TPFNs[3]; 133 | output << std::endl; 134 | output.close(); 135 | // Save w/o veg 136 | output.open(wo_veg_path, ios::app); 137 | output << n << "," << time_taken << "," << precision_wo_veg << "," << recall_wo_veg << "," << TPFNs_wo_veg[0] << "," 138 | << TPFNs_wo_veg[1] << "," << TPFNs_wo_veg[2] << "," << TPFNs_wo_veg[3]; 139 | output << std::endl; 140 | output.close(); 141 | } 142 | 143 | 144 | // Publish msg 145 | pcl::PointCloud TP; 146 | pcl::PointCloud FP; 147 | pcl::PointCloud FN; 148 | pcl::PointCloud TN; 149 | discern_ground(pc_ground, TP, FP); 150 | discern_ground(pc_non_ground, FN, TN); 151 | 152 | // if (save_flag) { 153 | // std::map pc_curr_gt_counts, g_est_gt_counts; 154 | // double accuracy; 155 | // save_all_accuracy(pc_curr, pc_ground, acc_filename, accuracy, pc_curr_gt_counts, g_est_gt_counts); 156 | // 157 | // std::string count_str = std::to_string(msg->header.seq); 158 | // std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 159 | // std::string pcd_filename = pcd_savepath + "/" + count_str_padded + ".pcd"; 160 | // pc2pcdfile(TP, FP, FN, TN, pcd_filename); 161 | // } 162 | // Write data 163 | CloudPublisher.publish(cvt::cloud2msg(pc_curr)); 164 | TPPublisher.publish(cvt::cloud2msg(TP)); 165 | FPPublisher.publish(cvt::cloud2msg(FP)); 166 | FNPublisher.publish(cvt::cloud2msg(FN)); 167 | pub_score("p", precision); 168 | pub_score("r", recall); 169 | 170 | } 171 | 172 | int main(int argc, char **argv) { 173 | ros::init(argc, argv, "Benchmark"); 174 | ros::NodeHandle nh; 175 | nh.param("/algorithm", algorithm, "patchwork"); 176 | nh.param("/save_flag", save_flag, false); 177 | nh.param("/seq", seq, "00"); 178 | nh.param("/save_csv_file", save_csv_file, false); 179 | nh.param("/output_csvpath", output_csvpath, "/data/"); 180 | 181 | 182 | std::cout << "\033[1;32mHey??" << std::endl; 183 | std::cout << "Hey??" << algorithm << "\033[0m" << std::endl; 184 | 185 | gpf.reset(new GroundPlaneFit(&nh)); 186 | r_gpf.reset(new RegionwiseGPF(&nh)); 187 | ransac_gpf.reset(new RansacGPF(&nh)); 188 | sp.reset(new PatchWork(&nh)); 189 | cascaded_gseg.reset(new CascadedGroundSeg(&nh)); 190 | gpregression.reset(new pcl::GaussianFloorSegmentation(&nh)); 191 | 192 | string HOME = std::getenv("HOME"); 193 | output_csvpath = HOME + output_csvpath + algorithm + "_"; 194 | 195 | // For initialization 196 | // ofstream sc_output(output_filename); 197 | // sc_output.close(); 198 | // if (save_flag) { 199 | // acc_filename = ABS_DIR + "/" + algorithm + "_" + std::to_string(use_z_thr) + "_acc_" + seq + ".csv"; 200 | // pcd_savepath = ABS_DIR + "/pcds/" + algorithm + "/" + seq; 201 | // ofstream sc_output2(acc_filename); 202 | // sc_output2.close(); 203 | // } 204 | 205 | OutputPublisher = nh.advertise("/node_out", 100); 206 | CloudPublisher = nh.advertise("/benchmark/cloud", 100); 207 | TPPublisher = nh.advertise("/benchmark/TP", 100); 208 | FPPublisher = nh.advertise("/benchmark/FP", 100); 209 | FNPublisher = nh.advertise("/benchmark/FN", 100); 210 | 211 | PrecisionPublisher = nh.advertise("/precision", 1); 212 | RecallPublisher = nh.advertise("/recall", 1); 213 | 214 | ros::Subscriber NodeSubscriber = nh.subscribe("/node", 5000, callbackNode); 215 | 216 | ros::spin(); 217 | 218 | return 0; 219 | } 220 | -------------------------------------------------------------------------------- /src/utils/count_num.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | if __name__ == "__main__": 5 | 6 | 7 | algorithm = ["cascaded_gseg","gpf","gpregression","patchwork","r_gpf","ransac","linefit"] 8 | src_dir = "/media/jeewon/Elements/semantic_kitti_raw/" 9 | 10 | for alg in algorithm: 11 | print("start %s"%alg) 12 | tgt_dir = "/media/jeewon/Elements/data/"+alg+"_ground_labels/" 13 | wrong=0 14 | for seq_id in range(11): 15 | wrong=0 16 | sequence = "%02d"%seq_id 17 | print("start %s"%sequence) 18 | src_seq_dir = src_dir + sequence + "/labels/" 19 | bin_dir = src_dir+sequence+"/velodyne/" 20 | tgt_seq_dir = tgt_dir + sequence+"/" 21 | 22 | src_files = os.listdir(src_seq_dir) 23 | bin_files = os.listdir(bin_dir) 24 | tgt_files = os.listdir(tgt_seq_dir) 25 | 26 | num_src = len(src_files) 27 | num_bin = len(bin_files) 28 | num_tgt = len(tgt_files) 29 | if (num_src != num_tgt or num_bin!=num_tgt): 30 | wrong=1 31 | print("Something's wrong! Check the Seq. %s"%sequence) 32 | break 33 | else: 34 | for label_id in range(num_tgt): 35 | label="%06d"%label_id 36 | src_file_name=label+".label" 37 | tgt_file_name=label+".csv" 38 | bin_file_name=label+".bin" 39 | if ((src_file_name not in src_files) or (tgt_file_name not in tgt_files) or (bin_file_name not in bin_files) ): 40 | print("Missing label or bin or csv file of label %s"%label) 41 | wrong=1 42 | break 43 | bin_file = np.fromfile(bin_dir+bin_file_name) 44 | tgt_file = np.fromfile(tgt_seq_dir+tgt_file_name) 45 | if (len(bin_file)/8 != len(tgt_file)): 46 | print("Total counts of veloyne and label csv are different!") 47 | wrong=1 48 | break 49 | if (wrong==1): 50 | print("Error in Seq. %s"%sequence) 51 | else: 52 | print("Pass! Seq. %s"%sequence) 53 | if (wrong==1): 54 | print("Error in %s"%alg) 55 | if (wrong==1): 56 | print("Error exists") 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/utils/viz_all_frames.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | import time 4 | import os.path 5 | 6 | ######################## Set parameters ######################### 7 | alg = "cascaded_gseg" 8 | seq = "04" 9 | kittiraw_dir = "/home/user/data/SemanticKITTI/" # absolute path of KITTI dataset folder 10 | label_csv_dir = "/home/user/data/" # absolute path of directory to save csv files 11 | ################################################################# 12 | 13 | kittiraw_dir = kittiraw_dir+seq+"/velodyne/" 14 | velo_list = os.listdir(kittiraw_dir) 15 | 16 | frame_num_list = [] 17 | for file in velo_list: 18 | if file.count(".") == 1: 19 | name = file.split('.')[0] 20 | frame_num_list.append(name) 21 | else: 22 | for k in range(len(file)-1,0,-1): 23 | if file[k]=='.': 24 | frame_num_list.append(file[:k]) 25 | break 26 | frame_num_list = sorted(frame_num_list) 27 | 28 | viz = o3d.visualization.Visualizer() 29 | pc = o3d.geometry.PointCloud() 30 | viz.create_window() 31 | viz.add_geometry(pc) 32 | 33 | for frame_num in frame_num_list: 34 | velo_path=kittiraw_dir+frame_num+".bin" 35 | label_path = label_csv_dir+alg+"_ground_labels/"+seq+"/"+frame_num+".csv" 36 | 37 | pcd = ((np.fromfile(velo_path, dtype=np.float32)).reshape(-1, 4))[:, 0:3] 38 | points = np.asarray(pcd) 39 | labels = np.loadtxt(label_path,dtype=np.int) 40 | 41 | colors = [] 42 | for i in range(len(labels)): 43 | if labels[i] == 1: 44 | colors.append([0,1,0]) 45 | else: 46 | colors.append([0,0,0]) 47 | 48 | pc.points=o3d.utility.Vector3dVector(points) 49 | pc.colors=o3d.utility.Vector3dVector(colors) 50 | viz.add_geometry(pc) 51 | viz.update_geometry(pc) 52 | viz.poll_events() 53 | viz.update_renderer() 54 | time.sleep(0.1) 55 | 56 | viz.destroy_window() -------------------------------------------------------------------------------- /src/utils/viz_each_class.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "../lib/cvt.h" 15 | #include "common.hpp" 16 | ros::Publisher UnlabeledPub; 17 | ros::Publisher OutlierPub; 18 | ros::Publisher RoadPub; 19 | ros::Publisher ParkingPub; 20 | ros::Publisher SideWalkPub; 21 | ros::Publisher OGPub; 22 | ros::Publisher LanePub; 23 | ros::Publisher TerrainPub; 24 | ros::Publisher BuildingPub; 25 | ros::Publisher FensePub; 26 | ros::Publisher VegetationPub; 27 | ros::Publisher TrafficSignPub; 28 | 29 | void callbackNode(const gseg_benchmark::node::ConstPtr& msg) 30 | { 31 | static int count = 0; 32 | pcl::PointCloud pc_curr = cvt::cloudmsg2cloud(msg->lidar); 33 | pcl::PointCloud pc_road, pc_parking, pc_sidewalk; 34 | pcl::PointCloud pc_og, pc_lane, pc_terrain; 35 | pcl::PointCloud pc_unlabeled, pc_outlier; 36 | pcl::PointCloud pc_building, pc_fense, pc_vegetation, pc_traffic; 37 | 38 | for (const auto &pt: pc_curr.points){ 39 | // if ( ( pt.x > -5.) && (pt.x < 5.) && (pt.y < -5) ) std::cout<header.seq<("/unlabeled", 10); 91 | OutlierPub = nodeHandler.advertise("/outlier", 10); 92 | RoadPub = nodeHandler.advertise("/road", 10); 93 | ParkingPub = nodeHandler.advertise("/parking", 10); 94 | SideWalkPub = nodeHandler.advertise("/sidewalk", 10); 95 | OGPub = nodeHandler.advertise("/other_ground", 10); 96 | LanePub = nodeHandler.advertise("/lane", 10); 97 | TerrainPub = nodeHandler.advertise("/terrain", 10); 98 | 99 | BuildingPub = nodeHandler.advertise("/building", 10); 100 | FensePub = nodeHandler.advertise("/fense", 10); 101 | VegetationPub = nodeHandler.advertise("/vegetation", 10); 102 | 103 | TrafficSignPub = nodeHandler.advertise("/trafficSign", 10); 104 | 105 | ros::Subscriber NodeSubscriber = nodeHandler.subscribe("/node",1000, callbackNode); 106 | 107 | ros::spin(); 108 | 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /src/utils/viz_estimates.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "common.hpp" 17 | #include "../lib/cvt.h" 18 | #include "../lib/datahandle.h" 19 | 20 | void parse_pcd(const pcl::PointCloud& src, 21 | pcl::PointCloud& TP, pcl::PointCloud& FP, 22 | pcl::PointCloud& FN, pcl::PointCloud& TN){ 23 | 24 | if (!TP.points.empty()) TP.points.clear(); 25 | if (!FP.points.empty()) FP.points.clear(); 26 | if (!FN.points.empty()) FN.points.clear(); 27 | if (!TN.points.empty()) TN.points.clear(); 28 | 29 | for (const auto &pt: src.points){ 30 | int state = pt.intensity; 31 | if (state == TRUEPOSITIVE){ 32 | TP.points.push_back(pt); 33 | }else if (state == TRUENEGATIVE){ 34 | TN.points.push_back(pt); 35 | }else if (state == FALSENEGATIVE){ 36 | FN.points.push_back(pt); 37 | }else if (state == FALSEPOSITIVE){ 38 | FP.points.push_back(pt); 39 | } 40 | } 41 | } 42 | 43 | int main(int argc, char **argv) 44 | { 45 | ros::init(argc, argv, "mapviz"); 46 | std::cout<<"KiTTI MAPVIZ STARTED"<("/abs_dir", abs_dir, "/media/shapelim/UX960NVMe1/gpf_output/pcds"); 53 | nodeHandler.param("/seq", target_seq, "10"); 54 | nodeHandler.param("/frame", target_frame, 1); 55 | // 0. Declare publishers 56 | ros::Publisher casTP_pub = nodeHandler.advertise("/cascade/TP", 100); 57 | ros::Publisher casFP_pub = nodeHandler.advertise("/cascade/FP", 100); 58 | ros::Publisher casFN_pub = nodeHandler.advertise("/cascade/FN", 100); 59 | ros::Publisher casTN_pub = nodeHandler.advertise("/cascade/TN", 100); 60 | 61 | 62 | // 1. Set each filename 63 | std::string count_str = std::to_string(target_frame); 64 | std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 65 | std::string cascade_name = abs_dir + "/cascaded_gseg/" + target_seq + "/" + count_str_padded + ".pcd"; 66 | 67 | 68 | //////////////////////////////////////////////////////////////////// 69 | std::cout<<"\033[1;32m Loading map..."<::Ptr ptr_cas(new pcl::PointCloud); 73 | datahandle3d::load_pcd(cascade_name, ptr_cas); 74 | 75 | // // load original src 76 | // pcl::PointCloud::Ptr ptr_src(new pcl::PointCloud); 77 | // datahandle3d::load_pcd(srcName, ptr_src); 78 | 79 | // pcl::PointCloud::Ptr ptr_removert(new pcl::PointCloud); 80 | // datahandle3d::load_pcd(removertName, ptr_removert); 81 | //////////////////////////////////////////////////////////////////// 82 | // pcl::PointCloud oursStatic, oursDynamic; 83 | // pcl::PointCloud removertStatic, removertDynamic; 84 | // pcl::PointCloud mapStatic, mapDynamic; 85 | 86 | pcl::PointCloud casTP, casFP, casFN, casTN; 87 | 88 | parse_pcd(*ptr_cas, casTP, casFP, casFN, casTN); 89 | 90 | auto cas_tp_msg = cvt::cloud2msg(casTP); 91 | auto cas_fp_msg = cvt::cloud2msg(casFP); 92 | auto cas_fn_msg = cvt::cloud2msg(casFN); 93 | auto cas_tn_msg = cvt::cloud2msg(casTN); 94 | 95 | ros::Rate loop_rate(2); 96 | static int count_ = 0; 97 | while (ros::ok()) 98 | { 99 | casTP_pub.publish(cas_tp_msg); 100 | casFP_pub.publish(cas_fp_msg); 101 | casFN_pub.publish(cas_fn_msg); 102 | casTN_pub.publish(cas_tn_msg); 103 | if (++count_ %2 == 0) std::cout<<"On "<