42 #include <pcl/filters/median_filter.h>
43 #include <pcl/common/io.h>
44 #include <pcl/common/point_tests.h>
46 template <
typename Po
intT>
void
49 if (!input_->isOrganized ())
51 PCL_ERROR (
"[pcl::MedianFilter] Input cloud needs to be organized\n");
58 int height =
static_cast<int> (output.
height);
59 int width =
static_cast<int> (output.
width);
60 for (
int y = 0; y < height; ++y)
61 for (
int x = 0; x < width; ++x)
64 std::vector<float> vals;
65 vals.reserve (window_size_ * window_size_);
67 for (
int y_dev = -window_size_/2; y_dev <= window_size_/2; ++y_dev)
68 for (
int x_dev = -window_size_/2; x_dev <= window_size_/2; ++x_dev)
70 if (x + x_dev >= 0 && x + x_dev < width &&
71 y + y_dev >= 0 && y + y_dev < height &&
73 vals.push_back ((*input_)(x+x_dev, y+y_dev).z);
80 partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ());
81 float new_depth = vals[vals.size () / 2];
83 if (std::abs (new_depth - (*input_)(x, y).z) < max_allowed_movement_)
84 output (x, y).z = new_depth;
86 output (x, y).z = (*input_)(x, y).z +
87 max_allowed_movement_ * (new_depth - (*input_)(x, y).z) / std::abs (new_depth - (*input_)(x, y).z);