├── CMakeLists.txt ├── LICENSE ├── README.md ├── images ├── ground_filter.png └── table_filter.png ├── pcl_test.cpp ├── pointcloud_files ├── 000000.pcd └── table_scene_lms400.pcd ├── range_image.cpp └── voxel_grid.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project(pcl_test) 3 | project(voxel_grid) 4 | project(range_image) 5 | 6 | find_package(PCL 1.2 REQUIRED) 7 | 8 | include_directories(${PCL_INCLUDE_DIRS}) 9 | link_directories(${PCL_LIBRARY_DIRS}) 10 | add_definitions(${PCL_DEFINITIONS}) 11 | add_executable(pcl_test pcl_test.cpp) 12 | add_executable(range_image range_image.cpp) 13 | 14 | target_link_libraries (pcl_test ${PCL_LIBRARIES}) 15 | add_executable (voxel_grid voxel_grid.cpp) 16 | target_link_libraries (voxel_grid ${PCL_LIBRARIES}) 17 | SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}) 18 | target_link_libraries (range_image ${PCL_LIBRARIES}) 19 | SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}) 20 | 21 | install(TARGETS pcl_test RUNTIME DESTINATION bin) 22 | install(TARGETS voxel_grid RUNTIME DESTINATION bin) 23 | install(TARGETS range_image RUNTIME DESTINATION bin) 24 | -------------------------------------------------------------------------------- /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 | ## enviroment 2 | ``` 3 | ubuntu 16.04 4 | pcl 1.7 5 | ``` 6 | ## usage 7 | ``` 8 | mkdir build 9 | cd build 10 | cmake .. 11 | make 12 | cd .. 13 | ./pcl_test 14 | ``` 15 | You can use `pcl_viewer` tool to visualize the result: `pcl_viewer pointcloud_files/000000_filtered.pcd`. 16 | 17 | Comparison before & after filtering ground(point cloud file is from kitti dataset): 18 | ![](images/ground_filter.png) 19 | 20 | use `./voxel_gird`, and you can have: 21 | ![](images/table_filter.png) -------------------------------------------------------------------------------- /images/ground_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Qjizhi/pcl_tutorials/48f9926cdc1211503d9a4dcebba789640765320e/images/ground_filter.png -------------------------------------------------------------------------------- /images/table_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Qjizhi/pcl_tutorials/48f9926cdc1211503d9a4dcebba789640765320e/images/table_filter.png -------------------------------------------------------------------------------- /pcl_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | void detectObjectsOnCloud(pcl::PointCloud::Ptr &cloud, pcl::PointCloud::Ptr &cloud_filtered) 11 | { 12 | if (cloud->size() > 0) 13 | { 14 | pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); 15 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices); 16 | pcl::SACSegmentation seg; 17 | seg.setOptimizeCoefficients(true); 18 | seg.setModelType(pcl::SACMODEL_PLANE); 19 | seg.setMethodType(pcl::SAC_RANSAC); 20 | // you can modify the parameter below 21 | seg.setMaxIterations(10000); 22 | seg.setDistanceThreshold(0.15); 23 | seg.setInputCloud(cloud); 24 | seg.segment(*inliers, *coefficients); 25 | if (inliers->indices.size() == 0) 26 | { 27 | cout<<"error! Could not found any inliers!"< extractor; 31 | extractor.setInputCloud(cloud); 32 | extractor.setIndices(inliers); 33 | extractor.setNegative(true); 34 | extractor.filter(*cloud_filtered); 35 | // vise-versa, remove the ground not just extract the ground 36 | // just setNegative to be true 37 | cout << "filter done."<::Ptr cloud (new pcl::PointCloud); 48 | pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); 49 | 50 | // Fill in the cloud data 51 | pcl::PCDReader reader; 52 | // Replace the path below with the path where you saved your file 53 | reader.read ("pointcloud_files/000000.pcd", *cloud); 54 | detectObjectsOnCloud(cloud, cloud_filtered); 55 | pcl::PCDWriter writer; 56 | writer.write ("pointcloud_files/000000_filtered.pcd", *cloud_filtered, false); 57 | 58 | return (0); 59 | } 60 | -------------------------------------------------------------------------------- /pointcloud_files/000000.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Qjizhi/pcl_tutorials/48f9926cdc1211503d9a4dcebba789640765320e/pointcloud_files/000000.pcd -------------------------------------------------------------------------------- /pointcloud_files/table_scene_lms400.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Qjizhi/pcl_tutorials/48f9926cdc1211503d9a4dcebba789640765320e/pointcloud_files/table_scene_lms400.pcd -------------------------------------------------------------------------------- /range_image.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | typedef pcl::PointXYZ PointType; 11 | 12 | // -------------------- 13 | // -----Parameters----- 14 | // -------------------- 15 | float angular_resolution_x = 0.5f, 16 | angular_resolution_y = angular_resolution_x; 17 | pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; 18 | bool live_update = false; 19 | 20 | // -------------- 21 | // -----Help----- 22 | // -------------- 23 | void 24 | printUsage (const char* progName) 25 | { 26 | std::cout << "\n\nUsage: "<\n\n" 27 | << "Options:\n" 28 | << "-------------------------------------------\n" 29 | << "-rx angular resolution in degrees (default "< angular resolution in degrees (default "< coordinate frame (default "<< (int)coordinate_frame<<")\n" 32 | << "-l live update - update the range image according to the selected view in the 3D viewer.\n" 33 | << "-h this help\n" 34 | << "\n\n"; 35 | } 36 | 37 | void 38 | setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose) 39 | { 40 | Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0); 41 | Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector; 42 | Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0); 43 | viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2], 44 | look_at_vector[0], look_at_vector[1], look_at_vector[2], 45 | up_vector[0], up_vector[1], up_vector[2]); 46 | } 47 | 48 | // -------------- 49 | // -----Main----- 50 | // -------------- 51 | int 52 | main (int argc, char** argv) 53 | { 54 | // -------------------------------------- 55 | // -----Parse Command Line Arguments----- 56 | // -------------------------------------- 57 | if (pcl::console::find_argument (argc, argv, "-h") >= 0) 58 | { 59 | printUsage (argv[0]); 60 | return 0; 61 | } 62 | if (pcl::console::find_argument (argc, argv, "-l") >= 0) 63 | { 64 | live_update = true; 65 | std::cout << "Live update is on.\n"; 66 | } 67 | if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0) 68 | std::cout << "Setting angular resolution in x-direction to "<= 0) 70 | std::cout << "Setting angular resolution in y-direction to "<= 0) 73 | { 74 | coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame); 75 | std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n"; 76 | } 77 | angular_resolution_x = pcl::deg2rad (angular_resolution_x); 78 | angular_resolution_y = pcl::deg2rad (angular_resolution_y); 79 | 80 | // ------------------------------------------------------------------ 81 | // -----Read pcd file or create example point cloud if not given----- 82 | // ------------------------------------------------------------------ 83 | pcl::PointCloud::Ptr point_cloud_ptr (new pcl::PointCloud); 84 | pcl::PointCloud& point_cloud = *point_cloud_ptr; 85 | Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ()); 86 | std::vector pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd"); 87 | if (!pcd_filename_indices.empty ()) 88 | { 89 | std::string filename = argv[pcd_filename_indices[0]]; 90 | if (pcl::io::loadPCDFile (filename, point_cloud) == -1) 91 | { 92 | std::cout << "Was not able to open file \""< Generating example point cloud.\n\n"; 104 | for (float x=-0.5f; x<=0.5f; x+=0.01f) 105 | { 106 | for (float y=-0.5f; y<=0.5f; y+=0.01f) 107 | { 108 | PointType point; point.x = x; point.y = y; point.z = 2.0f - y; 109 | point_cloud.push_back (point); 110 | } 111 | } 112 | point_cloud.width = point_cloud.size (); point_cloud.height = 1; 113 | } 114 | 115 | // ----------------------------------------------- 116 | // -----Create RangeImage from the PointCloud----- 117 | // ----------------------------------------------- 118 | float noise_level = 0.0; 119 | float min_range = 0.0f; 120 | int border_size = 1; 121 | pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage); 122 | pcl::RangeImage& range_image = *range_image_ptr; 123 | range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, 124 | pcl::deg2rad (360.0f), pcl::deg2rad (180.0f), 125 | scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size); 126 | 127 | // -------------------------------------------- 128 | // -----Open 3D viewer and add point cloud----- 129 | // -------------------------------------------- 130 | pcl::visualization::PCLVisualizer viewer ("3D Viewer"); 131 | viewer.setBackgroundColor (1, 1, 1); 132 | pcl::visualization::PointCloudColorHandlerCustom range_image_color_handler (range_image_ptr, 0, 0, 0); 133 | viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image"); 134 | viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image"); 135 | //viewer.addCoordinateSystem (1.0f, "global"); 136 | //PointCloudColorHandlerCustom point_cloud_color_handler (point_cloud_ptr, 150, 150, 150); 137 | //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud"); 138 | viewer.initCameraParameters (); 139 | setViewerPose(viewer, range_image.getTransformationToWorldSystem ()); 140 | 141 | // -------------------------- 142 | // -----Show range image----- 143 | // -------------------------- 144 | pcl::visualization::RangeImageVisualizer range_image_widget ("Range image"); 145 | range_image_widget.showRangeImage (range_image); 146 | 147 | //-------------------- 148 | // -----Main loop----- 149 | //-------------------- 150 | while (!viewer.wasStopped ()) 151 | { 152 | range_image_widget.spinOnce (); 153 | viewer.spinOnce (); 154 | pcl_sleep (0.01); 155 | 156 | if (live_update) 157 | { 158 | scene_sensor_pose = viewer.getViewerPose(); 159 | range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, 160 | pcl::deg2rad (360.0f), pcl::deg2rad (180.0f), 161 | scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size); 162 | range_image_widget.showRangeImage (range_image); 163 | } 164 | } 165 | } -------------------------------------------------------------------------------- /voxel_grid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include"time.h" 3 | #include 4 | #include 5 | #include 6 | 7 | int main (int argc, char** argv) 8 | { 9 | pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ()); 10 | pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); 11 | 12 | // Fill in the cloud data 13 | pcl::PCDReader reader; 14 | // Replace the path below with the path where you saved your file 15 | reader.read ("pointcloud_files/table_scene_lms400.pcd", *cloud); 16 | 17 | std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 18 | << " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl; 19 | 20 | clock_t start, finish; 21 | 22 | start =clock(); 23 | 24 | // Create the filtering object 25 | pcl::VoxelGrid sor; 26 | sor.setInputCloud (cloud); 27 | sor.setLeafSize (0.01f, 0.01f, 0.01f); 28 | sor.filter (*cloud_filtered); 29 | 30 | finish = clock(); 31 | 32 | printf("%f seconds\n",(double)(finish-start)/CLOCKS_PER_SEC); 33 | 34 | std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 35 | << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl; 36 | 37 | pcl::PCDWriter writer; 38 | writer.write ("pointcloud_files/table_scene_lms400_downsampled.pcd", *cloud_filtered, 39 | Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false); 40 | 41 | return (0); 42 | } 43 | --------------------------------------------------------------------------------