00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 00007 * All rights reserved. 00008 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ 00041 #define PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ 00042 00043 #include <pcl/filters/median_filter.h> 00044 #include <pcl/common/io.h> 00045 00046 template <typename PointT> void 00047 pcl::MedianFilter<PointT>::applyFilter (PointCloud &output) 00048 { 00049 if (!input_->isOrganized ()) 00050 { 00051 PCL_ERROR ("[pcl::MedianFilter] Input cloud needs to be organized\n"); 00052 return; 00053 } 00054 00055 // Copy everything from the input cloud to the output cloud (takes care of all the fields) 00056 copyPointCloud (*input_, output); 00057 00058 for (int y = 0; y < output.height; ++y) 00059 for (int x = 0; x < output.width; ++x) 00060 if (pcl::isFinite ((*input_)(x, y))) 00061 { 00062 std::vector<float> vals; 00063 vals.reserve (window_size_ * window_size_); 00064 // Fill in the vector of values with the depths around the interest point 00065 for (int y_dev = -window_size_/2; y_dev <= window_size_/2; ++y_dev) 00066 for (int x_dev = -window_size_/2; x_dev <= window_size_/2; ++x_dev) 00067 { 00068 if (x + x_dev >= 0 && x + x_dev < output.width && 00069 y + y_dev >= 0 && y + y_dev < output.height && 00070 pcl::isFinite ((*input_)(x+x_dev, y+y_dev))) 00071 vals.push_back ((*input_)(x+x_dev, y+y_dev).z); 00072 } 00073 00074 if (vals.size () == 0) 00075 continue; 00076 00077 // The output depth will be the median of all the depths in the window 00078 partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ()); 00079 float new_depth = vals[vals.size () / 2]; 00080 // Do not allow points to move more than the set max_allowed_movement_ 00081 if (fabs (new_depth - (*input_)(x, y).z) < max_allowed_movement_) 00082 output (x, y).z = new_depth; 00083 else 00084 output (x, y).z = (*input_)(x, y).z + 00085 max_allowed_movement_ * (new_depth - (*input_)(x, y).z) / fabsf (new_depth - (*input_)(x, y).z); 00086 } 00087 } 00088 00089 00090 #endif /* PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ */