40 #ifndef PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_
41 #define PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_
43 #include <pcl/filters/median_filter.h>
44 #include <pcl/common/io.h>
46 template <
typename Po
intT>
void
49 if (!input_->isOrganized ())
51 PCL_ERROR (
"[pcl::MedianFilter] Input cloud needs to be organized\n");
58 for (
int y = 0; y < output.
height; ++y)
59 for (
int x = 0; x < output.
width; ++x)
62 std::vector<float> vals;
63 vals.reserve (window_size_ * window_size_);
65 for (
int y_dev = -window_size_/2; y_dev <= window_size_/2; ++y_dev)
66 for (
int x_dev = -window_size_/2; x_dev <= window_size_/2; ++x_dev)
68 if (x + x_dev >= 0 && x + x_dev < output.
width &&
69 y + y_dev >= 0 && y + y_dev < output.
height &&
71 vals.push_back ((*input_)(x+x_dev, y+y_dev).z);
74 if (vals.size () == 0)
78 partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ());
79 float new_depth = vals[vals.size () / 2];
81 if (fabs (new_depth - (*input_)(x, y).z) < max_allowed_movement_)
82 output (x, y).z = new_depth;
84 output (x, y).z = (*input_)(x, y).z +
85 max_allowed_movement_ * (new_depth - (*input_)(x, y).z) / fabsf (new_depth - (*input_)(x, y).z);