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);
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...