Point Cloud Library (PCL) 1.12.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#pragma once
39
40#include <pcl/io/openni_camera/openni_image.h>
41#include <thrust/tuple.h>
42#include <pcl/pcl_exports.h>
43
44namespace 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_;
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_;
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)