$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #include <srs_env_model/but_server/plugins/octomap_plugin_tools/octomap_filter_ground.h> 00029 #include <pcl/sample_consensus/method_types.h> 00030 #include <pcl/sample_consensus/model_types.h> 00031 #include <pcl/segmentation/sac_segmentation.h> 00032 #include <pcl/filters/extract_indices.h> 00033 #include <pcl/filters/passthrough.h> 00034 00038 srs_env_model::COcFilterGround::COcFilterGround(const std::string & octree_frame_id, ERunMode mode /*= FILTER_ALLWAYS*/) 00039 : COcTreeFilterBase(octree_frame_id,mode), m_numSpecRemoved(0) 00040 , m_inputPc(0) 00041 , m_groundPc(new tPointCloud) 00042 , m_nongroundPc(new tPointCloud) 00043 , m_groundFilterDistance(0.04) 00044 , m_groundFilterAngle(0.15) 00045 , m_groundFilterPlaneDistance(0.07) 00046 { 00047 assert(m_groundPc != 0 && m_nongroundPc != 0); 00048 } 00049 00053 void srs_env_model::COcFilterGround::setCloud(const tPointCloud * cloud) 00054 { 00055 assert(cloud != 0); 00056 m_inputPc = cloud; 00057 } 00058 00062 void srs_env_model::COcFilterGround::init(ros::NodeHandle & node_handle) 00063 { 00064 // distance of points from plane for RANSAC 00065 node_handle.param("ocmap_ground_filter/distance", m_groundFilterDistance, 00066 m_groundFilterDistance); 00067 // angular derivation of found plane: 00068 node_handle.param("ocmap_ground_filter/angle", m_groundFilterAngle, 00069 m_groundFilterAngle); 00070 // distance of found plane from z=0 to be detected as ground (e.g. to exclude tables) 00071 node_handle.param("ocmap_ground_filter/plane_distance", 00072 m_groundFilterPlaneDistance, m_groundFilterPlaneDistance); 00073 } 00074 00078 void srs_env_model::COcFilterGround::filterInternal( tButServerOcTree & tree ) 00079 { 00080 m_groundPc->header = m_inputPc->header; 00081 m_groundPc->header = m_inputPc->header; 00082 00083 if (m_inputPc->size() < 50) 00084 { 00085 ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction"); 00086 *m_nongroundPc = *m_inputPc; 00087 } 00088 else 00089 { 00090 // plane detection for ground plane removal: 00091 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); 00092 pcl::PointIndices::Ptr inliers(new pcl::PointIndices); 00093 00094 // Create the segmentation object and set up: 00095 pcl::SACSegmentation<tPclPoint> seg; 00096 seg.setOptimizeCoefficients(true); 00097 // TODO: maybe a filtering based on the surface normals might be more robust / accurate? 00098 seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); 00099 seg.setMethodType(pcl::SAC_RANSAC); 00100 seg.setMaxIterations(200); 00101 seg.setDistanceThreshold(m_groundFilterDistance); 00102 seg.setAxis(Eigen::Vector3f(0, 0, 1)); 00103 seg.setEpsAngle(m_groundFilterAngle); 00104 00105 tPointCloud cloud_filtered(*m_inputPc); 00106 // Create the filtering object 00107 pcl::ExtractIndices<tPclPoint> extract; 00108 bool groundPlaneFound = false; 00109 00110 while (cloud_filtered.size() > 10 && !groundPlaneFound) { 00111 seg.setInputCloud(cloud_filtered.makeShared()); 00112 seg.segment(*inliers, *coefficients); 00113 if (inliers->indices.size() == 0) { 00114 ROS_WARN("No plane found in cloud."); 00115 00116 break; 00117 } 00118 00119 extract.setInputCloud(cloud_filtered.makeShared()); 00120 extract.setIndices(inliers); 00121 00122 if (std::abs(coefficients->values.at(3)) 00123 < m_groundFilterPlaneDistance) { 00124 ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), 00125 cloud_filtered.size(), coefficients->values.at(0), coefficients->values.at(1), 00126 coefficients->values.at(2), coefficients->values.at(3)); 00127 extract.setNegative(false); 00128 extract.filter(*m_groundPc); 00129 00130 // remove ground points from full pointcloud: 00131 // workaround for PCL bug: 00132 if (inliers->indices.size() != cloud_filtered.size()) { 00133 extract.setNegative(true); 00134 tPointCloud cloud_out; 00135 extract.filter(cloud_out); 00136 *m_nongroundPc += cloud_out; 00137 cloud_filtered = cloud_out; 00138 } 00139 00140 groundPlaneFound = true; 00141 } else { 00142 ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), 00143 cloud_filtered.size(), coefficients->values.at(0), coefficients->values.at(1), 00144 coefficients->values.at(2), coefficients->values.at(3)); 00145 tPointCloud cloud_out; 00146 extract.setNegative(false); 00147 extract.filter(cloud_out); 00148 *m_nongroundPc += cloud_out; 00149 // debug 00150 // pcl::PCDWriter writer; 00151 // writer.write<tPclPoint>("nonground_plane.pcd",cloud_out, false); 00152 00153 // remove current plane from scan for next iteration: 00154 // workaround for PCL bug: 00155 if (inliers->indices.size() != cloud_filtered.size()) { 00156 extract.setNegative(true); 00157 cloud_out.points.clear(); 00158 extract.filter(cloud_out); 00159 cloud_filtered = cloud_out; 00160 } else { 00161 cloud_filtered.points.clear(); 00162 } 00163 } 00164 00165 } 00166 // TODO: also do this if overall starting pointcloud too small? 00167 if (!groundPlaneFound) { // no plane found or remaining points too small 00168 ROS_WARN("No ground plane found in scan"); 00169 00170 // do a rough filtering on height to prevent spurious obstacles 00171 pcl::PassThrough<tPclPoint> second_pass; 00172 second_pass.setFilterFieldName("z"); 00173 second_pass.setFilterLimits(-m_groundFilterPlaneDistance, 00174 m_groundFilterPlaneDistance); 00175 second_pass.setInputCloud(m_inputPc->makeShared()); 00176 second_pass.filter(*m_groundPc); 00177 00178 second_pass.setFilterLimitsNegative(true); 00179 second_pass.filter(*m_nongroundPc); 00180 } 00181 00182 // debug: 00183 // pcl::PCDWriter writer; 00184 // if (pc_ground.size() > 0) 00185 // writer.write<tPclPoint>("ground.pcd",pc_ground, false); 00186 // if (pc_nonground.size() > 0) 00187 // writer.write<tPclPoint>("nonground.pcd",pc_nonground, false); 00188 00189 } 00190 }