Point Cloud Library (PCL)  1.14.1-dev
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 #pragma once
39 
40 #include <pcl/io/openni_camera/openni_image.h>
41 #include <thrust/tuple.h>
42 #include <pcl/pcl_exports.h>
43 
44 namespace pcl
45 {
46  namespace cuda
47  {
48 
50  {
51  DisparityBoundSmoothing (int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)
52  : width_ (width), height_ (height), window_size_ (window_size)
53  , focal_length_(focal_length), baseline_(baseline), disparity_threshold_(disparity_threshold)
54  , data_(data), raw_data_(raw_data)
55  {}
56 
60  float baseline_;
62  float *data_;
63  float *raw_data_;
64 
65  // helper function
66  inline __host__ __device__
67  float disparity2depth (float disparity)
68  {
69  return baseline_ * focal_length_ / disparity;
70  }
71 
72  // helper function
73  inline __host__ __device__
74  float depth2disparity (float depth)
75  {
76  return baseline_ * focal_length_ / depth;
77  }
78 
79  // clampToDisparityBounds
80  inline __host__ __device__
81  float clampToDisparityBounds (float avg_depth, float depth)
82  {
83  float disparity = depth2disparity (depth);
84  float avg_disparity = depth2disparity (avg_depth);
85  float min_disparity = disparity - disparity_threshold_;
86  float max_disparity = disparity + disparity_threshold_;
87 
88  if (avg_disparity > max_disparity)
89  return disparity2depth (max_disparity);
90  if (avg_disparity < min_disparity)
91  return disparity2depth (min_disparity);
92 
93  return avg_depth;
94  }
95 
96  // actual kernel operator
97  inline __host__ __device__
98  float operator () (int idx)
99  {
100  float depth = data_ [idx];
101 #ifdef __CUDACC__
102  if (depth == 0 | isnan(depth) | isinf(depth))
103  return 0;
104 #else
105  if (depth == 0 | std::isnan(depth) | std::isinf(depth))
106  return 0;
107 #endif
108  int xIdx = idx % width_;
109  int yIdx = idx / width_;
110  // TODO: test median
111  // This implements a fixed window size in image coordinates (pixels)
112  int4 bounds = make_int4 (
113  xIdx - window_size_,
114  xIdx + window_size_,
115  yIdx - window_size_,
116  yIdx + window_size_
117  );
118 
119  // clamp the coordinates to fit to depth image size
120  bounds.x = clamp (bounds.x, 0, width_-1);
121  bounds.y = clamp (bounds.y, 0, width_-1);
122  bounds.z = clamp (bounds.z, 0, height_-1);
123  bounds.w = clamp (bounds.w, 0, height_-1);
124 
125  float average_depth = depth;
126  int counter = 1;
127  // iterate over all pixels in the rectangular region
128  for (int y = bounds.z; y <= bounds.w; ++y)
129  {
130  for (int x = bounds.x; x <= bounds.y; ++x)
131  {
132  // find index in point cloud from x,y pixel positions
133  int otherIdx = ((int)y) * width_ + ((int)x);
134  float otherDepth = data_[otherIdx];
135 
136  // ignore invalid points
137  if (otherDepth == 0)
138  continue;
139  if (std::abs(otherDepth - depth) > 200)
140  continue;
141 
142  ++counter;
143  average_depth += otherDepth;
144  }
145  }
146 
147  return clampToDisparityBounds (average_depth / counter, raw_data_[idx]);
148  }
149  };
150 
151 
152  // This version requires a pre-computed map of float3 (nr_valid_points, min_allowable_depth, max_allowable_depth);
154  {
155  DisparityClampedSmoothing (float* data, float3* disparity_helper_map, int width, int height, int window_size)
156  : data_(data), disparity_helper_map_(disparity_helper_map), width_(width), height_(height), window_size_(window_size)
157  {}
158 
159  float* data_;
161  int width_;
162  int height_;
164 
165  template <typename Tuple>
166  inline __host__ __device__
167  float operator () (Tuple t)
168  {
169  float depth = thrust::get<0> (t);
170  int idx = thrust::get<1> (t);
171  float3 dhel = disparity_helper_map_[idx];
172  int nr = (int) dhel.x;
173  float min_d = dhel.y;
174  float max_d = dhel.z;
175 #ifdef __CUDACC__
176  if (depth == 0 | isnan(depth) | isinf(depth))
177  return 0.0f;
178 #else
179  if (depth == 0 | std::isnan(depth) | std::isinf(depth))
180  return 0.0f;
181 #endif
182  int xIdx = idx % width_;
183  int yIdx = idx / width_;
184 
185  // This implements a fixed window size in image coordinates (pixels)
186  int4 bounds = make_int4 (
187  xIdx - window_size_,
188  xIdx + window_size_,
189  yIdx - window_size_,
190  yIdx + window_size_
191  );
192 
193  // clamp the coordinates to fit to disparity image size
194  bounds.x = clamp (bounds.x, 0, width_-1);
195  bounds.y = clamp (bounds.y, 0, width_-1);
196  bounds.z = clamp (bounds.z, 0, height_-1);
197  bounds.w = clamp (bounds.w, 0, height_-1);
198 
199  // iterate over all pixels in the rectangular region
200  for (int y = bounds.z; y <= bounds.w; ++y)
201  {
202  for (int x = bounds.x; x <= bounds.y; ++x)
203  {
204  // find index in point cloud from x,y pixel positions
205  int otherIdx = ((int)y) * width_ + ((int)x);
206  depth += data_[otherIdx];
207  }
208  }
209 
210  return clamp (depth / nr, min_d, max_d);
211  }
212  };
213 
215  {
216  DisparityHelperMap (float* data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
217  : data_(data), width_(width), height_(height), window_size_(window_size), baseline_(baseline), focal_length_(focal_length), disp_thresh_(disp_thresh)
218  {}
219 
220  float* data_;
221  int width_;
222  int height_;
224  float baseline_;
227 
228  // helper function
229  inline __host__ __device__
230  float disparity2depth (float disparity)
231  {
232  return baseline_ * focal_length_ / disparity;
233  }
234 
235  // helper function
236  inline __host__ __device__
237  float depth2disparity (float depth)
238  {
239  return baseline_ * focal_length_ / depth;
240  }
241 
242  inline __host__ __device__
243  float3 operator () (int idx)
244  {
245  float disparity = depth2disparity (data_ [idx]);
246 #ifdef __CUDACC__
247  if (disparity == 0 | isnan(disparity) | isinf(disparity))
248  return make_float3 (0,0,0);
249 #else
250  if (disparity == 0 | std::isnan(disparity) | std::isinf(disparity))
251  return make_float3 (0,0,0);
252 #endif
253  int xIdx = idx % width_;
254  int yIdx = idx / width_;
255 
256  // This implements a fixed window size in image coordinates (pixels)
257  int4 bounds = make_int4 (
258  xIdx - window_size_,
259  xIdx + window_size_,
260  yIdx - window_size_,
261  yIdx + window_size_
262  );
263 
264  // clamp the coordinates to fit to disparity image size
265  bounds.x = clamp (bounds.x, 0, width_-1);
266  bounds.y = clamp (bounds.y, 0, width_-1);
267  bounds.z = clamp (bounds.z, 0, height_-1);
268  bounds.w = clamp (bounds.w, 0, height_-1);
269 
270  int counter = 1;
271  // iterate over all pixels in the rectangular region
272  for (int y = bounds.z; y <= bounds.w; ++y)
273  {
274  for (int x = bounds.x; x <= bounds.y; ++x)
275  {
276  // find index in point cloud from x,y pixel positions
277  int otherIdx = ((int)y) * width_ + ((int)x);
278  float otherDepth = data_[otherIdx];
279 
280  // ignore invalid points
281  if (otherDepth > 0)
282  ++counter;
283  }
284  }
285 
286  return make_float3 ((float) counter,
287  disparity2depth (disparity + disp_thresh_),
288  disparity2depth (disparity - disp_thresh_));
289  }
290  };
291 
292 
293  } // namespace
294 } // namespace
DisparityBoundSmoothing(int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)
__host__ __device__ float disparity2depth(float disparity)
__host__ __device__ float depth2disparity(float depth)
__host__ __device__ float operator()(int idx)
__host__ __device__ float clampToDisparityBounds(float avg_depth, float depth)
__host__ __device__ float operator()(Tuple t)
DisparityClampedSmoothing(float *data, float3 *disparity_helper_map, int width, int height, int window_size)
__host__ __device__ float disparity2depth(float disparity)
DisparityHelperMap(float *data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
__host__ __device__ float3 operator()(int idx)
__host__ __device__ float depth2disparity(float depth)