1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of Willow Garage, Inc. nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 */
38 #include <pcl/pcl_config.h>
39 #include <pcl/memory.h>
40 #ifdef HAVE_OPENNI
41
42 #include <pcl/io/openni_camera/openni_image_bayer_grbg.h>
43 #include <pcl/io/debayer.h>
44
45 #define AVG(a,b) static_cast<unsigned char>((int(a) + int(b)) >> 1)
46 #define AVG3(a,b,c) static_cast<unsigned char>((int(a) + int(b) + int(c)) / 3)
47 #define AVG4(a,b,c,d) static_cast<unsigned char>((int(a) + int(b) + int(c) + int(d)) >> 2)
48 #define WAVG4(a,b,c,d,x,y) static_cast<unsigned char>( ( (int(a) + int(b)) * int(x) + (int(c) + int(d)) * int(y) ) / ( (int(x) + (int(y))) << 1 ) )
49
50 //////////////////////////////////////////////////////////////////////////////
ImageBayerGRBG(pcl::shared_ptr<xn::ImageMetaData> image_meta_data,DebayeringMethod method)51 openni_wrapper::ImageBayerGRBG::ImageBayerGRBG (pcl::shared_ptr<xn::ImageMetaData> image_meta_data, DebayeringMethod method) noexcept
52 : Image (std::move(image_meta_data))
53 , debayering_method_ (method)
54 {
55 }
56
57 //////////////////////////////////////////////////////////////////////////////
~ImageBayerGRBG()58 openni_wrapper::ImageBayerGRBG::~ImageBayerGRBG () noexcept
59 {
60 }
61
62 //////////////////////////////////////////////////////////////////////////////
63 bool
isResizingSupported(unsigned input_width,unsigned input_height,unsigned output_width,unsigned output_height) const64 openni_wrapper::ImageBayerGRBG::isResizingSupported (
65 unsigned input_width, unsigned input_height,
66 unsigned output_width, unsigned output_height) const
67 {
68 return (openni_wrapper::ImageBayerGRBG::resizingSupported (input_width, input_height, output_width, output_height));
69 }
70
71 //////////////////////////////////////////////////////////////////////////////
72 void
fillGrayscale(unsigned width,unsigned height,unsigned char * gray_buffer,unsigned gray_line_step) const73 openni_wrapper::ImageBayerGRBG::fillGrayscale (
74 unsigned width, unsigned height,
75 unsigned char* gray_buffer, unsigned gray_line_step) const
76 {
77 if (width > image_md_->XRes () || height > image_md_->YRes ())
78 THROW_OPENNI_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", image_md_->XRes (), image_md_->YRes (), width, height);
79
80 if (gray_line_step == 0)
81 gray_line_step = width;
82
83 // padding skip for destination image
84 unsigned gray_line_skip = gray_line_step - width;
85 if (image_md_->XRes () == width && image_md_->YRes () == height)
86 { // if no downsampling
87 const XnUInt8 *bayer_pixel = image_md_->Data ();
88 int line_skip = image_md_->XRes ();
89 if (debayering_method_ == Bilinear)
90 {
91 // first line GRGRGR
92 for (unsigned xIdx = 0; xIdx < width - 2; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
93 {
94 gray_buffer[0] = bayer_pixel[0]; // green pixel
95 gray_buffer[1] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[1 + line_skip]); // interpolated green pixel
96 }
97 gray_buffer[0] = bayer_pixel[0];
98 gray_buffer[1] = AVG (bayer_pixel[0], bayer_pixel[1 + line_skip]);
99 gray_buffer += 2 + gray_line_skip;
100 bayer_pixel += 2;
101
102 for (unsigned yIdx = 1; yIdx < height - 1; yIdx += 2)
103 {
104 // blue line
105 gray_buffer[0] = AVG3 (bayer_pixel[-line_skip], bayer_pixel[line_skip], bayer_pixel[1]);
106 gray_buffer[1] = bayer_pixel[1];
107 gray_buffer += 2;
108 bayer_pixel += 2;
109 for (unsigned xIdx = 2; xIdx < width; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
110 {
111 gray_buffer[0] = AVG4 (bayer_pixel[-line_skip], bayer_pixel[line_skip], bayer_pixel[-1], bayer_pixel[1]);
112 gray_buffer[1] = bayer_pixel[1];
113 }
114 gray_buffer += gray_line_skip;
115
116 // red line
117 for (unsigned xIdx = 0; xIdx < width - 2; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
118 {
119 gray_buffer[0] = bayer_pixel[0]; // green pixel
120 gray_buffer[1] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[-line_skip + 1], bayer_pixel[line_skip + 1]); // interpolated green pixel
121 }
122 gray_buffer[0] = bayer_pixel[0];
123 gray_buffer[1] = AVG3 (bayer_pixel[-line_skip + 1], bayer_pixel[line_skip + 1], bayer_pixel[-1]);
124 gray_buffer += 2 + gray_line_skip;
125 bayer_pixel += 2;
126 }
127
128 // last line BGBGBG
129 gray_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-line_skip]);
130 gray_buffer[1] = bayer_pixel[1];
131 gray_buffer += 2;
132 bayer_pixel += 2;
133 for (unsigned xIdx = 2; xIdx < width; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
134 {
135 gray_buffer[0] = AVG3 (bayer_pixel[-1], bayer_pixel[1], bayer_pixel[-line_skip]);
136 gray_buffer[1] = bayer_pixel[1];
137 }
138 }
139 else if (debayering_method_ == EdgeAware)
140 {
141 int dv, dh;
142 // first line GRGRGR
143 for (unsigned xIdx = 0; xIdx < width - 2; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
144 {
145 gray_buffer[0] = bayer_pixel[0]; // green pixel
146 gray_buffer[1] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[1 + line_skip]); // interpolated green pixel
147 }
148 gray_buffer[0] = bayer_pixel[0];
149 gray_buffer[1] = AVG (bayer_pixel[0], bayer_pixel[1 + line_skip]);
150 gray_buffer += 2 + gray_line_skip;
151 bayer_pixel += 2;
152
153 for (unsigned yIdx = 1; yIdx < height - 1; yIdx += 2)
154 {
155 // blue line
156 gray_buffer[0] = AVG3 (bayer_pixel[-line_skip], bayer_pixel[line_skip], bayer_pixel[1]);
157 gray_buffer[1] = bayer_pixel[1];
158 gray_buffer += 2;
159 bayer_pixel += 2;
160 for (unsigned xIdx = 2; xIdx < width; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
161 {
162 dv = std::abs (bayer_pixel[-line_skip] - bayer_pixel[line_skip]);
163 dh = std::abs (bayer_pixel[-1] - bayer_pixel[1]);
164 if (dh > dv)
165 gray_buffer[0] = AVG (bayer_pixel[-line_skip], bayer_pixel[line_skip]);
166 else if (dv > dh)
167 gray_buffer[0] = AVG (bayer_pixel[-1], bayer_pixel[1]);
168 else
169 gray_buffer[0] = AVG4 (bayer_pixel[-line_skip], bayer_pixel[line_skip], bayer_pixel[-1], bayer_pixel[1]);
170
171 gray_buffer[1] = bayer_pixel[1];
172 }
173 gray_buffer += gray_line_skip;
174
175 // red line
176 for (unsigned xIdx = 0; xIdx < width - 2; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
177 {
178 gray_buffer[0] = bayer_pixel[0];
179
180 dv = std::abs (bayer_pixel[1 - line_skip] - bayer_pixel[1 + line_skip]);
181 dh = std::abs (bayer_pixel[0] - bayer_pixel[2]);
182 if (dh > dv)
183 gray_buffer[1] = AVG (bayer_pixel[1 - line_skip], bayer_pixel[1 + line_skip]);
184 else if (dv > dh)
185 gray_buffer[1] = AVG (bayer_pixel[0], bayer_pixel[2]);
186 else
187 gray_buffer[1] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[-line_skip + 1], bayer_pixel[line_skip + 1]);
188 }
189 gray_buffer[0] = bayer_pixel[0];
190 gray_buffer[1] = AVG3 (bayer_pixel[-line_skip + 1], bayer_pixel[line_skip + 1], bayer_pixel[-1]);
191 gray_buffer += 2 + gray_line_skip;
192 bayer_pixel += 2;
193 }
194
195 // last line BGBGBG
196 gray_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-line_skip]);
197 gray_buffer[1] = bayer_pixel[1];
198 gray_buffer += 2;
199 bayer_pixel += 2;
200 for (unsigned xIdx = 2; xIdx < width; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
201 {
202 gray_buffer[0] = AVG3 (bayer_pixel[-1], bayer_pixel[1], bayer_pixel[-line_skip]);
203 gray_buffer[1] = bayer_pixel[1];
204 }
205 }
206 else if (debayering_method_ == EdgeAwareWeighted)
207 {
208 int dv, dh;
209 // first line GRGRGR
210 for (unsigned xIdx = 0; xIdx < width - 2; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
211 {
212 gray_buffer[0] = bayer_pixel[0]; // green pixel
213 gray_buffer[1] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[1 + line_skip]); // interpolated green pixel
214 }
215 gray_buffer[0] = bayer_pixel[0];
216 gray_buffer[1] = AVG (bayer_pixel[0], bayer_pixel[1 + line_skip]);
217 gray_buffer += 2 + gray_line_skip;
218 bayer_pixel += 2;
219
220 for (unsigned yIdx = 1; yIdx < height - 1; yIdx += 2)
221 {
222 // blue line
223 gray_buffer[0] = AVG3 (bayer_pixel[-line_skip], bayer_pixel[line_skip], bayer_pixel[1]);
224 gray_buffer[1] = bayer_pixel[1];
225 gray_buffer += 2;
226 bayer_pixel += 2;
227 for (unsigned xIdx = 2; xIdx < width; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
228 {
229 dv = std::abs (bayer_pixel[-line_skip] - bayer_pixel[line_skip]);
230 dh = std::abs (bayer_pixel[-1] - bayer_pixel[1]);
231
232 if (dv == 0 && dh == 0)
233 gray_buffer[0] = AVG4 (bayer_pixel[-line_skip], bayer_pixel[line_skip], bayer_pixel[-1], bayer_pixel[1]);
234 else
235 gray_buffer[0] = WAVG4 (bayer_pixel[-line_skip], bayer_pixel[line_skip], bayer_pixel[-1], bayer_pixel[1], dh, dv);
236
237 gray_buffer[1] = bayer_pixel[1];
238 }
239
240 gray_buffer += gray_line_skip;
241
242 // red line
243 for (unsigned xIdx = 0; xIdx < width - 2; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
244 {
245 gray_buffer[0] = bayer_pixel[0];
246
247 dv = std::abs (bayer_pixel[1 - line_skip] - bayer_pixel[1 + line_skip]);
248 dh = std::abs (bayer_pixel[0] - bayer_pixel[2]);
249
250 if (dv == 0 && dh == 0)
251 gray_buffer[1] = AVG4 (bayer_pixel[1 - line_skip], bayer_pixel[1 + line_skip], bayer_pixel[0], bayer_pixel[2]);
252 else
253 gray_buffer[1] = WAVG4 (bayer_pixel[1 - line_skip], bayer_pixel[1 + line_skip], bayer_pixel[0], bayer_pixel[2], dh, dv);
254 }
255 gray_buffer[0] = bayer_pixel[0];
256 gray_buffer[1] = AVG3 (bayer_pixel[-line_skip + 1], bayer_pixel[line_skip + 1], bayer_pixel[-1]);
257 gray_buffer += 2 + gray_line_skip;
258 bayer_pixel += 2;
259 }
260
261 // last line BGBGBG
262 gray_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-line_skip]);
263 gray_buffer[1] = bayer_pixel[1];
264 gray_buffer += 2;
265 bayer_pixel += 2;
266 for (unsigned xIdx = 2; xIdx < width; xIdx += 2, gray_buffer += 2, bayer_pixel += 2)
267 {
268 gray_buffer[0] = AVG3 (bayer_pixel[-1], bayer_pixel[1], bayer_pixel[-line_skip]);
269 gray_buffer[1] = bayer_pixel[1];
270 }
271 }
272 else
273 THROW_OPENNI_EXCEPTION ("Unknown Debayering method: %d", debayering_method_);
274
275 }
276 else // downsampling
277 {
278 if ((image_md_->XRes () >> 1) % width != 0 || (image_md_->YRes () >> 1) % height != 0)
279 THROW_OPENNI_EXCEPTION ("Downsampling only possible for multiple of 2 in both dimensions. Request was %d x %d -> %d x %d.", image_md_->XRes (), image_md_->YRes (), width, height);
280
281 // fast method -> simply takes each or each 2nd pixel-group to get gray values out
282 unsigned bayerXStep = image_md_->XRes () / width;
283 unsigned bayerYSkip = (image_md_->YRes () / height - 1) * image_md_->XRes ();
284
285 // Downsampling and debayering at once
286 const XnUInt8* bayer_buffer = image_md_->Data ();
287
288 for (unsigned yIdx = 0; yIdx < height; ++yIdx, bayer_buffer += bayerYSkip, gray_buffer += gray_line_skip) // skip a line
289 {
290 for (unsigned xIdx = 0; xIdx < width; ++xIdx, ++gray_buffer, bayer_buffer += bayerXStep)
291 {
292 *gray_buffer = AVG (bayer_buffer[0], bayer_buffer[ image_md_->XRes () + 1]);
293 }
294 }
295 } // downsampling
296 }
297
298 //////////////////////////////////////////////////////////////////////////////
299 void
fillRGB(unsigned width,unsigned height,unsigned char * rgb_buffer,unsigned rgb_line_step) const300 openni_wrapper::ImageBayerGRBG::fillRGB (
301 unsigned width, unsigned height,
302 unsigned char* rgb_buffer, unsigned rgb_line_step) const
303 {
304 if (width > image_md_->XRes () || height > image_md_->YRes ())
305 THROW_OPENNI_EXCEPTION ("Upsampling only possible for multiple of 2 in both dimensions. Request was %d x %d -> %d x %d.", image_md_->XRes (), image_md_->YRes (), width, height);
306
307 if (rgb_line_step == 0)
308 rgb_line_step = width * 3;
309
310 // padding skip for destination image
311 unsigned rgb_line_skip = rgb_line_step - width * 3;
312
313 if (image_md_->XRes () == width && image_md_->YRes () == height)
314 {
315 const unsigned char* bayer_pixel = image_md_->Data ();
316
317 int bayer_line_step = image_md_->XRes ();
318 int bayer_line_step2 = image_md_->XRes () << 1;
319
320 pcl::io::DeBayer d;
321 if (debayering_method_ == Bilinear)
322 d.debayerBilinear (bayer_pixel, rgb_buffer, width, height, bayer_line_step, bayer_line_step2, rgb_line_step);
323 else if (debayering_method_ == EdgeAware)
324 d.debayerEdgeAware (bayer_pixel, rgb_buffer, width, height, bayer_line_step, bayer_line_step2, rgb_line_step);
325 else if (debayering_method_ == EdgeAwareWeighted)
326 d.debayerEdgeAwareWeighted (bayer_pixel, rgb_buffer, width, height, bayer_line_step, bayer_line_step2, rgb_line_step);
327 else
328 THROW_OPENNI_EXCEPTION ("Unknown debayering method: %d", debayering_method_);
329 }
330 else
331 {
332 if (image_md_->XRes () % width != 0 || image_md_->YRes () % height != 0)
333 THROW_OPENNI_EXCEPTION ("Downsampling only possible for integer scales in both dimensions. Request was %d x %d -> %d x %d.", image_md_->XRes (), image_md_->YRes (), width, height);
334
335 // get each or each 2nd pixel group to find rgb values!
336 unsigned bayerXStep = image_md_->XRes () / width;
337 unsigned bayerYSkip = (image_md_->YRes () / height - 1) * image_md_->XRes ();
338
339 // Downsampling and debayering at once
340 const XnUInt8* bayer_buffer = image_md_->Data ();
341
342 for (unsigned yIdx = 0; yIdx < height; ++yIdx, bayer_buffer += bayerYSkip, rgb_buffer += rgb_line_skip) // skip a line
343 {
344 for (unsigned xIdx = 0; xIdx < width; ++xIdx, rgb_buffer += 3, bayer_buffer += bayerXStep)
345 {
346 rgb_buffer[ 2 ] = bayer_buffer[ image_md_->XRes () ];
347 rgb_buffer[ 1 ] = AVG (bayer_buffer[0], bayer_buffer[ image_md_->XRes () + 1]);
348 rgb_buffer[ 0 ] = bayer_buffer[ 1 ];
349 }
350 }
351 }
352 }
353
354 #endif
355