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 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);
76 if (vals.size () == 0)
80 partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ());
81 float new_depth = vals[vals.size () / 2];
83 if (fabs (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) / fabsf (new_depth - (*input_)(x, y).z);
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
uint32_t height
The point cloud height (if organized as an image-structure).
uint32_t width
The point cloud width (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...