00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2011 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob_environment_perception_intern 00012 * ROS package name: cob_3d_mapping_filters 00013 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Waqas Tanveer, email:waqas.informatik@googlemail.com 00018 * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de 00019 * 00020 * Date of creation: 05/2011 00021 00022 * Redistribution and use in source and binary forms, with or without 00023 * modification, are permitted provided that the following conditions are met: 00024 * 00025 * * Redistributions of source code must retain the above copyright 00026 * notice, this list of conditions and the following disclaimer. 00027 * * Redistributions in binary form must reproduce the above copyright 00028 * notice, this list of conditions and the following disclaimer in the 00029 * documentation and/or other materials provided with the distribution. 00030 * * Neither the name of the Fraunhofer Institute for Manufacturing 00031 * Engineering and Automation (IPA) nor the names of its 00032 * contributors may be used to endorse or promote products derived from 00033 * this software without specific prior written permission. 00034 * 00035 * This program is free software: you can redistribute it and/or modify 00036 * it under the terms of the GNU Lesser General Public License LGPL as 00037 * published by the Free Software Foundation, either version 3 of the 00038 * License, or (at your option) any later version. 00039 * 00040 * This program is distributed in the hope that it will be useful, 00041 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00042 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00043 * GNU Lesser General Public License LGPL for more details. 00044 * 00045 * You should have received a copy of the GNU Lesser General Public 00046 * License LGPL along with this program. 00047 * If not, see <http://www.gnu.org/licenses/>. 00048 * 00049 ****************************************************************/ 00050 #ifndef INTENSITY_FILTER_HPP_ 00051 #define INTENSITY_FILTER_HPP_ 00052 00053 //################## 00054 //#### includes #### 00055 00056 // cob_3d_mapping_filters includes 00057 #include "cob_3d_mapping_filters/smoothing_filter.h" 00058 00059 template<typename PointT> 00060 void 00061 cob_3d_mapping_filters::SmoothingFilter<PointT>::applyFilter (PointCloud &pc_out) 00062 { 00063 // set the parameters for output poincloud (pc_out) 00064 pc_out.points.resize (input_->points.size ()); 00065 pc_out.header = input_->header; 00066 00067 //resize pc_out according to filtered points 00068 pc_out.width = input_->width; 00069 pc_out.height = input_->height; 00070 pc_out.is_dense = input_->is_dense; 00071 00072 Eigen::Matrix3f F; 00073 F.fill (smoothing_factor_); 00074 F (1, 1) = 1; 00075 const float sum = F.sum (); 00076 00077 //Go through all points and discard points with intensity value above filter limit 00078 for (unsigned int x = 0; x < input_->width; x++) 00079 for (unsigned int y = 0; y < input_->height; y++) 00080 { 00081 float z = (*input_) (x, y).z; 00082 float nz = 0; 00083 for (int a = 0; a < 3; a++) 00084 for (int b = 0; b < 3; b++) 00085 if ((x + a - 1) < input_->width && (y + b - 1) < input_->height 00086 && std::abs ((*input_) (x + a - 1, y + b - 1).z - z) < edge_threshold_) 00087 nz += (*input_) (x + a - 1, y + b - 1).z * F (a, b); 00088 else 00089 nz += z * F (a, b); 00090 00091 pc_out (x, y) = (*input_) (x, y); 00092 pc_out (x, y).z = nz / sum; 00093 00094 if (last_pc_.width == input_->width && last_pc_.height == input_->height 00095 && std::abs (last_pc_ (x, y).z - pc_out (x, y).z) < edge_threshold_) 00096 pc_out (x, y).z = (1 - integral_factor_) * pc_out (x, y).z + integral_factor_ * last_pc_ (x, y).z; 00097 } 00098 00099 last_pc_ = pc_out; 00100 } 00101 00102 #define PCL_INSTANTIATE_SmoothingFilter(T) template class cob_3d_mapping_filters::SmoothingFilter<T>; 00103 #endif /* INTENSITY_FILTER_HPP_ */