├── LICENSE ├── README.md ├── code ├── CMakeLists.txt ├── jpc_groundremove.cpp ├── jpc_groundremove.h └── main.cpp ├── full_cloud2.pcd ├── image ├── JPC.png ├── JPC_principle.png ├── RECM.png ├── RECM_priciple.png ├── algorithm1.png ├── algorithm2.png ├── dilate.png └── lidar_result.png ├── jpc_seg ├── CMakeLists.txt ├── package.xml └── src │ ├── jpc_groundremove.cpp │ ├── jpc_groundremove.h │ ├── kitti_file_main.cpp │ └── kitti_topic_main.cpp ├── remotesensing-13-03239.pdf └── test.gif /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fast-Ground-Segmentation-Based-on-JPC 2 | 3 | [![build passing](https://img.shields.io/badge/build-passing-brightgreen.svg)](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC) [![velodyne_HDL_64 compliant](https://img.shields.io/badge/velodyne_HDL_64-compliant-red.svg)](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC) [![ros kinetic](https://img.shields.io/badge/ros-kinetic-blue.svg)](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC) 4 | 5 | An implementation on ["Shen Z, Liang H, Lin L, Wang Z, Huang W, Yu J. Fast Ground Segmentation for 3D LiDAR Point Cloud Based on Jump-Convolution-Process. Remote Sensing. 2021; 13(16):3239. https://doi.org/10.3390/rs13163239"](https://www.mdpi.com/2072-4292/13/16/3239/) 6 | 7 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/test.gif) 8 | 9 | ## update on 2021/11/10 10 | 11 | 1. Support ros 12 | 13 | 2. Fix some bug, but still need to be improved 14 | 15 | 16 | ## 1.Priciple of Algorithm 17 | 18 | #### (1) RECM for Coarse Ground Segmentation 19 | 20 | ##### Image 21 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/RECM_priciple.png) 22 | 23 | ##### Algorithm 24 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/algorithm1.png) 25 | 26 | #### (2) JPC for Refine 27 | 28 | ##### Image 29 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/JPC_principle.png) 30 | 31 | ##### Algorithm 32 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/algorithm2.png) 33 | 34 | More detail please check in the paper. 35 | 36 | ## 2.How to Use 37 | 38 | mkdir build 39 | cd build 40 | make -j 41 | ./main your lidarfile 42 | 43 | ## 3.Result 44 | 45 | #### (1) RECM 46 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/RECM.png) 47 | 48 | #### (2) Dilate 49 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/dilate.png) 50 | 51 | #### (3) JPC 52 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/JPC.png) 53 | 54 | #### (4) Pointcloud 55 | ![Image text](https://github.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/blob/main/image/lidar_result.png) 56 | -------------------------------------------------------------------------------- /code/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(point) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -g") 4 | set(CMAKE_BUILD_TYPE "Release") 5 | find_package(PCL 1.8 REQUIRED) 6 | #find_package(CSF REQUIRED) 7 | find_package(Eigen3 REQUIRED) 8 | find_package(Boost REQUIRED COMPONENTS system iostreams) 9 | 10 | find_package(OpenCV REQUIRED) 11 | 12 | add_definitions( ${PCL_DEFINITIONS} ) 13 | include_directories(${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ) 14 | 15 | 16 | add_executable(main main.cpp jpc_groundremove.cpp) 17 | target_link_libraries(main ${EIGEN3_INCLUDE_DIR} ${PCL_LIBRARIES} ${Boost_INCLUDE_DIRS} ${OpenCV_LIBS}) 18 | 19 | -------------------------------------------------------------------------------- /code/jpc_groundremove.cpp: -------------------------------------------------------------------------------- 1 | #include "jpc_groundremove.h" 2 | 3 | JpcGroundRemove::JpcGroundRemove(){ 4 | pcl::PointCloud::Ptr tempc(new pcl::PointCloud); 5 | cloud_ = tempc; 6 | length_ = int((max_range_ - min_range)/deltaR_); 7 | region_minz_.assign(width_ * length_, 100); 8 | cloud_index_.assign(width_ * height_, -1); 9 | range_image_ = cv::Mat::zeros(height_, width_, CV_8UC3); 10 | region_ = cv::Mat::zeros(height_, width_, CV_8UC1); 11 | } 12 | 13 | void JpcGroundRemove::calAngle(float x, float y, float &temp_tangle){ 14 | if (x == 0 && y == 0) { 15 | temp_tangle = 0; 16 | } else if (y >= 0) { 17 | temp_tangle = (float) atan2(y, x); 18 | } else if (y <= 0) { 19 | temp_tangle = (float) atan2(y, x) + 2 * M_PI; 20 | } 21 | } 22 | 23 | 24 | void JpcGroundRemove::calRange(const PointXYZIR& p, float& range){ 25 | range = sqrt(p.x*p.x + p.y*p.y); 26 | } 27 | 28 | void JpcGroundRemove::calRangeDiff(const PointXYZIR& p1, const PointXYZIR& p2, float& range){ 29 | range = sqrt((p1.x- p2.x)*(p1.x- p2.x)+(p1.y- p2.y)*(p1.y- p2.y) +(p1.z- p2.z)*(p1.z- p2.z)); 30 | } 31 | 32 | void JpcGroundRemove::RangeProjection(){ 33 | cout<points.size()<points.size(); ++i){ 36 | float u(0), range(0); 37 | 38 | calAngle(cloud_->points[i].x, cloud_->points[i].y, u); 39 | calRange(cloud_->points[i], range); 40 | if(range<2 || range>70) 41 | continue; 42 | 43 | int col = round((width_-1)*(u *180.0/M_PI)/360.0); 44 | int ind = cloud_->points[i].ring; 45 | int region = int((range-min_range)/deltaR_); 46 | 47 | int region_index = col * length_ + region; 48 | int index = col * height_ + ind; 49 | range_image_.at(ind, col) = cv::Vec3b(0,255,0); 50 | region_minz_[region_index] = min(region_minz_[region_index], cloud_->points[i].z); 51 | region_.at(ind, col) = region; 52 | cloud_index_[index] = i; 53 | } 54 | // cv::Mat show_image; 55 | // cv::flip(range_image_,show_image, 0); 56 | // cv::imshow("TEst", show_image); 57 | // cv::waitKey(0); 58 | } 59 | 60 | void JpcGroundRemove::RECM(){ 61 | float pre_th = 0.; 62 | for(int i=0; i(j, i); 75 | float th_height = region_minz_[i*length_ + region_i]; 76 | int id = cloud_index_[index]; 77 | if(id == -1) 78 | continue; 79 | if(cloud_->points[id].z >= (th_height+th_g_)){ 80 | range_image_.at(j, i) = cv::Vec3b(0,0,255); 81 | } 82 | } 83 | } 84 | //cv::Mat show_image; 85 | //cv::flip(range_image_,show_image, 0); 86 | //cv::imshow("TEst", show_image); 87 | //cv::waitKey(0); 88 | } 89 | 90 | void JpcGroundRemove::JCP(){ 91 | vector channels; 92 | cv::split(range_image_, channels); 93 | cv::Mat element = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(5,5)); 94 | cv::dilate(channels[2],channels[2],element); 95 | cv::merge(channels, range_image_); 96 | 97 | queue qt; 98 | for(int i=0; i(j, i) == cv::Vec3b(0,255,255)){ 101 | Index id; 102 | id.x = i; 103 | id.y = j; 104 | if(cloud_index_[j * height_ + i] != -1){ 105 | qt.push(id); 106 | range_image_.at(j, i) = cv::Vec3b(255,0,0); 107 | }else{ 108 | range_image_.at(j, i) = cv::Vec3b(0,0,255); 109 | } 110 | } 111 | } 112 | } 113 | // cv::Mat show_image; 114 | // cv::flip(range_image_,show_image, 0); 115 | // cv::imshow("TEst", show_image); 116 | // cv::waitKey(0); 117 | 118 | while(!qt.empty()){ 119 | Index id = qt.front(); 120 | qt.pop(); 121 | int cloud_id = id.x * height_ + id.y; 122 | Eigen::VectorXf D(24); 123 | int mask[24]; 124 | float sumD(0); 125 | for(int i=0; i<24; ++i){ 126 | int nx = neighborx_[i] + id.x; 127 | int ny = neighbory_[i] + id.y; 128 | 129 | int ncloud_id = nx * height_ + ny; 130 | float range_diff(0); 131 | 132 | if(nx < 0 || nx >= width_ || ny < 0 ||ny >= height_ || cloud_index_[ncloud_id] == -1){ 133 | D(i) = 0; 134 | sumD += D(i) ; 135 | mask[i] = -1; 136 | continue; 137 | } 138 | 139 | calRangeDiff(cloud_->points[cloud_index_[cloud_id]], cloud_->points[cloud_index_[ncloud_id]], range_diff); 140 | if(range_diff > 0){ 141 | D(i) = 0; 142 | sumD += D(i); 143 | }else{ 144 | D(i) = (exp(-5 * range_diff)); 145 | sumD += D(i); 146 | } 147 | if(range_image_.at(ny, nx) == cv::Vec3b(255,0,0)){ 148 | mask[i] = 2; 149 | }else if(range_image_.at(ny, nx) == cv::Vec3b(0,255,0)){ 150 | mask[i] = 1; 151 | }else if(range_image_.at(ny, nx) == cv::Vec3b(0,0,255)){ 152 | mask[i] = 0; 153 | } 154 | } 155 | 156 | Eigen::VectorXf W(24); 157 | W = D / sumD; 158 | 159 | float score_r(0), score_g(0); 160 | for(int i=0; i score_g){ 169 | range_image_.at(id.y, id.x) = cv::Vec3b(0,0,255); 170 | }else{ 171 | range_image_.at(id.y, id.x) = cv::Vec3b(0,255,0); 172 | } 173 | } 174 | // cv::flip(range_image_,show_image, 0); 175 | // cv::imshow("TEst", show_image); 176 | // cv::waitKey(0); 177 | } 178 | 179 | void JpcGroundRemove::GroundRemove(pcl::PointCloud& cloud_IN, 180 | pcl::PointCloud& cloud_gr, 181 | pcl::PointCloud& cloud_ob){ 182 | 183 | for(auto p:cloud_IN.points){ 184 | cloud_->points.push_back(p); 185 | } 186 | RangeProjection(); 187 | RECM(); 188 | JCP(); 189 | 190 | for(int i=0; i(j, i) == cv::Vec3b(0,255,0)){ 196 | cloud_gr.push_back(cloud_->points[index]); 197 | }else if(range_image_.at(j, i) == cv::Vec3b(0,0,255)){ 198 | cloud_ob.push_back(cloud_->points[index]); 199 | } 200 | } 201 | } 202 | } 203 | -------------------------------------------------------------------------------- /code/jpc_groundremove.h: -------------------------------------------------------------------------------- 1 | #ifndef JPC_GROUNDREMOVE_H 2 | #define JPC_GROUNDREMOVE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | //PCL 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include //自定义点云类型时要加 29 | 30 | //Eigen 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | using namespace std; 39 | 40 | struct PointXYZIR{ 41 | PCL_ADD_POINT4D; 42 | float intensity; 43 | uint16_t ring; 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | }EIGEN_ALIGN16; 46 | 47 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR, 48 | (float,x,x) 49 | (float,y,y) 50 | (float,z,z) 51 | (float,intensity,intensity) 52 | (uint16_t,ring,ring) 53 | ) 54 | 55 | struct Index{ 56 | int x = 0; 57 | int y = 0; 58 | }; 59 | 60 | 61 | class JpcGroundRemove{ 62 | 63 | public: 64 | JpcGroundRemove(); 65 | ~JpcGroundRemove(){} 66 | 67 | void RangeProjection(); 68 | void RECM(); 69 | void JCP(); 70 | void GroundRemove(pcl::PointCloud& cloud_IN, 71 | pcl::PointCloud& cloud_gr, 72 | pcl::PointCloud& cloud_ob); 73 | void calAngle(float x, float y, float &temp_tangle); 74 | void calRange(const PointXYZIR& p, float& range); 75 | void calRangeDiff(const PointXYZIR& p1, const PointXYZIR& p2, float& range); 76 | 77 | private: 78 | 79 | pcl::PointCloud::Ptr cloud_; 80 | 81 | int width_ = 2083; 82 | int height_ = 64; 83 | float max_range_ = 70.0; 84 | float min_range = 2.0; 85 | int length_ = 0; 86 | 87 | int neighborx_[24] = {-2, -1, 0, 1, 2,-2, -1, 0, 1, 2,-2,-1, 1, 2, -2,-1,0, 1, 2,-2,-1, 0, 1, 2}; 88 | int neighbory_[24] = {-2, -2, -2, -2, -2,-1, -1, -1, -1, -1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2}; 89 | 90 | vector region_minz_; 91 | //vector cloud_index_; 92 | vector cloud_index_; 93 | cv::Mat range_image_; 94 | cv::Mat region_; 95 | 96 | 97 | float th_g_ = 0.2; 98 | float sigma_ = 7.; 99 | float deltaR_ = 2.; 100 | float th_d_ = 1; 101 | 102 | }; 103 | 104 | #endif 105 | 106 | 107 | -------------------------------------------------------------------------------- /code/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "jpc_groundremove.h" 3 | 4 | int main(int argc, char** argv){ 5 | 6 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 7 | pcl::io::loadPCDFile(argv[1], *cloud); 8 | cloud->height = 1; 9 | cloud->width = cloud->points.size(); 10 | 11 | cout<< cloud->points.size()<is_dense = false; 13 | 14 | pcl::PointCloud::Ptr cloud_gr(new pcl::PointCloud); 15 | pcl::PointCloud::Ptr cloud_ob(new pcl::PointCloud); 16 | 17 | JpcGroundRemove groundremove; 18 | groundremove.GroundRemove(*cloud, *cloud_gr, *cloud_ob); 19 | 20 | 21 | boost::shared_ptr viewer( 22 | new pcl::visualization::PCLVisualizer("pcd")); //PCLVisualizer 可视化类 23 | viewer->setBackgroundColor(0.1, 0.1, 0.1); 24 | 25 | pcl::visualization::PointCloudColorHandlerCustom color_gr(cloud_gr, (0), (255), (0)); 26 | string s_gr = "cloud_gr"; 27 | viewer->addPointCloud(cloud_gr, color_gr, s_gr); 28 | 29 | pcl::visualization::PointCloudColorHandlerCustom color_ob(cloud_ob, (255), (0), (0)); 30 | string s_ob = "cloud_ob"; 31 | viewer->addPointCloud(cloud_ob, color_ob, s_ob); 32 | 33 | while (!viewer->wasStopped()) { 34 | viewer->spin(); 35 | } 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /image/JPC.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/JPC.png -------------------------------------------------------------------------------- /image/JPC_principle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/JPC_principle.png -------------------------------------------------------------------------------- /image/RECM.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/RECM.png -------------------------------------------------------------------------------- /image/RECM_priciple.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/RECM_priciple.png -------------------------------------------------------------------------------- /image/algorithm1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/algorithm1.png -------------------------------------------------------------------------------- /image/algorithm2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/algorithm2.png -------------------------------------------------------------------------------- /image/dilate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/dilate.png -------------------------------------------------------------------------------- /image/lidar_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/image/lidar_result.png -------------------------------------------------------------------------------- /jpc_seg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(jpc_seg) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | SET(CMAKE_BUILD_TYPE "Release") 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | cv_bridge 12 | image_transport 13 | roscpp 14 | rospy 15 | sensor_msgs 16 | std_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | find_package(Eigen3 REQUIRED) 22 | find_package(OpenCV) 23 | find_package(PCL 1.8 REQUIRED) 24 | find_package(Boost REQUIRED COMPONENTS filesystem thread system program_options) 25 | 26 | add_definitions(${PCL_DEFINITIONS}) 27 | 28 | catkin_package( 29 | # INCLUDE_DIRS include 30 | # LIBRARIES pub_lidar 31 | CATKIN_DEPENDS cv_bridge image_transport roscpp rospy sensor_msgs std_msgs 32 | # DEPENDS system_lib 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | ## Specify additional locations of header files 40 | ## Your package locations should be listed before other locations 41 | include_directories( 42 | # include 43 | ${catkin_INCLUDE_DIRS} 44 | ${EIGEN3_INCLUDE_DIR} 45 | ${Boost_INCLUDE_DIRS} 46 | ${PCL_INCLUDE_DIRS} 47 | ) 48 | 49 | 50 | add_executable(jpc_seg_node src/kitti_topic_main.cpp src/jpc_groundremove.cpp) 51 | target_link_libraries(jpc_seg_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ${PCL_LIBRARIES}) 52 | add_executable(jpc_seg_node2 src/kitti_file_main.cpp src/jpc_groundremove.cpp) 53 | target_link_libraries(jpc_seg_node2 ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ${PCL_LIBRARIES}) 54 | 55 | -------------------------------------------------------------------------------- /jpc_seg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jpc_seg 4 | 0.0.0 5 | The jpc_seg package 6 | 7 | 8 | 9 | 10 | wx 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | cv_bridge 53 | image_transport 54 | roscpp 55 | rospy 56 | sensor_msgs 57 | std_msgs 58 | cv_bridge 59 | image_transport 60 | roscpp 61 | rospy 62 | sensor_msgs 63 | std_msgs 64 | cv_bridge 65 | image_transport 66 | roscpp 67 | rospy 68 | sensor_msgs 69 | std_msgs 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /jpc_seg/src/jpc_groundremove.cpp: -------------------------------------------------------------------------------- 1 | #include "jpc_groundremove.h" 2 | 3 | JpcGroundRemove::JpcGroundRemove(){ 4 | pcl::PointCloud::Ptr tempc(new pcl::PointCloud); 5 | cloud_ = tempc; 6 | length_ = int((max_range_ - min_range)/deltaR_); 7 | region_minz_.assign(width_ * length_, 100); 8 | cloud_index_.assign(width_ * height_, -1); 9 | range_image_ = cv::Mat::zeros(height_, width_, CV_8UC3); 10 | region_ = cv::Mat::zeros(height_, width_, CV_8UC1); 11 | } 12 | 13 | 14 | void JpcGroundRemove::calAngle(float x, float y, float &temp_tangle){ 15 | if (x == 0 && y == 0) { 16 | temp_tangle = 0; 17 | } else if (y >= 0) { 18 | temp_tangle = (float) atan2(y, x); 19 | } else if (y <= 0) { 20 | temp_tangle = (float) atan2(y, x) + 2 * M_PI; 21 | } 22 | } 23 | 24 | 25 | void JpcGroundRemove::calRange(const PointXYZIR& p, float& range){ 26 | range = sqrt(p.x*p.x + p.y*p.y); 27 | } 28 | 29 | 30 | void JpcGroundRemove::calRangeDiff(const PointXYZIR& p1, const PointXYZIR& p2, float& range){ 31 | range = sqrt((p1.x- p2.x)*(p1.x- p2.x)+(p1.y- p2.y)*(p1.y- p2.y) +(p1.z- p2.z)*(p1.z- p2.z)); 32 | } 33 | 34 | 35 | void JpcGroundRemove::RangeProjection(){ 36 | //cout<points.size()<points.size(); ++i){ 38 | float u(0), range(0); 39 | 40 | calAngle(cloud_->points[i].x, cloud_->points[i].y, u); 41 | calRange(cloud_->points[i], range); 42 | 43 | int col = round((width_-1)*(u *180.0/M_PI)/360.0); 44 | int ind = cloud_->points[i].ring; 45 | if(range<2 || range>70 || col<0 || col > width_ || ind <0 || ind > height_ || 46 | ((cloud_->points[i].x < 3 && cloud_->points[i].x > -2) 47 | &&(cloud_->points[i].y < 1.5 && cloud_->points[i].y > -1.5)) 48 | || (cloud_->points[i].z < -3 && cloud_->points[i].z > 1)) 49 | continue; 50 | 51 | int region = int((range-min_range)/deltaR_); 52 | 53 | int region_index = col * length_ + region; 54 | int index = col * height_ + ind; 55 | range_image_.at(ind, col) = cv::Vec3b(0,255,0); 56 | region_minz_[region_index] = min(region_minz_[region_index], cloud_->points[i].z); 57 | region_.at(ind, col) = region; 58 | cloud_index_[index] = i; 59 | } 60 | // cv::Mat show_image; 61 | // cv::flip(range_image_,show_image, 0); 62 | // cv::imshow("TEst", show_image); 63 | // cv::waitKey(0); 64 | } 65 | 66 | 67 | void JpcGroundRemove::RECM(){ 68 | bool flag = false; 69 | for(int i=0; i0.5 && fabs(region_minz_[i] - region_minz_[i + 1])>0.5) 86 | region_minz_[i] = (region_minz_[i-1] +region_minz_[i+1])/2; 87 | } 88 | } 89 | 90 | float pre_th = 0.; 91 | float region_num = 0; 92 | for(int i=0; i(j, i); 106 | float th_height = region_minz_[i*length_ + region_i]; 107 | int id = cloud_index_[index]; 108 | if(id == -1) 109 | continue; 110 | if(cloud_->points[id].z >= (th_height+th_g_)){ 111 | range_image_.at(j, i) = cv::Vec3b(0,0,255); 112 | } 113 | 114 | } 115 | } 116 | //cv::Mat show_image; 117 | //cv::flip(range_image_,show_image, 0); 118 | //cv::imshow("RECM image", show_image); 119 | //cv::waitKey(1); 120 | } 121 | 122 | 123 | cv::Mat JpcGroundRemove::JCP(){ 124 | vector channels; 125 | cv::split(range_image_, channels); 126 | cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5,5)); 127 | cv::dilate(channels[2],channels[2],element); 128 | 129 | cv::merge(channels, range_image_); 130 | 131 | queue qt; 132 | for(int i=0; i(j, i) == cv::Vec3b(0,255,255)){ 135 | Index id; 136 | id.x = i; 137 | id.y = j; 138 | if(cloud_index_[j * height_ + i] != -1){ 139 | qt.push(id); 140 | range_image_.at(j, i) = cv::Vec3b(255,0,0); 141 | }else{ 142 | range_image_.at(j, i) = cv::Vec3b(0,0,255); 143 | } 144 | } 145 | } 146 | } 147 | cv::Mat show_image; 148 | cv::flip(range_image_,show_image, 0); 149 | cv::imshow("Dilate image", show_image); 150 | cv::waitKey(1); 151 | 152 | while(!qt.empty()){ 153 | Index id = qt.front(); 154 | qt.pop(); 155 | int cloud_id = id.x * height_ + id.y; 156 | Eigen::VectorXf D(24); 157 | int mask[24]; 158 | float sumD(0); 159 | for(int i=0; i<24; ++i){ 160 | int nx = neighborx_[i] + id.x; 161 | int ny = neighbory_[i] + id.y; 162 | 163 | int ncloud_id = nx * height_ + ny; 164 | float range_diff(0); 165 | 166 | if(nx < 0 || nx >= width_ || ny < 0 ||ny >= height_ || cloud_index_[ncloud_id] == -1){ 167 | D(i) = 0; 168 | sumD += D(i) ; 169 | mask[i] = -1; 170 | continue; 171 | } 172 | 173 | calRangeDiff(cloud_->points[cloud_index_[cloud_id]], cloud_->points[cloud_index_[ncloud_id]], range_diff); 174 | if(range_diff > 3){ 175 | D(i) = 0; 176 | sumD += D(i); 177 | }else{ 178 | D(i) = (exp(-5 * range_diff)); 179 | sumD += D(i); 180 | } 181 | if(range_image_.at(ny, nx) == cv::Vec3b(255,0,0)){ 182 | mask[i] = 2; 183 | }else if(range_image_.at(ny, nx) == cv::Vec3b(0,255,0)){ 184 | mask[i] = 1; 185 | }else if(range_image_.at(ny, nx) == cv::Vec3b(0,0,255)){ 186 | mask[i] = 0; 187 | } 188 | } 189 | 190 | Eigen::VectorXf W(24); 191 | W = D / sumD; 192 | 193 | float score_r(0), score_g(0); 194 | for(int i=0; i score_g){ 203 | range_image_.at(id.y, id.x) = cv::Vec3b(0,0,255); 204 | }else{ 205 | range_image_.at(id.y, id.x) = cv::Vec3b(0,255,0); 206 | } 207 | 208 | } 209 | //cv::flip(range_image_,show_image, 0); 210 | //cv::imshow("Final image", show_image); 211 | //cv::waitKey(1); 212 | return show_image; 213 | } 214 | 215 | 216 | void JpcGroundRemove::GroundRemove(pcl::PointCloud& cloud_IN, 217 | pcl::PointCloud& cloud_gr, 218 | pcl::PointCloud& cloud_ob, 219 | cv::Mat& range_image){ 220 | for(auto p:cloud_IN.points){ 221 | cloud_->points.push_back(p); 222 | } 223 | printf("[INFO] Start segmentation!\n"); 224 | 225 | RangeProjection(); 226 | RECM(); 227 | range_image = JCP(); 228 | for(int i=0; i(j, i) == cv::Vec3b(0,255,0)){ 233 | cloud_gr.push_back(cloud_->points[index]); 234 | }else if(range_image_.at(j, i) == cv::Vec3b(0,0,255)){ 235 | cloud_ob.push_back(cloud_->points[index]); 236 | } 237 | } 238 | } 239 | } 240 | } 241 | -------------------------------------------------------------------------------- /jpc_seg/src/jpc_groundremove.h: -------------------------------------------------------------------------------- 1 | #ifndef JPC_GROUNDREMOVE_H 2 | #define JPC_GROUNDREMOVE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | //PCL 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include //自定义点云类型时要加 29 | 30 | //Eigen 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | using namespace std; 39 | 40 | struct PointXYZIR{ 41 | PCL_ADD_POINT4D; 42 | float intensity; 43 | uint16_t ring; 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | }EIGEN_ALIGN16; 46 | 47 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR, 48 | (float,x,x) 49 | (float,y,y) 50 | (float,z,z) 51 | (float,intensity,intensity) 52 | (uint16_t,ring,ring) 53 | ) 54 | 55 | struct Index{ 56 | int x = 0; 57 | int y = 0; 58 | }; 59 | 60 | 61 | class JpcGroundRemove{ 62 | 63 | public: 64 | JpcGroundRemove(); 65 | ~JpcGroundRemove(){} 66 | 67 | void RangeProjection(); 68 | void RECM(); 69 | cv::Mat JCP(); 70 | void GroundRemove(pcl::PointCloud& cloud_IN, 71 | pcl::PointCloud& cloud_gr, 72 | pcl::PointCloud& cloud_ob, 73 | cv::Mat& range_image); 74 | void calAngle(float x, float y, float &temp_tangle); 75 | void calRange(const PointXYZIR& p, float& range); 76 | void calRangeDiff(const PointXYZIR& p1, const PointXYZIR& p2, float& range); 77 | 78 | private: 79 | 80 | pcl::PointCloud::Ptr cloud_; 81 | float sensor_height = -1.73; 82 | 83 | int width_ = 2083; 84 | int height_ = 64; 85 | float max_range_ = 70.0; 86 | float min_range = 2.0; 87 | int length_ = 0; 88 | 89 | int neighborx_[24] = {-2, -1, 0, 1, 2,-2, -1, 0, 1, 2,-2,-1, 1, 2, -2,-1,0, 1, 2,-2,-1, 0, 1, 2}; 90 | int neighbory_[24] = {-2, -2, -2, -2, -2,-1, -1, -1, -1, -1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2}; 91 | 92 | vector region_minz_; 93 | //vector cloud_index_; 94 | vector cloud_index_; 95 | cv::Mat range_image_; 96 | cv::Mat region_; 97 | 98 | 99 | float th_g_ = 0.2; 100 | float sigma_ = 10.; 101 | float deltaR_ = 2.; 102 | float th_d_ = 1; 103 | 104 | }; 105 | 106 | #endif 107 | 108 | 109 | -------------------------------------------------------------------------------- /jpc_seg/src/kitti_file_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | //PCL 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include //自定义点云类型时要加 22 | #include 23 | 24 | //Eigen 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | 32 | 33 | //Boost 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | 42 | //ROS 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include "jpc_groundremove.h" 52 | 53 | using namespace std; 54 | typedef boost::tokenizer > tokenizer; 55 | 56 | 57 | template 58 | Type stringToNum(const string& str) 59 | { 60 | istringstream iss(str); 61 | Type num; 62 | iss >> num; 63 | return num; 64 | } 65 | 66 | template string toString(const T& t) { 67 | ostringstream oss; 68 | oss << t; 69 | return oss.str(); 70 | } 71 | 72 | 73 | 74 | int fileNameFilter(const struct dirent *cur) { 75 | std::string str(cur->d_name); 76 | if (str.find(".bin") != std::string::npos||str.find(".velodata") != std::string::npos 77 | || str.find(".pcd") != std::string::npos 78 | || str.find(".png") != std::string::npos 79 | || str.find(".jpg") != std::string::npos 80 | || str.find(".txt") != std::string::npos) { 81 | return 1; 82 | } 83 | return 0; 84 | } 85 | 86 | 87 | bool get_all_files(const std::string& dir_in, 88 | std::vector& files) { 89 | 90 | if (dir_in.empty()) { 91 | return false; 92 | } 93 | struct stat s; 94 | stat(dir_in.c_str(), &s); 95 | if (!S_ISDIR(s.st_mode)) { 96 | return false; 97 | } 98 | DIR* open_dir = opendir(dir_in.c_str()); 99 | if (NULL == open_dir) { 100 | std::exit(EXIT_FAILURE); 101 | } 102 | dirent* p = nullptr; 103 | while ((p = readdir(open_dir)) != nullptr) { 104 | struct stat st; 105 | if (p->d_name[0] != '.') { 106 | //因为是使用devC++ 获取windows下的文件，所以使用了 "\" ,linux下要换成"/" 107 | //cout<d_name)<d_name); 110 | stat(name.c_str(), &st); 111 | if (S_ISDIR(st.st_mode)) { 112 | get_all_files(name, files); 113 | } else if (S_ISREG(st.st_mode)) { 114 | boost::char_separator sepp { "." }; 115 | tokenizer tokn(std::string(p->d_name), sepp); 116 | vector filename_sep(tokn.begin(), tokn.end()); 117 | string type_ = "." + filename_sep[1]; 118 | break; 119 | } 120 | } 121 | } 122 | 123 | struct dirent **namelist; 124 | int n = scandir(dir_in.c_str(), &namelist, fileNameFilter, alphasort); 125 | if (n < 0) { 126 | return false; 127 | } 128 | for (int i = 0; i < n; ++i) { 129 | std::string filePath(namelist[i]->d_name); 130 | files.push_back(filePath); 131 | free(namelist[i]); 132 | }; 133 | free(namelist); 134 | closedir(open_dir); 135 | return true; 136 | } 137 | 138 | bool Load_Sensor_Data_Path(std::vector& lidarfile_name, string& path){ 139 | string lidar_file_path = path; 140 | cout< 0 && y >= 0) 153 | res = 1; 154 | else if (x <= 0 && y > 0) 155 | res = 2; 156 | else if (x < 0 && y <= 0) 157 | res = 3; 158 | else if (x >= 0 && y < 0) 159 | res = 4; 160 | return res; 161 | } 162 | 163 | 164 | 165 | void add_ring_info(pcl::PointCloud& input, pcl::PointCloud& output) 166 | { 167 | int previous_quadrant = 0; 168 | uint16_t ring_ = (uint16_t)64-1; 169 | for (pcl::PointCloud::iterator pt = input.points.begin(); pt < input.points.end()-1; ++pt){ 170 | int quadrant = get_quadrant(*pt); 171 | if (quadrant == 1 && previous_quadrant == 4 && ring_ > 0) 172 | ring_ -= 1; 173 | PointXYZIR point; 174 | point.x = pt->x; 175 | point.y = pt->y; 176 | point.z = pt->z; 177 | point.intensity = pt->intensity; 178 | point.ring = ring_; 179 | output.push_back(point); 180 | previous_quadrant = quadrant; 181 | } 182 | } 183 | 184 | 185 | 186 | bool LoadKittiPointcloud(pcl::PointCloud& cloud_IN, string path){ 187 | string lidar_filename_path = path; 188 | ifstream inputfile; 189 | inputfile.open(lidar_filename_path, ios::binary); 190 | if (!inputfile) { 191 | cerr << "ERROR: Cannot open file " << lidar_filename_path 192 | << "! Aborting..." << endl; 193 | return false; 194 | } 195 | 196 | inputfile.seekg(0, ios::beg); 197 | for (int i = 0; inputfile.good() && !inputfile.eof(); i++) { 198 | pcl::PointXYZI p; 199 | inputfile.read((char *) &p.x, 3 * sizeof(float)); 200 | inputfile.read((char *) &p.intensity, sizeof(float)); 201 | cloud_IN.points.push_back(p); 202 | } 203 | return true; 204 | } 205 | 206 | 207 | int main(int argc, char** argv){ 208 | 209 | ros::init(argc, argv, "lidar_read_node"); 210 | 211 | ros::NodeHandle nh; 212 | string datapath = "/media/wx/File/2011_09_30_drive_0027_sync/2011_09_30/2011_09_30_drive_0027_sync/velodyne_points/data"; 213 | 214 | ros::Publisher publidar = nh.advertise ("/pointcloud", 1); 215 | ros::Publisher pubgrlidar = nh.advertise ("/h_ground_point", 1); 216 | ros::Publisher puboblidar = nh.advertise ("/h_obstacle_point", 1); 217 | image_transport::ImageTransport it(nh); 218 | image_transport::Publisher pubimage = it.advertise("/seg_image", 1); 219 | 220 | vector lidarname; 221 | if(!Load_Sensor_Data_Path(lidarname, datapath)){ 222 | cout<<"Detecion file wrong!"< frame_num; 229 | boost::char_separator sep { " " }; 230 | /*if(!lidartype){ 231 | for(int i=0; i lines(tok_line.begin(), tok_line.end()); 234 | frame_num.push_back(stringToNum(lines[0])); 235 | } 236 | }*/ 237 | 238 | sort(frame_num.begin(), frame_num.end(), [](int a, int b){ return a::Ptr Cloud( 251 | new pcl::PointCloud); 252 | pcl::PointCloud::Ptr cloud_ring( 253 | new pcl::PointCloud); 254 | LoadKittiPointcloud(*Cloud, cloudpath); 255 | add_ring_info(*Cloud, *cloud_ring); 256 | pcl::toROSMsg(*cloud_ring, ros_cloud); 257 | ros_cloud.header.frame_id = "global_init_frame"; 258 | publidar.publish(ros_cloud); 259 | 260 | pcl::PointCloud::Ptr cloud_b( 261 | new pcl::PointCloud); 262 | pcl::PointCloud::Ptr cloud_g( 263 | new pcl::PointCloud); 264 | cv::Mat range_image; 265 | 266 | JpcGroundRemove groundremove; 267 | groundremove.GroundRemove(*cloud_ring, *cloud_g, *cloud_b, range_image); 268 | 269 | sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8",range_image).toImageMsg(); //转换为ros接受的消息 270 | pubimage.publish(imgmsg); 271 | 272 | sensor_msgs::PointCloud2 ros_cloud2; 273 | pcl::toROSMsg(*cloud_b, ros_cloud2); 274 | ros_cloud2.header.frame_id = "global_init_frame"; 275 | puboblidar.publish(ros_cloud2); 276 | 277 | sensor_msgs::PointCloud2 ros_cloud3; 278 | pcl::toROSMsg(*cloud_g, ros_cloud3); 279 | ros_cloud3.header.frame_id = "global_init_frame"; 280 | pubgrlidar.publish(ros_cloud3); 281 | frame++; 282 | } 283 | 284 | return 0; 285 | } 286 | -------------------------------------------------------------------------------- /jpc_seg/src/kitti_topic_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | //PCL 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include //自定义点云类型时要加 27 | 28 | //Eigen 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | //ROS 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | 46 | using namespace std; 47 | 48 | #include "jpc_groundremove.h" 49 | 50 | int get_quadrant(pcl::PointXYZI point) 51 | { 52 | int res = 0; 53 | float x = point.x; 54 | float y = point.y; 55 | if (x > 0 && y >= 0) 56 | res = 1; 57 | else if (x <= 0 && y > 0) 58 | res = 2; 59 | else if (x < 0 && y <= 0) 60 | res = 3; 61 | else if (x >= 0 && y < 0) 62 | res = 4; 63 | return res; 64 | } 65 | 66 | 67 | 68 | void add_ring_info(pcl::PointCloud& input, pcl::PointCloud& output) 69 | { 70 | int previous_quadrant = 0; 71 | uint16_t ring_ = (uint16_t)64-1; 72 | for (pcl::PointCloud::iterator pt = input.points.begin(); pt < input.points.end()-1; ++pt){ 73 | int quadrant = get_quadrant(*pt); 74 | if (quadrant == 1 && previous_quadrant == 4 && ring_ > 0) 75 | ring_ -= 1; 76 | PointXYZIR point; 77 | point.x = pt->x; 78 | point.y = pt->y; 79 | point.z = pt->z; 80 | point.intensity = pt->intensity; 81 | point.ring = ring_; 82 | output.push_back(point); 83 | previous_quadrant = quadrant; 84 | } 85 | } 86 | 87 | 88 | class SubscribeAndPublish { 89 | public: 90 | SubscribeAndPublish(ros::NodeHandle nh, std::string lidar_topic_name); 91 | 92 | 93 | ~SubscribeAndPublish(){ 94 | } 95 | 96 | void callback(const sensor_msgs::PointCloud2ConstPtr& cloudmsg) { 97 | 98 | pcl::PointCloud::Ptr cloud( 99 | new pcl::PointCloud); 100 | 101 | pcl::fromROSMsg(*cloudmsg, *cloud); 102 | 103 | pcl::PointCloud::Ptr cloud_ring(new pcl::PointCloud); 104 | add_ring_info(*cloud, *cloud_ring); 105 | 106 | ROS_INFO("Reciving data!"); 107 | 108 | pcl::PointCloud::Ptr cloud_gr(new pcl::PointCloud); 109 | pcl::PointCloud::Ptr cloud_ob(new pcl::PointCloud); 110 | cv::Mat range_image; 111 | 112 | JpcGroundRemove groundremove; 113 | groundremove.GroundRemove(*cloud_ring, *cloud_gr, *cloud_ob, range_image); 114 | ROS_INFO("segmentation"); 115 | 116 | sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8",range_image).toImageMsg(); 117 | pubimage_.publish(imgmsg); 118 | 119 | sensor_msgs::PointCloud2 ros_cloud2; 120 | pcl::toROSMsg(*cloud_gr, ros_cloud2); 121 | ros_cloud2.header.frame_id = "global_init_frame"; 122 | pub_.publish(ros_cloud2); 123 | 124 | sensor_msgs::PointCloud2 ros_cloud3; 125 | pcl::toROSMsg(*cloud_ob, ros_cloud3); 126 | ros_cloud3.header.frame_id = "global_init_frame"; 127 | pub2_.publish(ros_cloud3); 128 | 129 | } 130 | private: 131 | ros::NodeHandle n_; 132 | ros::Publisher pub_; 133 | ros::Publisher pub2_; 134 | ros::Subscriber sub_; 135 | image_transport::Publisher pubimage_; 136 | }; 137 | 138 | SubscribeAndPublish::SubscribeAndPublish(ros::NodeHandle nh, 139 | std::string lidar_topic_name) : 140 | n_(nh){ 141 | 142 | pub_ = nh.advertise < sensor_msgs::PointCloud2 > ("/ground_point", 1); 143 | pub2_ = nh.advertise < sensor_msgs::PointCloud2 > ("/obstacle_point", 1); 144 | sub_ = nh.subscribe(lidar_topic_name, 10, &SubscribeAndPublish::callback, this); 145 | image_transport::ImageTransport it(nh); 146 | pubimage_ = it.advertise("/seg_image", 1); 147 | } 148 | 149 | 150 | 151 | int main(int argc, char** argv){ 152 | 153 | ros::init(argc, argv, "jpc_seg_node"); 154 | SubscribeAndPublish SAPObject(ros::NodeHandle(), "pointcloud"); 155 | ROS_INFO("waiting for data!"); 156 | ros::spin(); 157 | return 0; 158 | } 159 | -------------------------------------------------------------------------------- /remotesensing-13-03239.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/remotesensing-13-03239.pdf -------------------------------------------------------------------------------- /test.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Fast-Ground-Segmentation-Based-on-JPC/0b4dc317db79197078e78a741b499c5042cdb7df/test.gif --------------------------------------------------------------------------------