38 #ifndef PCL_CUDA_KINECT_SMOOTHING_H_
39 #define PCL_CUDA_KINECT_SMOOTHING_H_
41 #include <pcl/io/openni_camera/openni_image.h>
42 #include <thrust/tuple.h>
43 #include <pcl/pcl_exports.h>
52 DisparityBoundSmoothing (
int width,
int height,
int window_size,
float focal_length,
float baseline,
float disparity_threshold,
float *data,
float *raw_data)
67 inline __host__ __device__
70 return baseline_ * focal_length_ / disparity;
74 inline __host__ __device__
77 return baseline_ * focal_length_ / depth;
81 inline __host__ __device__
89 if (avg_disparity > max_disparity)
91 if (avg_disparity < min_disparity)
98 inline __host__ __device__
101 float depth = data_ [idx];
103 if (depth == 0 | isnan(depth) | isinf(depth))
106 if (depth == 0 | pcl_isnan(depth) | pcl_isinf(depth))
113 int4 bounds = make_int4 (
121 bounds.x = clamp (bounds.x, 0, width_-1);
122 bounds.y = clamp (bounds.y, 0, width_-1);
123 bounds.z = clamp (bounds.z, 0, height_-1);
124 bounds.w = clamp (bounds.w, 0, height_-1);
126 float average_depth = depth;
129 for (
int y = bounds.z; y <= bounds.w; ++y)
131 for (
int x = bounds.x; x <= bounds.y; ++x)
134 int otherIdx = ((int)y) * width_ + ((int)x);
135 float otherDepth = data_[otherIdx];
140 if (fabs(otherDepth - depth) > 200)
144 average_depth += otherDepth;
166 template <
typename Tuple>
167 inline __host__ __device__
170 float depth = thrust::get<0> (t);
171 int idx = thrust::get<1> (t);
172 float3 dhel = disparity_helper_map_[idx];
173 int nr = (int) dhel.x;
174 float min_d = dhel.y;
175 float max_d = dhel.z;
177 if (depth == 0 | isnan(depth) | isinf(depth))
180 if (depth == 0 | pcl_isnan(depth) | pcl_isinf(depth))
187 int4 bounds = make_int4 (
195 bounds.x = clamp (bounds.x, 0, width_-1);
196 bounds.y = clamp (bounds.y, 0, width_-1);
197 bounds.z = clamp (bounds.z, 0, height_-1);
198 bounds.w = clamp (bounds.w, 0, height_-1);
201 for (
int y = bounds.z; y <= bounds.w; ++y)
203 for (
int x = bounds.x; x <= bounds.y; ++x)
206 int otherIdx = ((int)y) * width_ + ((int)x);
207 depth += data_[otherIdx];
211 return clamp (depth / nr, min_d, max_d);
217 DisparityHelperMap (
float* data,
int width,
int height,
int window_size,
float baseline,
float focal_length,
float disp_thresh)
230 inline __host__ __device__
233 return baseline_ * focal_length_ / disparity;
237 inline __host__ __device__
240 return baseline_ * focal_length_ / depth;
243 inline __host__ __device__
248 if (disparity == 0 | isnan(disparity) | isinf(disparity))
249 return make_float3 (0,0,0);
251 if (disparity == 0 | pcl_isnan(disparity) | pcl_isinf(disparity))
252 return make_float3 (0,0,0);
258 int4 bounds = make_int4 (
266 bounds.x = clamp (bounds.x, 0, width_-1);
267 bounds.y = clamp (bounds.y, 0, width_-1);
268 bounds.z = clamp (bounds.z, 0, height_-1);
269 bounds.w = clamp (bounds.w, 0, height_-1);
273 for (
int y = bounds.z; y <= bounds.w; ++y)
275 for (
int x = bounds.x; x <= bounds.y; ++x)
278 int otherIdx = ((int)y) * width_ + ((int)x);
279 float otherDepth = data_[otherIdx];
287 return make_float3 ((
float) counter,
__host__ __device__ float operator()(int idx)
This file defines compatibility wrappers for low level I/O functions.
__host__ __device__ float disparity2depth(float disparity)
__host__ __device__ float depth2disparity(float depth)
__host__ __device__ float depth2disparity(float depth)
DisparityClampedSmoothing(float *data, float3 *disparity_helper_map, int width, int height, int window_size)
__host__ __device__ float disparity2depth(float disparity)
float disparity_threshold_
__host__ __device__ float clampToDisparityBounds(float avg_depth, float depth)
__host__ __device__ float3 operator()(int idx)
DisparityHelperMap(float *data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
float3 * disparity_helper_map_
__host__ __device__ float operator()(Tuple t)
DisparityBoundSmoothing(int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)