40 #ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ 41 #define PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ 43 #include <pcl/filters/fast_bilateral_omp.h> 44 #include <pcl/common/io.h> 45 #include <pcl/console/time.h> 49 template <
typename Po
intT>
void 54 threads_ = omp_get_num_procs();
59 threads_ = nr_threads;
63 template <
typename Po
intT>
void 66 if (!input_->isOrganized ())
68 PCL_ERROR (
"[pcl::FastBilateralFilterOMP] Input cloud needs to be organized.\n");
73 float base_max = -std::numeric_limits<float>::max (),
74 base_min = std::numeric_limits<float>::max ();
75 bool found_finite =
false;
76 for (
size_t x = 0; x < output.
width; ++x)
78 for (
size_t y = 0; y < output.
height; ++y)
80 if (pcl_isfinite (output (x, y).z))
82 if (base_max < output (x, y).z)
83 base_max = output (x, y).z;
84 if (base_min > output (x, y).z)
85 base_min = output (x, y).z;
92 PCL_WARN (
"[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n");
96 #pragma omp parallel for num_threads (threads_) 98 for (
long int i = 0; i < static_cast<long int> (output.
size ()); ++i)
99 if (!pcl_isfinite (output.
at(i).z))
100 output.
at(i).z = base_max;
102 const float base_delta = base_max - base_min;
104 const size_t padding_xy = 2;
105 const size_t padding_z = 2;
107 const size_t small_width =
static_cast<size_t> (
static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy;
108 const size_t small_height =
static_cast<size_t> (
static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
109 const size_t small_depth =
static_cast<size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z;
111 Array3D data (small_width, small_height, small_depth);
113 #pragma omp parallel for num_threads (threads_) 115 for (
long int i = 0; i < static_cast<long int> (small_width * small_height); ++i)
117 size_t small_x =
static_cast<size_t> (i % small_width);
118 size_t small_y =
static_cast<size_t> (i / small_width);
119 size_t start_x =
static_cast<size_t>(
120 std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
121 size_t end_x =
static_cast<size_t>(
122 std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
123 size_t start_y =
static_cast<size_t>(
124 std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
125 size_t end_y =
static_cast<size_t>(
126 std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
127 for (
size_t x = start_x; x < end_x && x < input_->width; ++x)
129 for (
size_t y = start_y; y < end_y && y < input_->height; ++y)
131 const float z = output (x,y).z - base_min;
132 const size_t small_z =
static_cast<size_t> (
static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
133 Eigen::Vector2f& d = data (small_x, small_y, small_z);
134 d[0] += output (x,y).z;
140 std::vector<long int> offset (3);
141 offset[0] = &(data (1,0,0)) - &(data (0,0,0));
142 offset[1] = &(data (0,1,0)) - &(data (0,0,0));
143 offset[2] = &(data (0,0,1)) - &(data (0,0,0));
145 Array3D buffer (small_width, small_height, small_depth);
147 for (
size_t dim = 0; dim < 3; ++dim)
149 for (
size_t n_iter = 0; n_iter < 2; ++n_iter)
151 Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data);
152 Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer);
154 #pragma omp parallel for num_threads (threads_) 156 for(
long int i = 0; i < static_cast<long int> ((small_width - 2)*(small_height - 2)); ++i)
158 size_t x =
static_cast<size_t> (i % (small_width - 2) + 1);
159 size_t y =
static_cast<size_t> (i / (small_width - 2) + 1);
160 const long int off = offset[dim];
161 Eigen::Vector2f* d_ptr = &(current_data->operator() (x,y,1));
162 Eigen::Vector2f* b_ptr = &(current_buffer->operator() (x,y,1));
164 for(
size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
165 *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
175 for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.
begin (); d != data.
end (); ++d)
176 *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
179 #pragma omp parallel for num_threads (threads_) 181 for (
long int i = 0; i < static_cast<long int> (input_->size ()); ++i)
183 size_t x =
static_cast<size_t> (i % input_->width);
184 size_t y =
static_cast<size_t> (i / input_->width);
185 const float z = output (x,y).z - base_min;
187 static_cast<float> (y) / sigma_s_ + padding_xy,
188 z / sigma_r_ + padding_z);
189 output(x,y).z = D[0];
195 #pragma omp parallel for num_threads (threads_) 197 for (
long i = 0; i < static_cast<long int> (input_->size ()); ++i)
199 size_t x =
static_cast<size_t> (i % input_->width);
200 size_t y =
static_cast<size_t> (i / input_->width);
201 const float z = output (x,y).z - base_min;
203 static_cast<float> (y) / sigma_s_ + padding_xy,
204 z / sigma_r_ + padding_z);
205 output (x,y).z = D[0] / D[1];
Eigen::Vector2f trilinear_interpolation(const float x, const float y, const float z)
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > >::iterator end()
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
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.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
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. ...
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > >::iterator begin()
void applyFilter(PointCloud &output)
Filter the input data and store the results into output.