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