├── .gitignore ├── CMakeLists.txt ├── README.md ├── demo.pcd ├── docker └── run.sh ├── setup.py ├── tgs.hpp ├── travel_api.py ├── travel_demo.cpp ├── travel_demo.py └── travel_pybind.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | *.so 3 | *.egg-info 4 | *.pcd 5 | __pycache__ 6 | *.ipynb 7 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(travel) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") 7 | 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | find_package(PCL REQUIRED) 12 | 13 | include_directories(${PCL_INCLUDE_DIRS}) 14 | link_directories(${PCL_LIBRARY_DIRS}) 15 | add_definitions(${PCL_DEFINITIONS}) 16 | 17 | add_executable (travel travel_demo.cpp) 18 | target_link_libraries (travel ${PCL_LIBRARIES}) 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TRAVEL: Pointcloud Ground Segmentation 2 | Original algorithm: https://github.com/url-kaist/TRAVEL 3 | 4 | I wanted to use this ground segmentation in a python repository and so I converted the C++ functions to python apis. I use Open3D to interface with the function (see travel_demo.py for usage). 5 | 6 | Note that this only does the ground segmentation part of TRAVEL, not the object clustering. 7 | 8 | Additional changes: Modified the estimateGround function to return ground_inds, rather than the separated ground and obj pcd to allow for indexing the original input pointcloud and retaining extra channel information. 9 | 10 | # Pre-Requisites 11 | Apt libraries: 12 | ``` 13 | apt update && apt install libeigen3-dev libflann-dev libglu1-mesa-dev freeglut3-dev mesa-common-dev libboost1.65-dev 14 | ``` 15 | Download BOOST lib (https://www.boost.org/doc/libs/1_65_1/more/getting_started/unix-variants.html) 16 | ``` 17 | wget https://boostorg.jfrog.io/artifactory/main/release/1.65.1/source/boost_1_65_1.tar.gz 18 | tar -xvf boost_1_65_1.tar.gz 19 | cd boost_1_65_1 && ./bootstrap.sh # Downloads boost to /usr/local 20 | ./b2 install 21 | ``` 22 | Download pcl-1.8 library (https://pcl.readthedocs.io/projects/tutorials/en/latest/compiling_pcl_posix.html) 23 | ``` 24 | wget https://github.com/PointCloudLibrary/pcl/archive/refs/tags/pcl-1.8.0.tar.gz 25 | tar -xvf pcl-1.8.0.tar.gz 26 | cd pcl-1.8.0 && mkdir build && cd build 27 | cmake -DCMAKE_BUILD_TYPE=Release .. 28 | make -j2 29 | make -j2 install 30 | ``` 31 | Change the path of pcl-1.8 based on where it installs to. Default is: "/usr/local/include/pcl-1.8/" 32 | 33 | To include in any project, structure the files like this: 34 | ``` 35 | my_project 36 | - travel 37 | - travel_api.py 38 | - src 39 | - tgs.hpp 40 | - travel_pybind.cpp 41 | 42 | ``` 43 | Then in the setup.py file, 44 | ``` 45 | ext_modules=[ 46 | Pybind11Extension( 47 | name='travel.ground_seg', 48 | sources=['travel/src/travel_pybind.cpp'], 49 | include_dirs=[ 50 | os.environ.get("EIGEN_INCLUDE_DIR", "/usr/include/eigen3/"), 51 | "/usr/local/include/pcl-1.8/"], 52 | extra_compile_args=['-std=c++14'], 53 | ), 54 | ...OtherExtensions...] 55 | ``` 56 | 57 | ### Troubleshooting 58 | If the above doesn't work, a dirty workaround is to keep this repo's setup.py in the travel folder and build it from there. 59 | ``` 60 | my_project 61 | - travel 62 | - travel_api.py 63 | - setup.py 64 | - src 65 | - tgs.hpp 66 | - travel_pybind.cpp 67 | ``` 68 | Building with either `python setup.py develop` or `pip install -e . --user` should work. 69 | 70 | # How to run 71 | Run docker. Note that python in my provided docker image needs to run with python3.7 i.e. `python3.7 travel_demo.py` 72 | ``` 73 | bash docker/run.sh 74 | ``` 75 | 76 | ### Running TRAVEL in python 77 | ``` 78 | python setup.py develop 79 | python travel_demo.py 80 | ``` 81 | 82 | 83 | ### Running TRAVEL in cpp (without ROS) 84 | Build files 85 | ``` 86 | mkdir build && cd build 87 | cmake .. 88 | make 89 | ``` 90 | Run the program with 91 | ``` 92 | ./travel 93 | ``` 94 | -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Modify these paths and GPU ids 4 | DATA_PATH="/media/darren/Samsung_T5/data" 5 | CODE_PATH="/media/darren/Samsung_T5/code/TRAVEL" 6 | GPU_ID="0" 7 | 8 | ENVS=" --env=NVIDIA_VISIBLE_DEVICES=$GPU_ID 9 | --env=CUDA_VISIBLE_DEVICES=$GPU_ID 10 | --env=NVIDIA_DRIVER_CAPABILITIES=all" 11 | 12 | # Modify these paths to mount the data 13 | VOLUMES=" --volume=$DATA_PATH:/TRAVEL/data 14 | --volume=$CODE_PATH:/TRAVEL" 15 | 16 | # Setup environmetns for pop-up visualization of point cloud 17 | VISUAL=" --env=DISPLAY 18 | --env=QT_X11_NO_MITSHM=1 19 | --volume=/tmp/.X11-unix:/tmp/.X11-unix" 20 | xhost +local:docker 21 | 22 | echo "Running the docker image [GPUS: ${GPU_ID}]" 23 | docker_image="darrenjkt/travel:v1.0" 24 | 25 | # Start docker image 26 | docker run -d -it --rm \ 27 | $VOLUMES \ 28 | $ENVS \ 29 | $VISUAL \ 30 | --runtime=nvidia \ 31 | --gpus $GPU_ID \ 32 | --privileged \ 33 | --net=host \ 34 | --shm-size=16G \ 35 | --workdir=/TRAVEL \ 36 | $docker_image 37 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | from pybind11.setup_helpers import Pybind11Extension, build_ext 3 | import os 4 | 5 | setup(name='travel', 6 | version='1.0', 7 | description='Ground estimation with TRAVEL', 8 | author='Darren Tsai', 9 | author_email='d.tsai@acfr.usyd.edu.au', 10 | license='MIT', 11 | cmdclass={'build_ext': build_ext}, 12 | ext_modules=[ 13 | Pybind11Extension( 14 | name='travel', 15 | sources=['travel_pybind.cpp',], 16 | include_dirs=[ 17 | os.environ.get("EIGEN_INCLUDE_DIR", "/usr/include/eigen3/"), 18 | "/usr/include/pcl-1.8/"], 19 | extra_compile_args=['-std=c++14'], 20 | )], 21 | 22 | python_requires=">=3.7", 23 | ) -------------------------------------------------------------------------------- /tgs.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TRAVEL_GSEG_H 2 | #define TRAVEL_GSEG_H 3 | // 4 | // Created by Minho Oh & Euigon Jung on 1/31/22. 5 | // We really appreciate Hyungtae Lim and Prof. Hyun Myung! :) 6 | // 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | // #include 21 | 22 | namespace travel { 23 | 24 | #define PTCLOUD_SIZE 132000 25 | #define NODEWISE_PTCLOUDSIZE 5000 26 | 27 | #define UNKNOWN 1 28 | #define NONGROUND 2 29 | #define GROUND 3 30 | 31 | using Eigen::MatrixXf; 32 | using Eigen::JacobiSVD; 33 | using Eigen::VectorXf; 34 | 35 | template 36 | bool point_z_cmp(PointType a, PointType b){ 37 | return a.z Pair; 46 | bool is_traversable; 47 | }; 48 | 49 | template 50 | struct TriGridNode { 51 | int node_type; 52 | pcl::PointCloud ptCloud; 53 | 54 | bool is_curr_data; 55 | 56 | // planar model 57 | Eigen::Vector3f normal; 58 | Eigen::Vector3f mean_pt; 59 | double d; 60 | 61 | Eigen::Vector3f singular_values; 62 | Eigen::Matrix3f eigen_vectors; 63 | double weight; 64 | 65 | double th_dist_d; 66 | double th_outlier_d; 67 | 68 | // graph_searching 69 | bool need_recheck; 70 | bool is_visited; 71 | bool is_rejection; 72 | int check_life; 73 | int depth; 74 | }; 75 | 76 | struct TriGridCorner { 77 | double x, y; 78 | std::vector zs; 79 | std::vector weights; 80 | }; 81 | 82 | template 83 | using GridNode = std::vector>; 84 | 85 | template 86 | using TriGridField = std::vector>>; 87 | 88 | template 89 | class TravelGroundSeg{ 90 | private: 91 | 92 | bool REFINE_MODE_; 93 | bool VIZ_MDOE_; 94 | 95 | double MAX_RANGE_; 96 | double MIN_RANGE_; 97 | double TGF_RESOLUTION_; 98 | 99 | int NUM_ITER_; 100 | int NUM_LRP_; 101 | int NUM_MIN_POINTS_; 102 | 103 | double TH_SEEDS_; 104 | double TH_DIST_; 105 | double TH_OUTLIER_; 106 | 107 | double TH_NORMAL_; 108 | double TH_WEIGHT_; 109 | double TH_LCC_NORMAL_SIMILARITY_; 110 | double TH_LCC_PLANAR_MODEL_DIST_; 111 | double TH_OBSTACLE_HEIGHT_; 112 | 113 | TriGridField trigrid_field_; 114 | std::vector trigrid_edges_; 115 | std::vector> trigrid_corners_; 116 | std::vector> trigrid_centers_; 117 | 118 | pcl::PointCloud empty_cloud_; 119 | TriGridNode empty_trigrid_node_; 120 | GridNode empty_grid_nodes_; 121 | TriGridCorner empty_trigrid_corner_; 122 | TriGridCorner empty_trigrid_center_; 123 | 124 | pcl::PointCloud ptCloud_tgfwise_ground_; 125 | pcl::PointCloud ptCloud_tgfwise_nonground_; 126 | pcl::PointCloud ptCloud_tgfwise_outliers_; 127 | pcl::PointCloud ptCloud_tgfwise_obstacle_; 128 | pcl::PointCloud ptCloud_nodewise_ground_; 129 | pcl::PointCloud ptCloud_nodewise_nonground_; 130 | pcl::PointCloud ptCloud_nodewise_outliers_; 131 | pcl::PointCloud ptCloud_nodewise_obstacle_; 132 | 133 | pcl::PointCloud ptCloud_nodewise_nonground_tmp; 134 | pcl::PointCloud ptCloud_nodewise_ground_tmp; 135 | pcl::PointCloud ptCloud_nodewise_obstacle_tmp; 136 | 137 | public: 138 | TravelGroundSeg(){ 139 | 140 | }; 141 | 142 | void setParams(const double max_range, const double min_range, const double resolution, 143 | const int num_iter, const int num_lpr, const int num_min_pts, const double th_seeds, 144 | const double th_dist, const double th_outlier, const double th_normal, const double th_weight, 145 | const double th_lcc_normal_similiarity, const double th_lcc_planar_model_dist, const double th_obstacle, 146 | const bool refine_mode, const bool visualization) { 147 | 148 | MAX_RANGE_ = max_range; 149 | 150 | MIN_RANGE_ = min_range; 151 | 152 | TGF_RESOLUTION_ = resolution; 153 | 154 | NUM_ITER_ = num_iter; 155 | 156 | NUM_LRP_ = num_lpr; 157 | 158 | NUM_MIN_POINTS_ = num_min_pts; 159 | 160 | TH_SEEDS_ = th_seeds; 161 | 162 | TH_DIST_ = th_dist; 163 | 164 | TH_OUTLIER_ = th_outlier; 165 | 166 | TH_NORMAL_ = th_normal; 167 | 168 | TH_WEIGHT_ = th_weight; 169 | 170 | TH_LCC_NORMAL_SIMILARITY_ = th_lcc_normal_similiarity; 171 | 172 | TH_LCC_PLANAR_MODEL_DIST_ = th_lcc_planar_model_dist; 173 | 174 | TH_OBSTACLE_HEIGHT_ = th_obstacle; 175 | 176 | REFINE_MODE_ = refine_mode; 177 | VIZ_MDOE_ = visualization; 178 | 179 | initTriGridField(trigrid_field_); 180 | initTriGridCorners(trigrid_corners_, trigrid_centers_); 181 | 182 | ptCloud_tgfwise_ground_.clear(); 183 | ptCloud_tgfwise_ground_.reserve(PTCLOUD_SIZE); 184 | ptCloud_tgfwise_nonground_.clear(); 185 | ptCloud_tgfwise_nonground_.reserve(PTCLOUD_SIZE); 186 | ptCloud_tgfwise_outliers_.clear(); 187 | ptCloud_tgfwise_outliers_.reserve(PTCLOUD_SIZE); 188 | ptCloud_tgfwise_obstacle_.clear(); 189 | ptCloud_tgfwise_obstacle_.reserve(PTCLOUD_SIZE); 190 | 191 | ptCloud_nodewise_ground_.clear(); 192 | ptCloud_nodewise_ground_.reserve(NODEWISE_PTCLOUDSIZE); 193 | ptCloud_nodewise_nonground_.clear(); 194 | ptCloud_nodewise_nonground_.reserve(NODEWISE_PTCLOUDSIZE); 195 | ptCloud_nodewise_outliers_.clear(); 196 | ptCloud_nodewise_outliers_.reserve(NODEWISE_PTCLOUDSIZE); 197 | ptCloud_nodewise_obstacle_.clear(); 198 | ptCloud_nodewise_obstacle_.reserve(NODEWISE_PTCLOUDSIZE); 199 | 200 | ptCloud_nodewise_nonground_tmp.reserve(NODEWISE_PTCLOUDSIZE); 201 | ptCloud_nodewise_ground_tmp.reserve(NODEWISE_PTCLOUDSIZE); 202 | ptCloud_nodewise_obstacle_tmp.reserve(NODEWISE_PTCLOUDSIZE); 203 | }; 204 | 205 | // Converted inputs to Eigen::Vector3d so that we can feed Open3D vectors in 206 | void estimateGround(const std::vector& cloud_in_vec, 207 | std::vector& cloudGround_out_vec, 208 | std::vector& cloudNonground_out_vec, 209 | std::vector& ground_inds_vec){ 210 | 211 | // 0. Init 212 | pcl::PointCloud cloud_in; 213 | pcl::PointCloud cloudGround_out; 214 | pcl::PointCloud cloudNonground_out; 215 | 216 | // Convert Eigen to PointCloud 217 | for (uint32_t i=0; i < cloud_in_vec.size(); i++) { 218 | PointType point; 219 | point.x = cloud_in_vec[i][0]; 220 | point.y = cloud_in_vec[i][1]; 221 | point.z = cloud_in_vec[i][2]; 222 | point.id = i; 223 | cloud_in.push_back(point); 224 | 225 | } 226 | ptCloud_tgfwise_outliers_.clear(); 227 | ptCloud_tgfwise_outliers_.reserve(cloud_in.size()); 228 | 229 | // 1. Embed PointCloud to TriGridField 230 | clearTriGridField(trigrid_field_); 231 | clearTriGridCorners(trigrid_corners_, trigrid_centers_); 232 | 233 | embedCloudToTriGridField(cloud_in, trigrid_field_); 234 | 235 | // 2. Node-wise Terrain Modeling 236 | modelNodeWiseTerrain(trigrid_field_); 237 | 238 | // 3. Breadth-first Traversable Graph Search 239 | BreadthFirstTraversableGraphSearch(trigrid_field_); 240 | setTGFCornersCenters(trigrid_field_, trigrid_corners_, trigrid_centers_); 241 | 242 | // 4. TGF-wise Traversable Terrain Model Fitting 243 | if (REFINE_MODE_){ 244 | fitTGFWiseTraversableTerrainModel(trigrid_field_, trigrid_corners_, trigrid_centers_); 245 | } 246 | 247 | // 5. Ground Segmentation 248 | segmentTGFGround(trigrid_field_, ptCloud_tgfwise_ground_, ptCloud_tgfwise_nonground_, ptCloud_tgfwise_obstacle_, ptCloud_tgfwise_outliers_); 249 | cloudGround_out = ptCloud_tgfwise_ground_; 250 | cloudNonground_out = ptCloud_tgfwise_nonground_; 251 | 252 | // Convert PointCloud back to Eigen::Vector3d 253 | for (auto &point : cloudGround_out.points){ 254 | Eigen::Vector3d point_eigen (static_cast(point.x), static_cast(point.y), static_cast(point.z)); 255 | cloudGround_out_vec.push_back(point_eigen); 256 | 257 | Eigen::Vector3d inds (static_cast(point.id), 0.0, 0.0); 258 | ground_inds_vec.push_back(inds); 259 | } 260 | for (auto &point : cloudNonground_out.points){ 261 | Eigen::Vector3d point_eigen (static_cast(point.x), static_cast(point.y), static_cast(point.z)); 262 | cloudNonground_out_vec.push_back(point_eigen); 263 | } 264 | 265 | return; 266 | }; 267 | 268 | TriGridIdx getTriGridIdx(const float& x_in, const float& y_in){ 269 | TriGridIdx tgf_idx; 270 | int r_i = (x_in - tgf_min_x)/TGF_RESOLUTION_; 271 | int c_i = (y_in - tgf_min_y)/TGF_RESOLUTION_; 272 | int t_i = 0; 273 | double angle = atan2(y_in-(c_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_y), x_in-(r_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_x)); 274 | 275 | if (angle>=(M_PI/4) && angle <(3*M_PI/4)){ 276 | t_i = 1; 277 | } else if (angle>=(-M_PI/4) && angle <(M_PI/4)){ 278 | t_i = 0; 279 | } else if (angle>=(-3*M_PI/4) && angle <(-M_PI/4)){ 280 | t_i = 3; 281 | } else{ 282 | t_i = 2; 283 | } 284 | tgf_idx.row = r_i; 285 | tgf_idx.col = c_i; 286 | tgf_idx.tri = t_i; 287 | return tgf_idx; 288 | } 289 | 290 | TriGridNode getTriGridNode(const float& x_in, const float& y_in){ 291 | TriGridNode node; 292 | TriGridIdx node_idx = getTriGridIdx(x_in, y_in); 293 | node = trigrid_field_[node_idx.row][node_idx.col][node_idx.tri]; 294 | return node; 295 | }; 296 | 297 | TriGridNode getTriGridNode(const TriGridIdx& tgf_idx){ 298 | TriGridNode node; 299 | node = trigrid_field_[tgf_idx.row][tgf_idx.col][tgf_idx.tri]; 300 | return node; 301 | }; 302 | 303 | bool is_traversable(const float& x_in, const float& y_in){ 304 | TriGridNode node = getTriGridNode(x_in, y_in); 305 | if (node.node_type == GROUND){ 306 | return true; 307 | } else{ 308 | return false; 309 | } 310 | }; 311 | 312 | pcl::PointCloud getObstaclePC(){ 313 | pcl::PointCloud cloud_obstacle; 314 | cloud_obstacle = ptCloud_tgfwise_obstacle_; 315 | return cloud_obstacle; 316 | }; 317 | 318 | private: 319 | double tgf_max_x, tgf_max_y, tgf_min_x, tgf_min_y; 320 | double rows_, cols_; 321 | 322 | void initTriGridField(TriGridField& tgf_in){ 323 | // print("Initializing TriGridField..."); 324 | 325 | tgf_max_x = MAX_RANGE_; 326 | tgf_max_y = MAX_RANGE_; 327 | 328 | tgf_min_x = -MAX_RANGE_; 329 | tgf_min_y = -MAX_RANGE_; 330 | 331 | rows_ = (int)(tgf_max_x - tgf_min_x) / TGF_RESOLUTION_; 332 | cols_ = (int)(tgf_max_y - tgf_min_y) / TGF_RESOLUTION_; 333 | empty_cloud_.clear(); 334 | empty_cloud_.reserve(PTCLOUD_SIZE); 335 | 336 | // Set Node structure 337 | empty_trigrid_node_.node_type = UNKNOWN; 338 | empty_trigrid_node_.ptCloud.clear(); 339 | empty_trigrid_node_.ptCloud.reserve(NODEWISE_PTCLOUDSIZE); 340 | 341 | empty_trigrid_node_.is_curr_data = false; 342 | empty_trigrid_node_.need_recheck = false; 343 | empty_trigrid_node_.is_visited = false; 344 | empty_trigrid_node_.is_rejection = false; 345 | 346 | empty_trigrid_node_.check_life = 10; 347 | empty_trigrid_node_.depth = -1; 348 | 349 | empty_trigrid_node_.normal; 350 | empty_trigrid_node_.mean_pt; 351 | empty_trigrid_node_.d = 0; 352 | 353 | empty_trigrid_node_.singular_values; 354 | empty_trigrid_node_.eigen_vectors; 355 | empty_trigrid_node_.weight = 0; 356 | 357 | empty_trigrid_node_.th_dist_d = 0; 358 | empty_trigrid_node_.th_outlier_d = 0; 359 | 360 | // Set TriGridField 361 | tgf_in.clear(); 362 | std::vector> vec_gridnode; 363 | 364 | for (int i = 0; i < 4 ; i ++) 365 | empty_grid_nodes_.emplace_back(empty_trigrid_node_); 366 | 367 | for (int i=0; i< cols_; i++){ vec_gridnode.emplace_back(empty_grid_nodes_);} 368 | for (int j=0; j< rows_; j++){ tgf_in.emplace_back(vec_gridnode);} 369 | 370 | return; 371 | }; 372 | 373 | void initTriGridCorners(std::vector>& trigrid_corners_in, 374 | std::vector>& trigrid_centers_in){ 375 | // print("Initializing TriGridCorners..."); 376 | 377 | // Set TriGridCorner 378 | empty_trigrid_corner_.x = empty_trigrid_corner_.y = 0.0; 379 | empty_trigrid_corner_.zs.clear(); 380 | empty_trigrid_corner_.zs.reserve(8); 381 | empty_trigrid_corner_.weights.clear(); 382 | empty_trigrid_corner_.weights.reserve(8); 383 | 384 | empty_trigrid_center_.x = empty_trigrid_center_.y = 0.0; 385 | empty_trigrid_center_.zs.clear(); 386 | empty_trigrid_center_.zs.reserve(4); 387 | empty_trigrid_center_.weights.clear(); 388 | empty_trigrid_center_.weights.reserve(4); 389 | 390 | trigrid_corners_in.clear(); 391 | trigrid_centers_in.clear(); 392 | 393 | // Set Corners and Centers 394 | std::vector col_corners; 395 | std::vector col_centers; 396 | for (int i=0; i< cols_; i++){ 397 | col_corners.emplace_back(empty_trigrid_corner_); 398 | col_centers.emplace_back(empty_trigrid_center_); 399 | } 400 | col_corners.emplace_back(empty_trigrid_corner_); 401 | 402 | for (int j=0; j< rows_; j++){ 403 | trigrid_corners_in.emplace_back(col_corners); 404 | trigrid_centers_in.emplace_back(col_centers); 405 | } 406 | trigrid_corners_in.emplace_back(col_corners); 407 | 408 | return; 409 | }; 410 | 411 | void clearTriGridField(TriGridField &tgf_in){ 412 | // print("Clearing TriGridField..."); 413 | 414 | for (int r_i = 0; r_i < rows_; r_i++){ 415 | for (int c_i = 0; c_i < cols_; c_i++){ 416 | tgf_in[r_i][c_i] = empty_grid_nodes_; 417 | } 418 | } 419 | return; 420 | }; 421 | 422 | void clearTriGridCorners(std::vector>& trigrid_corners_in, 423 | std::vector>& trigrid_centers_in){ 424 | // print("Clearing TriGridCorners..."); 425 | 426 | TriGridCorner tmp_corner = empty_trigrid_corner_; 427 | TriGridCorner tmp_center = empty_trigrid_center_; 428 | for (int r_i = 0; r_i < rows_+1; r_i++){ 429 | for (int c_i = 0; c_i < cols_+1; c_i++){ 430 | tmp_corner.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; tmp_corner.y = (c_i)*TGF_RESOLUTION_+tgf_min_y; 431 | tmp_corner.zs.clear(); 432 | tmp_corner.weights.clear(); 433 | trigrid_corners_in[r_i][c_i] = tmp_corner; 434 | if (r_i < rows_ && c_i < cols_) { 435 | tmp_center.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; tmp_center.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y; 436 | tmp_center.zs.clear(); 437 | tmp_center.weights.clear(); 438 | trigrid_centers_in[r_i][c_i] = tmp_center; 439 | } 440 | } 441 | } 442 | return; 443 | }; 444 | 445 | double xy_2Dradius(double x, double y){ 446 | return sqrt(x*x + y*y); 447 | }; 448 | 449 | bool filterPoint(const PointType &pt_in){ 450 | double xy_range = xy_2Dradius(pt_in.x, pt_in.y); 451 | if (xy_range >= MAX_RANGE_ || xy_range <= MIN_RANGE_) return true; 452 | 453 | return false; 454 | } 455 | 456 | void embedCloudToTriGridField(const pcl::PointCloud& cloud_in, TriGridField& tgf_out) { 457 | // print("Embedding PointCloud to TriGridField..."); 458 | 459 | for (auto const &pt: cloud_in.points){ 460 | if (filterPoint(pt)){ 461 | ptCloud_tgfwise_outliers_.points.push_back(pt); 462 | continue; 463 | } 464 | 465 | int r_i = (pt.x - tgf_min_x)/TGF_RESOLUTION_; 466 | int c_i = (pt.y - tgf_min_y)/TGF_RESOLUTION_; 467 | 468 | if (r_i < 0 || r_i >= rows_ || c_i < 0 || c_i >= cols_) { 469 | ptCloud_tgfwise_outliers_.points.push_back(pt); 470 | continue; 471 | } 472 | 473 | double angle = atan2(pt.y-(c_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_y), pt.x-(r_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_x)); 474 | if (angle>=(M_PI/4) && angle <(3*M_PI/4)){ 475 | // left side 476 | tgf_out[r_i][c_i][1].ptCloud.push_back(pt); 477 | if(!tgf_out[r_i][c_i][1].is_curr_data) {tgf_out[r_i][c_i][1].is_curr_data = true;} 478 | } else if (angle>=(-M_PI/4) && angle <(M_PI/4)){ 479 | // upper side 480 | tgf_out[r_i][c_i][0].ptCloud.push_back(pt); 481 | if (!tgf_out[r_i][c_i][0].is_curr_data){tgf_out[r_i][c_i][0].is_curr_data = true;} 482 | 483 | } else if (angle>=(-3*M_PI/4) && angle <(-M_PI/4)){ 484 | // right side 485 | tgf_out[r_i][c_i][3].ptCloud.push_back(pt); 486 | if (!tgf_out[r_i][c_i][3].is_curr_data) {tgf_out[r_i][c_i][3].is_curr_data = true;} 487 | } else{ 488 | // lower side 489 | tgf_out[r_i][c_i][2].ptCloud.push_back(pt); 490 | if (!tgf_out[r_i][c_i][2].is_curr_data) {tgf_out[r_i][c_i][2].is_curr_data = true;} 491 | } 492 | } 493 | 494 | return; 495 | }; 496 | 497 | void extractInitialSeeds(const pcl::PointCloud& p_sorted, pcl::PointCloud& init_seeds){ 498 | //function for uniform mode 499 | init_seeds.points.clear(); 500 | 501 | // LPR is the mean of Low Point Representative 502 | double sum = 0; 503 | int cnt = 0; 504 | 505 | // Calculate the mean height value. 506 | for (int i=0; i< (int) p_sorted.points.size() && cnt& ground_in, TriGridNode& node_out) { 524 | 525 | // function for uniform mode 526 | Eigen::Matrix3f cov_; 527 | Eigen::Vector4f pc_mean_; 528 | pcl::computeMeanAndCovarianceMatrix(ground_in, cov_, pc_mean_); 529 | 530 | // Singular Value Decomposition: SVD 531 | Eigen::JacobiSVD svd(cov_, Eigen::DecompositionOptions::ComputeFullU); 532 | 533 | // Use the least singular vector as normal 534 | node_out.eigen_vectors = svd.matrixU(); 535 | if (node_out.eigen_vectors.col(2)(2,0)<0) { 536 | node_out.eigen_vectors.col(0)*= -1; 537 | node_out.eigen_vectors.col(2)*= -1; 538 | } 539 | node_out.normal = node_out.eigen_vectors.col(2); 540 | node_out.singular_values = svd.singularValues(); 541 | 542 | // mean ground seeds value 543 | node_out.mean_pt = pc_mean_.head<3>(); 544 | 545 | // according to noraml.T*[x,y,z] = -d 546 | node_out.d = -(node_out.normal.transpose()*node_out.mean_pt)(0,0); 547 | 548 | // set distance theshold to 'th_dist - d' 549 | node_out.th_dist_d = TH_DIST_ - node_out.d; 550 | node_out.th_outlier_d = -node_out.d - TH_OUTLIER_; 551 | 552 | return; 553 | } 554 | 555 | void modelPCAbasedTerrain(TriGridNode& node_in) { 556 | // Initailization 557 | if (!ptCloud_nodewise_ground_.empty()) ptCloud_nodewise_ground_.clear(); 558 | 559 | // Tri Grid Initialization 560 | // When to initialize the planar model, we don't have prior. so outlier is removed in heuristic parameter. 561 | pcl::PointCloud sort_ptCloud = node_in.ptCloud; 562 | 563 | // sort in z-coordinate 564 | sort(sort_ptCloud.points.begin(), sort_ptCloud.end(), point_z_cmp); 565 | 566 | // Set init seeds 567 | extractInitialSeeds(sort_ptCloud, ptCloud_nodewise_ground_); 568 | 569 | Eigen::MatrixXf points(sort_ptCloud.points.size(),3); 570 | int j = 0; 571 | for (auto& p:sort_ptCloud.points){ 572 | points.row(j++)<& node_in){ 605 | double weight = 0; 606 | 607 | // weight = (node_in.singular_values[0]/node_in.singular_values[2] + node_in.singular_values[1]/node_in.singular_values[2])/(node_in.singular_values[0]/node_in.singular_values[1]); 608 | weight = (node_in.singular_values[0] + node_in.singular_values[1])*node_in.singular_values[1]/(node_in.singular_values[0]*node_in.singular_values[2]+0.001); 609 | 610 | return weight; 611 | } 612 | 613 | void modelNodeWiseTerrain(TriGridField& tgf_in) { 614 | // print("Node-wise Terrain Modeling..."); 615 | 616 | for (int r_i = 0; r_i < rows_; r_i++){ 617 | for (int c_i = 0; c_i < cols_; c_i++){ 618 | for (int s_i = 0; s_i < 4; s_i++){ 619 | if (tgf_in[r_i][c_i][s_i].is_curr_data){ 620 | if (tgf_in[r_i][c_i][s_i].ptCloud.size() < NUM_MIN_POINTS_){ 621 | tgf_in[r_i][c_i][s_i].node_type = UNKNOWN; 622 | continue; 623 | } else { 624 | modelPCAbasedTerrain(tgf_in[r_i][c_i][s_i]); 625 | if (tgf_in[r_i][c_i][s_i].node_type == GROUND){ tgf_in[r_i][c_i][s_i].weight = calcNodeWeight(tgf_in[r_i][c_i][s_i]); } 626 | } 627 | } 628 | } 629 | } 630 | } 631 | 632 | return; 633 | }; 634 | 635 | void findDominantNode(const TriGridField& tgf_in, TriGridIdx& node_idx_out) { 636 | // Find the dominant node 637 | // std::cout << "Find the dominant node..." << std::endl; 638 | TriGridIdx max_tri_idx; 639 | TriGridIdx ego_idx; 640 | ego_idx.row = (int)((0-tgf_min_x)/TGF_RESOLUTION_); 641 | ego_idx.col = (int)((0-tgf_min_y)/TGF_RESOLUTION_); 642 | ego_idx.tri = 0; 643 | 644 | max_tri_idx = ego_idx; 645 | for (int r_i = ego_idx.row - 2; r_i < ego_idx.row + 2; r_i++){ 646 | for (int c_i = ego_idx.col - 2; c_i < ego_idx.col + 2; c_i++){ 647 | for (int s_i = 0; s_i < 4; s_i++){ 648 | if (tgf_in[r_i][c_i][s_i].is_curr_data){ 649 | if (tgf_in[r_i][c_i][s_i].node_type == GROUND){ 650 | if (tgf_in[r_i][c_i][s_i].weight > tgf_in[max_tri_idx.row][max_tri_idx.row][max_tri_idx.tri].weight){ 651 | max_tri_idx.row = r_i; 652 | max_tri_idx.col = c_i; 653 | max_tri_idx.tri = s_i; 654 | } 655 | } 656 | } 657 | } 658 | } 659 | } 660 | node_idx_out = max_tri_idx; 661 | return; 662 | }; 663 | 664 | void searchNeighborNodes(const TriGridIdx &cur_idx, std::vector &neighbor_idxs) { 665 | neighbor_idxs.clear(); 666 | neighbor_idxs.reserve(14); 667 | int r_i = cur_idx.row; 668 | int c_i = cur_idx.col; 669 | int s_i = cur_idx.tri; 670 | 671 | std::vector tmp_neighbors; 672 | tmp_neighbors.clear(); 673 | tmp_neighbors.reserve(14); 674 | 675 | TriGridIdx neighbor_idx; 676 | for (int s_i = 0; s_i < 4 ; s_i++){ 677 | if (s_i == cur_idx.tri) continue; 678 | 679 | neighbor_idx = cur_idx; 680 | neighbor_idx.tri = s_i; 681 | tmp_neighbors.push_back(neighbor_idx); 682 | } 683 | 684 | switch (s_i) { 685 | case 0: 686 | tmp_neighbors.push_back({r_i+1, c_i+1, 2}); 687 | tmp_neighbors.push_back({r_i+1, c_i+1, 3}); 688 | tmp_neighbors.push_back({r_i+1, c_i , 1}); 689 | tmp_neighbors.push_back({r_i+1, c_i , 2}); 690 | tmp_neighbors.push_back({r_i+1, c_i , 3}); 691 | tmp_neighbors.push_back({r_i+1, c_i-1, 1}); 692 | tmp_neighbors.push_back({r_i+1, c_i-1, 2}); 693 | tmp_neighbors.push_back({r_i , c_i+1, 0}); 694 | tmp_neighbors.push_back({r_i , c_i+1, 3}); 695 | tmp_neighbors.push_back({r_i , c_i-1, 0}); 696 | tmp_neighbors.push_back({r_i , c_i-1, 1}); 697 | break; 698 | case 1: 699 | tmp_neighbors.push_back({r_i+1, c_i+1, 2}); 700 | tmp_neighbors.push_back({r_i+1, c_i+1, 3}); 701 | tmp_neighbors.push_back({r_i+1, c_i , 1}); 702 | tmp_neighbors.push_back({r_i+1, c_i , 2}); 703 | tmp_neighbors.push_back({r_i , c_i+1, 0}); 704 | tmp_neighbors.push_back({r_i , c_i+1, 2}); 705 | tmp_neighbors.push_back({r_i , c_i+1, 3}); 706 | tmp_neighbors.push_back({r_i-1, c_i+1, 0}); 707 | tmp_neighbors.push_back({r_i-1, c_i+1, 3}); 708 | tmp_neighbors.push_back({r_i-1, c_i+1, 0}); 709 | tmp_neighbors.push_back({r_i-1, c_i , 1}); 710 | break; 711 | case 2: 712 | tmp_neighbors.push_back({r_i , c_i+1, 2}); 713 | tmp_neighbors.push_back({r_i , c_i+1, 3}); 714 | tmp_neighbors.push_back({r_i , c_i-1, 1}); 715 | tmp_neighbors.push_back({r_i , c_i-1, 2}); 716 | tmp_neighbors.push_back({r_i-1, c_i+1, 0}); 717 | tmp_neighbors.push_back({r_i-1, c_i+1, 3}); 718 | tmp_neighbors.push_back({r_i-1, c_i , 0}); 719 | tmp_neighbors.push_back({r_i-1, c_i , 1}); 720 | tmp_neighbors.push_back({r_i-1, c_i , 3}); 721 | tmp_neighbors.push_back({r_i-1, c_i-1, 0}); 722 | tmp_neighbors.push_back({r_i-1, c_i-1, 1}); 723 | break; 724 | case 3: 725 | tmp_neighbors.push_back({r_i+1, c_i , 2}); 726 | tmp_neighbors.push_back({r_i+1, c_i , 3}); 727 | tmp_neighbors.push_back({r_i+1, c_i-1, 1}); 728 | tmp_neighbors.push_back({r_i+1, c_i-1, 2}); 729 | tmp_neighbors.push_back({r_i , c_i-1, 0}); 730 | tmp_neighbors.push_back({r_i , c_i-1, 1}); 731 | tmp_neighbors.push_back({r_i , c_i-1, 2}); 732 | tmp_neighbors.push_back({r_i-1, c_i , 0}); 733 | tmp_neighbors.push_back({r_i-1, c_i , 3}); 734 | tmp_neighbors.push_back({r_i-1, c_i-1, 0}); 735 | tmp_neighbors.push_back({r_i-1, c_i-1, 1}); 736 | break; 737 | default: 738 | break; 739 | } 740 | 741 | for (int n_i = 0 ; n_i < (int) tmp_neighbors.size() ; n_i++) { 742 | 743 | if (tmp_neighbors[n_i].row >= rows_ || tmp_neighbors[n_i].row < 0) { 744 | continue; 745 | } 746 | 747 | if (tmp_neighbors[n_i].col >= cols_ || tmp_neighbors[n_i].col < 0) { 748 | continue; 749 | } 750 | 751 | neighbor_idxs.push_back(tmp_neighbors[n_i]); 752 | } 753 | } 754 | 755 | void searchAdjacentNodes(const TriGridIdx &cur_idx, std::vector &adjacent_idxs) { 756 | adjacent_idxs.clear(); 757 | adjacent_idxs.reserve(3); 758 | int r_i = cur_idx.row; 759 | int c_i = cur_idx.col; 760 | int s_i = cur_idx.tri; 761 | 762 | std::vector tmp_neighbors; 763 | tmp_neighbors.clear(); 764 | tmp_neighbors.reserve(3); 765 | 766 | TriGridIdx neighbor_idx; 767 | 768 | switch (s_i) { 769 | case 0: 770 | tmp_neighbors.push_back({r_i+1, c_i, 2}); 771 | tmp_neighbors.push_back({r_i , c_i, 3}); 772 | tmp_neighbors.push_back({r_i , c_i, 1}); 773 | 774 | break; 775 | case 1: 776 | tmp_neighbors.push_back({r_i, c_i+1, 3}); 777 | tmp_neighbors.push_back({r_i, c_i , 0}); 778 | tmp_neighbors.push_back({r_i, c_i , 2}); 779 | break; 780 | case 2: 781 | tmp_neighbors.push_back({r_i-1, c_i, 0}); 782 | tmp_neighbors.push_back({r_i , c_i, 1}); 783 | tmp_neighbors.push_back({r_i , c_i, 3}); 784 | break; 785 | case 3: 786 | tmp_neighbors.push_back({r_i, c_i-1, 1}); 787 | tmp_neighbors.push_back({r_i, c_i , 2}); 788 | tmp_neighbors.push_back({r_i, c_i , 0}); 789 | break; 790 | default: 791 | break; 792 | } 793 | 794 | for (int n_i = 0 ; n_i < (int) tmp_neighbors.size() ; n_i++) { 795 | 796 | if (tmp_neighbors[n_i].row >= rows_ || tmp_neighbors[n_i].row < 0) { 797 | continue; 798 | } 799 | 800 | if (tmp_neighbors[n_i].col >= cols_ || tmp_neighbors[n_i].col < 0) { 801 | continue; 802 | } 803 | 804 | adjacent_idxs.push_back(tmp_neighbors[n_i]); 805 | } 806 | } 807 | 808 | bool LocalConvecityConcavity(const TriGridField &tgf, const TriGridIdx &cur_node_idx, const TriGridIdx &neighbor_idx, 809 | double & thr_local_normal, double & thr_local_dist) { 810 | TriGridNode current_node = tgf[cur_node_idx.row][cur_node_idx.col][cur_node_idx.tri]; 811 | TriGridNode neighbor_node = tgf[neighbor_idx.row][neighbor_idx.col][neighbor_idx.tri]; 812 | 813 | Eigen::Vector3f normal_src = current_node.normal; 814 | Eigen::Vector3f normal_tgt = neighbor_node.normal; 815 | Eigen::Vector3f meanPt_diff_s2t = neighbor_node.mean_pt - current_node.mean_pt; 816 | 817 | double diff_norm = meanPt_diff_s2t.norm(); 818 | double dist_s2t = normal_src.dot(meanPt_diff_s2t); 819 | double dist_t2s = normal_tgt.dot(-meanPt_diff_s2t); 820 | 821 | double normal_similarity = normal_src.dot(normal_tgt); 822 | double TH_NORMAL_cos_similarity = sin(diff_norm*thr_local_normal); 823 | if ((normal_similarity < (1-TH_NORMAL_cos_similarity))) { 824 | return false; 825 | } 826 | 827 | double TH_DIST_to_planar = diff_norm*sin(thr_local_dist); 828 | if ( (abs(dist_s2t) > TH_DIST_to_planar || abs(dist_t2s) > TH_DIST_to_planar) ) { 829 | return false; 830 | } 831 | 832 | return true; 833 | } 834 | 835 | void BreadthFirstTraversableGraphSearch(TriGridField& tgf_in) { 836 | 837 | // Find the dominant node 838 | std::queue searching_idx_queue; 839 | TriGridIdx dominant_node_idx; 840 | findDominantNode(tgf_in, dominant_node_idx); 841 | tgf_in[dominant_node_idx.row][dominant_node_idx.col][dominant_node_idx.tri].is_visited = true; 842 | tgf_in[dominant_node_idx.row][dominant_node_idx.col][dominant_node_idx.tri].depth = 0; 843 | tgf_in[dominant_node_idx.row][dominant_node_idx.col][dominant_node_idx.tri].node_type = GROUND; 844 | 845 | searching_idx_queue.push(dominant_node_idx); 846 | 847 | double max_planar_height = 0; 848 | trigrid_edges_.clear(); 849 | trigrid_edges_.reserve(rows_*cols_*4); 850 | TriGridEdge cur_edge; 851 | TriGridIdx current_node_idx; 852 | while (!searching_idx_queue.empty()){ 853 | // set current node 854 | current_node_idx = searching_idx_queue.front(); 855 | searching_idx_queue.pop(); 856 | 857 | // search the neighbor nodes 858 | std::vector neighbor_idxs; 859 | searchNeighborNodes(current_node_idx, neighbor_idxs); 860 | 861 | // set the traversable edges 862 | for (int i = 0; i < (int) neighbor_idxs.size(); i++){ 863 | // if the neighbor node is traversable, add it to the queue 864 | 865 | TriGridIdx n_i = neighbor_idxs[i]; 866 | 867 | 868 | if (tgf_in[n_i.row][n_i.col][n_i.tri].depth >=0){ 869 | continue; 870 | } 871 | 872 | if (tgf_in[n_i.row][n_i.col][n_i.tri].is_visited) { 873 | if (!tgf_in[n_i.row][n_i.col][n_i.tri].need_recheck){ 874 | continue; 875 | } else { 876 | if (tgf_in[n_i.row][n_i.col][n_i.tri].check_life <= 0){ 877 | continue; 878 | } 879 | } 880 | continue; 881 | } else { 882 | if (tgf_in[n_i.row][n_i.col][n_i.tri].node_type != GROUND) { 883 | continue; 884 | } 885 | } 886 | 887 | tgf_in[n_i.row][n_i.col][n_i.tri].is_visited =true; 888 | 889 | if (!LocalConvecityConcavity(tgf_in, current_node_idx, n_i, TH_LCC_NORMAL_SIMILARITY_, TH_LCC_PLANAR_MODEL_DIST_)) { 890 | tgf_in[n_i.row][n_i.col][n_i.tri].is_rejection = true; 891 | tgf_in[n_i.row][n_i.col][n_i.tri].node_type = NONGROUND; 892 | 893 | if(tgf_in[n_i.row][n_i.col][n_i.tri].check_life > 0) { 894 | tgf_in[n_i.row][n_i.col][n_i.tri].check_life -=1; 895 | tgf_in[n_i.row][n_i.col][n_i.tri].need_recheck = true; 896 | } else { 897 | tgf_in[n_i.row][n_i.col][n_i.tri].need_recheck = false; 898 | } 899 | continue; 900 | } 901 | 902 | if (max_planar_height < tgf_in[n_i.row][n_i.col][n_i.tri].mean_pt[2]) max_planar_height = tgf_in[n_i.row][n_i.col][n_i.tri].mean_pt[2]; 903 | 904 | tgf_in[n_i.row][n_i.col][n_i.tri].node_type = GROUND; 905 | tgf_in[n_i.row][n_i.col][n_i.tri].is_rejection = false; 906 | tgf_in[n_i.row][n_i.col][n_i.tri].depth = tgf_in[current_node_idx.row][current_node_idx.col][current_node_idx.tri].depth + 1; 907 | 908 | if (VIZ_MDOE_){ 909 | cur_edge.Pair.first = current_node_idx; 910 | cur_edge.Pair.second = n_i; 911 | cur_edge.is_traversable = true; 912 | trigrid_edges_.push_back(cur_edge); 913 | } 914 | 915 | searching_idx_queue.push(n_i); 916 | } 917 | 918 | if (searching_idx_queue.empty()){ 919 | // set the new dominant node 920 | for (int r_i = 0; r_i < rows_; r_i++) { 921 | for (int c_i = 0; c_i < cols_; c_i++) { 922 | for (int s_i = 0; s_i < (int) tgf_in[r_i][c_i].size() ; s_i++){ 923 | if (tgf_in[r_i][c_i][s_i].is_visited) { continue; } 924 | 925 | if (tgf_in[r_i][c_i][s_i].node_type != GROUND ) { continue; } 926 | 927 | // if (tgf_in[r_i][c_i][s_i].mean_pt[2] >= max_planar_height+1) { continue; } 928 | 929 | if (tgf_in[r_i][c_i][s_i].depth >= 0) { continue; } 930 | 931 | // if (tgf_in[r_i][c_i][s_i].weight > 5*TH_WEIGHT_){ 932 | tgf_in[r_i][c_i][s_i].depth = 0; 933 | tgf_in[r_i][c_i][s_i].is_visited = true; 934 | 935 | TriGridIdx new_dominant_idx = {r_i, c_i, s_i}; 936 | searching_idx_queue.push(new_dominant_idx); 937 | // } 938 | } 939 | } 940 | } 941 | } 942 | } 943 | return; 944 | }; 945 | 946 | 947 | double getCornerWeight(const TriGridNode& node_in, const pcl::PointXYZ &tgt_corner){ 948 | double xy_dist = sqrt( (node_in.mean_pt[0]-tgt_corner.x)*(node_in.mean_pt[0]-tgt_corner.x)+(node_in.mean_pt[1]-tgt_corner.y)*(node_in.mean_pt[1]-tgt_corner.y) ); 949 | return (node_in.weight/xy_dist); 950 | } 951 | 952 | void setTGFCornersCenters(const TriGridField& tgf_in, 953 | std::vector>& trigrid_corners_out, 954 | std::vector>& trigrid_centers_out) { 955 | pcl::PointXYZ corner_TL, corner_BL, corner_BR, corner_TR, corner_C; 956 | 957 | for (int r_i = 0; r_i>& trigrid_corners_out, 1051 | std::vector>& trigrid_centers_out) { 1052 | 1053 | // update corners 1054 | TriGridCorner updated_corner = empty_trigrid_corner_; 1055 | for (int r_i = 0; r_i < rows_ +1; r_i++) { 1056 | for (int c_i = 0; c_i < cols_ +1; c_i++) { 1057 | if (trigrid_corners_out[r_i][c_i].zs.size() > 0 && trigrid_corners_out[r_i][c_i].weights.size() > 0) { 1058 | updated_corner = getMeanCorner(trigrid_corners_out[r_i][c_i]); 1059 | trigrid_corners_out[r_i][c_i] = updated_corner; 1060 | } else { 1061 | trigrid_corners_out[r_i][c_i].zs.clear(); 1062 | trigrid_corners_out[r_i][c_i].weights.clear(); 1063 | } 1064 | } 1065 | } 1066 | 1067 | // update centers 1068 | TriGridCorner updated_center = empty_trigrid_center_; 1069 | for (int r_i = 0; r_i < rows_; r_i++) { 1070 | for (int c_i = 0; c_i < cols_; c_i++) { 1071 | if (trigrid_centers_out[r_i][c_i].zs.size() > 0 && trigrid_centers_out[r_i][c_i].weights.size() > 0) { 1072 | updated_center = getMeanCorner(trigrid_centers_out[r_i][c_i]); 1073 | trigrid_centers_out[r_i][c_i] = updated_center; 1074 | // trigrid_centers_out[r_i][c_i].z = get_mean(trigrid_centers_out[r_i][c_i].zs,trigrid_centers_out[r_i][c_i].weights); 1075 | } else { 1076 | trigrid_centers_out[r_i][c_i].zs.clear(); 1077 | trigrid_centers_out[r_i][c_i].weights.clear(); 1078 | } 1079 | } 1080 | } 1081 | 1082 | return; 1083 | }; 1084 | 1085 | Eigen::Vector3f convertCornerToEigen(TriGridCorner &corner_in) { 1086 | Eigen::Vector3f corner_out; 1087 | if (corner_in.zs.size() != corner_in.weights.size()){ 1088 | std::cout << "ERROR in corners" << std::endl; 1089 | } 1090 | corner_out[0] = corner_in.x; 1091 | corner_out[1] = corner_in.y; 1092 | corner_out[2] = corner_in.zs[0]; 1093 | return corner_out; 1094 | }; 1095 | 1096 | void revertTraversableNodes(std::vector>& trigrid_corners_in, 1097 | std::vector>& trigrid_centers_in, 1098 | TriGridField& tgf_out) { 1099 | Eigen::Vector3f refined_corner_1, refined_corner_2, refined_center; 1100 | for (int r_i = 0; r_i < rows_; r_i++) { 1101 | for (int c_i = 0; c_i < cols_; c_i++) { 1102 | for (int s_i = 0; s_i < (int) tgf_out[r_i][c_i].size(); s_i++) { 1103 | // set the corners for the current trigrid node 1104 | switch (s_i) 1105 | { 1106 | case 0: 1107 | if ( trigrid_corners_in[r_i+1][c_i].zs.size()==0 || trigrid_corners_in[r_i+1][c_i+1].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){ 1108 | if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){ 1109 | tgf_out[r_i][c_i][s_i].node_type = UNKNOWN; 1110 | } 1111 | continue; 1112 | } 1113 | refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i]); 1114 | refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i+1]); 1115 | refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]); 1116 | break; 1117 | case 1: 1118 | if ( trigrid_corners_in[r_i+1][c_i+1].zs.size()==0 || trigrid_corners_in[r_i][c_i+1].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){ 1119 | if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){ 1120 | tgf_out[r_i][c_i][s_i].node_type = UNKNOWN; 1121 | } 1122 | continue; 1123 | } 1124 | refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i+1]); 1125 | refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i][c_i+1]); 1126 | refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]); 1127 | break; 1128 | case 2: 1129 | if ( trigrid_corners_in[r_i][c_i+1].zs.size()==0 || trigrid_corners_in[r_i][c_i].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){ 1130 | if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){ 1131 | tgf_out[r_i][c_i][s_i].node_type = UNKNOWN; 1132 | } 1133 | continue; 1134 | } 1135 | refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i][c_i+1]); 1136 | refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i][c_i]); 1137 | refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]); 1138 | break; 1139 | case 3: 1140 | if ( trigrid_corners_in[r_i][c_i].zs.size()==0 || trigrid_corners_in[r_i+1][c_i].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){ 1141 | if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){ 1142 | tgf_out[r_i][c_i][s_i].node_type = UNKNOWN; 1143 | } 1144 | continue; 1145 | } 1146 | refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i][c_i]); 1147 | refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i]); 1148 | refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]); 1149 | break; 1150 | default: 1151 | std::cout << "WRONG tri-grid indexing" << std::endl; 1152 | break; 1153 | } 1154 | 1155 | // calculate the refined planar model in the node 1156 | Eigen::Vector3f udpated_normal = (refined_corner_1-refined_center).cross(refined_corner_2-refined_center); 1157 | udpated_normal /= udpated_normal.norm(); 1158 | // if (udpated_normal[2] < 0){ 1159 | // std::cout << "Origin normal: " << tgf_out[r_i][c_i][s_i].normal << std::endl; 1160 | // std::cout << "Update normal: " << udpated_normal << std::endl; 1161 | // } 1162 | if (udpated_normal(2,0) < TH_NORMAL_ ){ // non-planar 1163 | tgf_out[r_i][c_i][s_i].normal = udpated_normal; 1164 | tgf_out[r_i][c_i][s_i].node_type = NONGROUND; 1165 | } else { 1166 | // planar 1167 | Eigen::Vector3f updated_mean_pt; 1168 | updated_mean_pt[0] = (refined_corner_1[0] + refined_corner_2[0] + refined_center[0])/3; 1169 | updated_mean_pt[1] = (refined_corner_1[1] + refined_corner_2[1] + refined_center[1])/3; 1170 | updated_mean_pt[2] = (refined_corner_1[2] + refined_corner_2[2] + refined_center[2])/3; 1171 | 1172 | tgf_out[r_i][c_i][s_i].normal = udpated_normal; 1173 | tgf_out[r_i][c_i][s_i].mean_pt = updated_mean_pt; 1174 | tgf_out[r_i][c_i][s_i].d = -(udpated_normal.dot(updated_mean_pt)); 1175 | tgf_out[r_i][c_i][s_i].th_dist_d = TH_DIST_ - tgf_out[r_i][c_i][s_i].d; 1176 | tgf_out[r_i][c_i][s_i].th_outlier_d = -TH_OUTLIER_ - tgf_out[r_i][c_i][s_i].d; 1177 | 1178 | tgf_out[r_i][c_i][s_i].node_type = GROUND; 1179 | } 1180 | } 1181 | } 1182 | } 1183 | 1184 | return; 1185 | }; 1186 | 1187 | void fitTGFWiseTraversableTerrainModel(TriGridField& tgf, 1188 | std::vector>& trigrid_corners, 1189 | std::vector>& trigrid_centers) { 1190 | 1191 | updateTGFCornersCenters(trigrid_corners, trigrid_centers); 1192 | 1193 | revertTraversableNodes(trigrid_corners, trigrid_centers, tgf); 1194 | 1195 | return; 1196 | }; 1197 | 1198 | void segmentNodeGround(const TriGridNode& node_in, 1199 | pcl::PointCloud& node_ground_out, 1200 | pcl::PointCloud& node_nonground_out, 1201 | pcl::PointCloud& node_obstacle_out, 1202 | pcl::PointCloud& node_outlier_out) { 1203 | node_ground_out.clear(); 1204 | node_nonground_out.clear(); 1205 | node_obstacle_out.clear(); 1206 | node_outlier_out.clear(); 1207 | 1208 | // segment ground 1209 | Eigen::MatrixXf points(node_in.ptCloud.points.size(),3); 1210 | int j = 0; 1211 | for (auto& p:node_in.ptCloud.points){ 1212 | points.row(j++)<& tgf_in, 1236 | pcl::PointCloud& ground_cloud_out, 1237 | pcl::PointCloud& nonground_cloud_out, 1238 | pcl::PointCloud& obstacle_cloud_out, 1239 | pcl::PointCloud& outlier_cloud_out) { 1240 | ground_cloud_out.clear(); 1241 | nonground_cloud_out.clear(); 1242 | obstacle_cloud_out.clear(); 1243 | 1244 | for (int r_i = 0; r_i < rows_; r_i++){ 1245 | for (int c_i = 0; c_i < cols_; c_i++){ 1246 | for (int s_i = 0; s_i < tgf_in[r_i][c_i].size(); s_i++) { 1247 | if (!tgf_in[r_i][c_i][s_i].is_curr_data) { 1248 | continue; 1249 | } 1250 | if (tgf_in[r_i][c_i][s_i].node_type == GROUND) { 1251 | segmentNodeGround(tgf_in[r_i][c_i][s_i], ptCloud_nodewise_ground_, ptCloud_nodewise_nonground_, ptCloud_nodewise_obstacle_, ptCloud_nodewise_outliers_); 1252 | } else { 1253 | ptCloud_nodewise_nonground_ = tgf_in[r_i][c_i][s_i].ptCloud; 1254 | ptCloud_nodewise_obstacle_ = tgf_in[r_i][c_i][s_i].ptCloud; 1255 | } 1256 | ground_cloud_out += ptCloud_nodewise_ground_; 1257 | nonground_cloud_out += ptCloud_nodewise_nonground_; 1258 | outlier_cloud_out += ptCloud_nodewise_outliers_; 1259 | obstacle_cloud_out += ptCloud_nodewise_obstacle_; 1260 | } 1261 | } 1262 | } 1263 | 1264 | return; 1265 | }; 1266 | 1267 | void segmentTGFGround_developing(const TriGridField& tgf_in, 1268 | pcl::PointCloud& ground_cloud_out, 1269 | pcl::PointCloud& nonground_cloud_out, 1270 | pcl::PointCloud& obstacle_cloud_out, 1271 | pcl::PointCloud& outlier_cloud_out) { 1272 | ground_cloud_out.clear(); 1273 | nonground_cloud_out.clear(); 1274 | obstacle_cloud_out.clear(); 1275 | TriGridIdx curr_tgf_idx; 1276 | std::vector adj_idx_vec; 1277 | pcl::PointCloud outlier_tmp; 1278 | outlier_tmp.clear(); 1279 | outlier_tmp.reserve(NODEWISE_PTCLOUDSIZE); 1280 | for (int r_i = 0; r_i < rows_; r_i++){ 1281 | for (int c_i = 0; c_i < cols_; c_i++){ 1282 | for (int s_i = 0; s_i < (int) tgf_in[r_i][c_i].size(); s_i++) { 1283 | if (!tgf_in[r_i][c_i][s_i].is_curr_data) { 1284 | continue; 1285 | } 1286 | if (tgf_in[r_i][c_i][s_i].node_type == UNKNOWN){ 1287 | continue; 1288 | } 1289 | if (tgf_in[r_i][c_i][s_i].node_type == GROUND) { 1290 | segmentNodeGround(tgf_in[r_i][c_i][s_i], ptCloud_nodewise_ground_, ptCloud_nodewise_nonground_, ptCloud_nodewise_obstacle_, ptCloud_nodewise_outliers_); 1291 | } else { 1292 | curr_tgf_idx.row = r_i; 1293 | curr_tgf_idx.col = c_i; 1294 | curr_tgf_idx.tri = s_i; 1295 | 1296 | searchAdjacentNodes(curr_tgf_idx, adj_idx_vec); 1297 | if (adj_idx_vec.empty()){ 1298 | ptCloud_nodewise_nonground_ = tgf_in[r_i][c_i][s_i].ptCloud; 1299 | ptCloud_nodewise_obstacle_ = tgf_in[r_i][c_i][s_i].ptCloud; 1300 | } else { 1301 | TriGridIdx highest_adj_tri_idx; 1302 | double highest_weight = 0; 1303 | bool is_adjacent = false; 1304 | for (int adj_i = 0; adj_i highest_weight) { 1310 | highest_weight = getTriGridNode(adj_idx_vec[adj_i]).weight; 1311 | highest_adj_tri_idx = adj_idx_vec[adj_i]; 1312 | } 1313 | } 1314 | 1315 | if (is_adjacent){ 1316 | TriGridNode tmp_node = getTriGridNode(highest_adj_tri_idx); 1317 | tmp_node.ptCloud = getTriGridNode(curr_tgf_idx).ptCloud; 1318 | segmentNodeGround(tmp_node, ptCloud_nodewise_ground_, ptCloud_nodewise_nonground_, ptCloud_nodewise_obstacle_, ptCloud_nodewise_outliers_); 1319 | } else { 1320 | ptCloud_nodewise_nonground_ = tgf_in[r_i][c_i][s_i].ptCloud; 1321 | ptCloud_nodewise_obstacle_ = tgf_in[r_i][c_i][s_i].ptCloud; 1322 | } 1323 | } 1324 | } 1325 | ground_cloud_out += ptCloud_nodewise_ground_; 1326 | nonground_cloud_out += ptCloud_nodewise_nonground_; 1327 | obstacle_cloud_out += ptCloud_nodewise_obstacle_; 1328 | outlier_cloud_out += ptCloud_nodewise_outliers_; 1329 | } 1330 | } 1331 | } 1332 | 1333 | return; 1334 | }; 1335 | }; 1336 | } 1337 | #endif -------------------------------------------------------------------------------- /travel_api.py: -------------------------------------------------------------------------------- 1 | import travel 2 | import open3d as o3d 3 | import numpy as np 4 | 5 | class Travel: 6 | def __init__(self): 7 | self.tgs = travel.TravelGroundSeg() 8 | self.set_params() 9 | 10 | def set_params(self, 11 | min_range=2.0,max_range=75.0,tgf_res=8.0, 12 | num_iter=3,num_lpr=5,num_min_pts=5, 13 | th_seeds=0.5,th_dist=0.125,th_outlier=1.0,th_normal=0.94, 14 | th_weight=100,th_obstacle=0.5, 15 | th_lcc_normal_similarity=0.03,th_lcc_planar_dist=0.2, 16 | refine_mode=False,viz_mode=False): 17 | 18 | self.tgs.setParams(max_range, min_range, tgf_res, 19 | num_iter, num_lpr, num_min_pts, th_seeds, 20 | th_dist, th_outlier, th_normal, th_weight, 21 | th_lcc_normal_similarity, th_lcc_planar_dist, th_obstacle, 22 | refine_mode, viz_mode) 23 | 24 | def estimate_ground(self, points): 25 | """ 26 | Takes in a numpy array of points (N,M) and returns the ground and nonground pc 27 | """ 28 | # Converting np array to np.float64 speeds up conversion to Vector3dVector by a lot (0.227s -> 0.003s) 29 | # https://github.com/isl-org/Open3D/issues/1045#issuecomment-513169038 30 | in_pc = o3d.utility.Vector3dVector(points[:,:3].astype(dtype=np.float64)) 31 | ground_pc = o3d.utility.Vector3dVector() 32 | nonground_pc = o3d.utility.Vector3dVector() 33 | ground_inds = o3d.utility.Vector3dVector() 34 | self.tgs.estimateGround(in_pc, ground_pc, nonground_pc, ground_inds) 35 | 36 | return np.asarray(ground_pc, dtype=np.float64), np.asarray(nonground_pc, dtype=np.float64), np.asarray(ground_inds, dtype=int)[:,0] 37 | 38 | if __name__ == "__main__": 39 | # from travel_api import Travel # if using from another python file 40 | 41 | # Read pcd 42 | pcd = o3d.io.read_point_cloud('/TRAVEL/demo.pcd') 43 | in_pc = np.asarray(pcd.points) 44 | print(f'Loaded input cloud: {np.asarray(pcd.points).shape}') 45 | 46 | ground_seg = Travel() 47 | ground_pc, nonground_pc, ground_inds = ground_seg.estimate_ground(in_pc) 48 | 49 | print(f'Ground cloud: {ground_pc.shape}') 50 | print(f'Non-ground cloud: {nonground_pc.shape}') -------------------------------------------------------------------------------- /travel_demo.cpp: -------------------------------------------------------------------------------- 1 | #define PCL_NO_PRECOMPILE 2 | 3 | // Might need to: apt install libpcl-dev 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | //#include 20 | #include 21 | 22 | #include "tgs.hpp" 23 | 24 | struct PointXYZILID 25 | { 26 | PCL_ADD_POINT4D; // quad-word XYZ 27 | float intensity; ///< laser intensity reading 28 | uint16_t label; ///< point label 29 | uint32_t id; 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 31 | } EIGEN_ALIGN16; 32 | 33 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID, 34 | (float, x, x) 35 | (float, y, y) 36 | (float, z, z) 37 | (float, intensity, intensity) 38 | (uint16_t, label, label) 39 | (uint32_t, id, id)) 40 | 41 | using PointT = PointXYZILID; 42 | 43 | boost::shared_ptr> travel_ground_seg; 44 | 45 | pcl::PointCloud::Ptr cloud_in; 46 | pcl::PointCloud::Ptr filtered_pc; 47 | pcl::PointCloud::Ptr ground_pc; 48 | pcl::PointCloud::Ptr nonground_pc; 49 | pcl::PointCloud::Ptr outlier_pc; 50 | pcl::PointCloud::Ptr labeled_pc; 51 | 52 | int main(void) 53 | { 54 | cloud_in.reset(new pcl::PointCloud()); 55 | filtered_pc.reset(new pcl::PointCloud()); 56 | ground_pc.reset(new pcl::PointCloud()); 57 | nonground_pc.reset(new pcl::PointCloud()); 58 | labeled_pc.reset(new pcl::PointCloud()); 59 | pcl::PCDReader reader; 60 | reader.read("demo.pcd", *cloud_in); 61 | std::cout << "Read Cloud Data Points Size: " << cloud_in->points.size() << std::endl; 62 | 63 | travel_ground_seg.reset(new travel::TravelGroundSeg()); 64 | 65 | // Ground seg 66 | float min_range_ = 2.0; 67 | float max_range_= 300.0; 68 | float tgf_res = 8.0; 69 | int num_iter = 3; 70 | int num_lpr = 5; 71 | int num_min_pts = 10; 72 | float th_seeds = 0.5; 73 | float th_dist = 0.125; 74 | float th_outlier = 1.0; 75 | float th_normal = 0.94; 76 | float th_weight = 100; 77 | float th_obstacle = 0.5; 78 | float th_lcc_normal_similarity = 0.03; 79 | float th_lcc_planar_dist = 0.2; 80 | 81 | bool refine_mode = false; 82 | bool viz_mode = false; 83 | 84 | travel_ground_seg->setParams(max_range_, min_range_, tgf_res, 85 | num_iter, num_lpr, num_min_pts, th_seeds, 86 | th_dist, th_outlier, th_normal, th_weight, 87 | th_lcc_normal_similarity, th_lcc_planar_dist, th_obstacle, 88 | refine_mode, viz_mode); 89 | 90 | filtered_pc->points.reserve(cloud_in->points.size()); 91 | for (auto &point : cloud_in->points){ 92 | // bool is_nan = std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z); 93 | // double pt_range = 0.0; 94 | // if (is_nan){ 95 | // continue; 96 | // } 97 | // pt_range = sqrt(pow(point.x, 2) + pow(point.y, 2) + pow(point.z, 2)); 98 | // if (pt_range <= min_range_ || pt_range >= max_range_){ 99 | // continue; 100 | // } 101 | filtered_pc->push_back(point); 102 | } 103 | 104 | // Convert from pcl::PointCloud to std::vector 105 | std::vector nonground_pc_vec; 106 | std::vector ground_pc_vec; 107 | std::vector filtered_pc_vec; 108 | std::vector ground_inds_vec; 109 | for (auto &point : filtered_pc->points){ 110 | Eigen::Vector3d point_eigen (static_cast(point.x), static_cast(point.y), static_cast(point.z)); 111 | filtered_pc_vec.push_back(point_eigen); 112 | } 113 | 114 | // Apply traversable ground segmentation 115 | travel_ground_seg->estimateGround(filtered_pc_vec, ground_pc_vec, nonground_pc_vec, ground_inds_vec); 116 | 117 | // Convert from std::vector to pcl::PointCloud 118 | for (auto &point_vec : ground_pc_vec){ 119 | PointT point; 120 | point.x = point_vec[0]; 121 | point.y = point_vec[1]; 122 | point.z = point_vec[2]; 123 | ground_pc->push_back(point); 124 | } 125 | for (auto &point_vec : nonground_pc_vec){ 126 | PointT point; 127 | point.x = point_vec[0]; 128 | point.y = point_vec[1]; 129 | point.z = point_vec[2]; 130 | nonground_pc->push_back(point); 131 | } 132 | 133 | std::cout << "\033[1;35m Traversable-Ground Seg: " << filtered_pc->size() << " -> Ground: " << ground_pc->size() << ", NonGround: " << nonground_pc->size() << "\033[0m" << std::endl; 134 | 135 | // save floor cloud & nofloor cloud data. 136 | pcl::PCDWriter writer; 137 | ground_pc->width = 1; 138 | ground_pc->height = ground_pc->points.size(); 139 | writer.write("onlyfloor.pcd", *ground_pc); 140 | 141 | nonground_pc->width = 1; 142 | nonground_pc->height = nonground_pc->points.size(); 143 | writer.write("nofloor.pcd", *nonground_pc); 144 | 145 | } 146 | -------------------------------------------------------------------------------- /travel_demo.py: -------------------------------------------------------------------------------- 1 | from travel_api import Travel 2 | import open3d as o3d 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | # Read pcd 7 | pcd = o3d.io.read_point_cloud('/TRAVEL/demo.pcd') 8 | in_pc = np.asarray(pcd.points) 9 | print(f'Loaded input cloud: {np.asarray(pcd.points).shape}') 10 | 11 | ground_seg = Travel() 12 | ground_pc, nonground_pc, ground_inds = ground_seg.estimate_ground(in_pc) 13 | 14 | print(f'Ground cloud: {ground_pc.shape}') 15 | print(f'Non-ground cloud: {nonground_pc.shape}') 16 | 17 | # Visualize 18 | nonground_o3d = o3d.geometry.PointCloud() 19 | nonground_o3d.points = o3d.utility.Vector3dVector(nonground_pc) 20 | cmap = plt.get_cmap('tab20') 21 | 22 | # Db scan to cluster objects - downsample then cluster 23 | downpcd, idx_c, idx_vox = nonground_o3d.voxel_down_sample_and_trace(0.2, nonground_o3d.get_min_bound(), nonground_o3d.get_max_bound(), False) 24 | cluster_labels = np.array(downpcd.cluster_dbscan(eps=0.3, min_points=5), dtype=int) # Converting to int here speeds up the computation a little 25 | 26 | # Apply cluster ID to every point in the voxel 27 | pcd_cids = np.zeros((np.asarray(nonground_o3d.points).shape[0]), dtype=np.int32) # For some reason np.int32 here is faster than python native int 28 | for (vox_ids, cls_ids) in zip(idx_vox, cluster_labels): 29 | pcd_cids[np.array(vox_ids, dtype=int)] = cls_ids # Converting vox_ids to int here speeds up the computation by a lot 30 | 31 | colors = np.ones((pcd_cids.shape[0],3)) 32 | colors[pcd_cids != -1] = cmap(pcd_cids[pcd_cids != -1]%20)[:,:3] 33 | nonground_o3d.colors = o3d.utility.Vector3dVector(colors) 34 | 35 | ground_pcd = o3d.geometry.PointCloud() 36 | ground_pcd.points = o3d.utility.Vector3dVector(ground_pc) 37 | ground_pcd.paint_uniform_color([0,0,0]) 38 | o3d.visualization.draw_geometries([ground_pcd, nonground_o3d]) 39 | -------------------------------------------------------------------------------- /travel_pybind.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "tgs.hpp" 4 | 5 | namespace py = pybind11; 6 | 7 | struct PointXYZILID 8 | { 9 | PCL_ADD_POINT4D; // quad-word XYZ 10 | float intensity; ///< laser intensity reading 11 | uint16_t label; ///< point label 12 | uint32_t id; 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 14 | } EIGEN_ALIGN16; 15 | 16 | 17 | PYBIND11_MODULE(travel, m) { 18 | py::class_>(m, "TravelGroundSeg") 19 | .def(py::init<>()) 20 | .def("setParams", &travel::TravelGroundSeg::setParams) 21 | .def("estimateGround", &travel::TravelGroundSeg::estimateGround); 22 | } --------------------------------------------------------------------------------