Point Cloud Library (PCL)  1.9.1
kinect_smoothing.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_CUDA_KINECT_SMOOTHING_H_
39 #define PCL_CUDA_KINECT_SMOOTHING_H_
40 
41 #include <pcl/io/openni_camera/openni_image.h>
42 #include <thrust/tuple.h>
43 #include <pcl/pcl_exports.h>
44 
45 namespace pcl
46 {
47  namespace cuda
48  {
49 
51  {
52  DisparityBoundSmoothing (int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)
53  : width_ (width), height_ (height), window_size_ (window_size)
54  , focal_length_(focal_length), baseline_(baseline), disparity_threshold_(disparity_threshold)
55  , data_(data), raw_data_(raw_data)
56  {}
57 
61  float baseline_;
63  float *data_;
64  float *raw_data_;
65 
66  // helper function
67  inline __host__ __device__
68  float disparity2depth (float disparity)
69  {
70  return baseline_ * focal_length_ / disparity;
71  }
72 
73  // helper function
74  inline __host__ __device__
75  float depth2disparity (float depth)
76  {
77  return baseline_ * focal_length_ / depth;
78  }
79 
80  // clampToDisparityBounds
81  inline __host__ __device__
82  float clampToDisparityBounds (float avg_depth, float depth)
83  {
84  float disparity = depth2disparity (depth);
85  float avg_disparity = depth2disparity (avg_depth);
86  float min_disparity = disparity - disparity_threshold_;
87  float max_disparity = disparity + disparity_threshold_;
88 
89  if (avg_disparity > max_disparity)
90  return disparity2depth (max_disparity);
91  if (avg_disparity < min_disparity)
92  return disparity2depth (min_disparity);
93 
94  return avg_depth;
95  }
96 
97  // actual kernel operator
98  inline __host__ __device__
99  float operator () (int idx)
100  {
101  float depth = data_ [idx];
102 #ifdef __CUDACC__
103  if (depth == 0 | isnan(depth) | isinf(depth))
104  return 0;
105 #else
106  if (depth == 0 | pcl_isnan(depth) | pcl_isinf(depth))
107  return 0;
108 #endif
109  int xIdx = idx % width_;
110  int yIdx = idx / width_;
111  // TODO: test median
112  // This implements a fixed window size in image coordinates (pixels)
113  int4 bounds = make_int4 (
114  xIdx - window_size_,
115  xIdx + window_size_,
116  yIdx - window_size_,
117  yIdx + window_size_
118  );
119 
120  // clamp the coordinates to fit to depth image size
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);
125 
126  float average_depth = depth;
127  int counter = 1;
128  // iterate over all pixels in the rectangular region
129  for (int y = bounds.z; y <= bounds.w; ++y)
130  {
131  for (int x = bounds.x; x <= bounds.y; ++x)
132  {
133  // find index in point cloud from x,y pixel positions
134  int otherIdx = ((int)y) * width_ + ((int)x);
135  float otherDepth = data_[otherIdx];
136 
137  // ignore invalid points
138  if (otherDepth == 0)
139  continue;
140  if (fabs(otherDepth - depth) > 200)
141  continue;
142 
143  ++counter;
144  average_depth += otherDepth;
145  }
146  }
147 
148  return clampToDisparityBounds (average_depth / counter, raw_data_[idx]);
149  }
150  };
151 
152 
153  // This version requires a pre-computed map of float3 (nr_valid_points, min_allowable_depth, max_allowable_depth);
155  {
156  DisparityClampedSmoothing (float* data, float3* disparity_helper_map, int width, int height, int window_size)
157  : data_(data), disparity_helper_map_(disparity_helper_map), width_(width), height_(height), window_size_(window_size)
158  {}
159 
160  float* data_;
162  int width_;
163  int height_;
165 
166  template <typename Tuple>
167  inline __host__ __device__
168  float operator () (Tuple t)
169  {
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;
176 #ifdef __CUDACC__
177  if (depth == 0 | isnan(depth) | isinf(depth))
178  return 0.0f;
179 #else
180  if (depth == 0 | pcl_isnan(depth) | pcl_isinf(depth))
181  return 0.0f;
182 #endif
183  int xIdx = idx % width_;
184  int yIdx = idx / width_;
185 
186  // This implements a fixed window size in image coordinates (pixels)
187  int4 bounds = make_int4 (
188  xIdx - window_size_,
189  xIdx + window_size_,
190  yIdx - window_size_,
191  yIdx + window_size_
192  );
193 
194  // clamp the coordinates to fit to disparity image size
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);
199 
200  // iterate over all pixels in the rectangular region
201  for (int y = bounds.z; y <= bounds.w; ++y)
202  {
203  for (int x = bounds.x; x <= bounds.y; ++x)
204  {
205  // find index in point cloud from x,y pixel positions
206  int otherIdx = ((int)y) * width_ + ((int)x);
207  depth += data_[otherIdx];
208  }
209  }
210 
211  return clamp (depth / nr, min_d, max_d);
212  }
213  };
214 
216  {
217  DisparityHelperMap (float* data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
218  : data_(data), width_(width), height_(height), window_size_(window_size), baseline_(baseline), focal_length_(focal_length), disp_thresh_(disp_thresh)
219  {}
220 
221  float* data_;
222  int width_;
223  int height_;
225  float baseline_;
228 
229  // helper function
230  inline __host__ __device__
231  float disparity2depth (float disparity)
232  {
233  return baseline_ * focal_length_ / disparity;
234  }
235 
236  // helper function
237  inline __host__ __device__
238  float depth2disparity (float depth)
239  {
240  return baseline_ * focal_length_ / depth;
241  }
242 
243  inline __host__ __device__
244  float3 operator () (int idx)
245  {
246  float disparity = depth2disparity (data_ [idx]);
247 #ifdef __CUDACC__
248  if (disparity == 0 | isnan(disparity) | isinf(disparity))
249  return make_float3 (0,0,0);
250 #else
251  if (disparity == 0 | pcl_isnan(disparity) | pcl_isinf(disparity))
252  return make_float3 (0,0,0);
253 #endif
254  int xIdx = idx % width_;
255  int yIdx = idx / width_;
256 
257  // This implements a fixed window size in image coordinates (pixels)
258  int4 bounds = make_int4 (
259  xIdx - window_size_,
260  xIdx + window_size_,
261  yIdx - window_size_,
262  yIdx + window_size_
263  );
264 
265  // clamp the coordinates to fit to disparity image size
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);
270 
271  int counter = 1;
272  // iterate over all pixels in the rectangular region
273  for (int y = bounds.z; y <= bounds.w; ++y)
274  {
275  for (int x = bounds.x; x <= bounds.y; ++x)
276  {
277  // find index in point cloud from x,y pixel positions
278  int otherIdx = ((int)y) * width_ + ((int)x);
279  float otherDepth = data_[otherIdx];
280 
281  // ignore invalid points
282  if (otherDepth > 0)
283  ++counter;
284  }
285  }
286 
287  return make_float3 ((float) counter,
288  disparity2depth (disparity + disp_thresh_),
289  disparity2depth (disparity - disp_thresh_));
290  }
291  };
292 
293 
294  } // namespace
295 } // namespace
296 
297 #endif
298 
__host__ __device__ float operator()(int idx)
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
__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)
__host__ __device__ float clampToDisparityBounds(float avg_depth, float depth)
DisparityHelperMap(float *data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
DisparityBoundSmoothing(int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)