1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011 2011 Willow Garage, Inc.
5 * Suat Gedikli <gedikli@willowgarage.com>
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37 #include <pcl/pcl_config.h>
38 #include <pcl/memory.h>
39 #ifdef HAVE_OPENNI
40
41 #include <pcl/io/openni_camera/openni_image_yuv_422.h>
42 #include <sstream>
43 #include <iostream>
44
45 #define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
46
47 namespace openni_wrapper
48 {
49
ImageYUV422(pcl::shared_ptr<xn::ImageMetaData> image_meta_data)50 ImageYUV422::ImageYUV422 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) noexcept
51 : Image (std::move(image_meta_data))
52 {
53 }
54
~ImageYUV422()55 ImageYUV422::~ImageYUV422 () noexcept
56 {
57 }
58
isResizingSupported(unsigned input_width,unsigned input_height,unsigned output_width,unsigned output_height) const59 bool ImageYUV422::isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const
60 {
61 return ImageYUV422::resizingSupported (input_width, input_height, output_width, output_height);
62 }
63
fillRGB(unsigned width,unsigned height,unsigned char * rgb_buffer,unsigned rgb_line_step) const64 void ImageYUV422::fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step) const
65 {
66 // 0 1 2 3
67 // u y1 v y2
68
69 if (image_md_->XRes() != width && image_md_->YRes() != height)
70 {
71 if (width > image_md_->XRes () || height > image_md_->YRes ())
72 THROW_OPENNI_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", image_md_->XRes (), image_md_->YRes (), width, height);
73
74 if ( image_md_->XRes () % width != 0 || image_md_->YRes () % height != 0
75 || (image_md_->XRes () / width) & 0x01 || (image_md_->YRes () / height & 0x01) )
76 THROW_OPENNI_EXCEPTION ("Downsampling only possible for power of two scale in both dimensions. Request was %d x %d -> %d x %d.", image_md_->XRes (), image_md_->YRes (), width, height);
77 }
78
79 const XnUInt8* yuv_buffer = image_md_->Data();
80
81 unsigned rgb_line_skip = 0;
82 if (rgb_line_step != 0)
83 rgb_line_skip = rgb_line_step - width * 3;
84
85 if (image_md_->XRes() == width && image_md_->YRes() == height)
86 {
87 for( unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_buffer += rgb_line_skip )
88 {
89 for( unsigned xIdx = 0; xIdx < width; xIdx += 2, rgb_buffer += 6, yuv_buffer += 4 )
90 {
91 int v = yuv_buffer[2] - 128;
92 int u = yuv_buffer[0] - 128;
93
94 rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14));
95 rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
96 rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14));
97
98 rgb_buffer[3] = CLIP_CHAR (yuv_buffer[3] + ((v * 18678 + 8192 ) >> 14));
99 rgb_buffer[4] = CLIP_CHAR (yuv_buffer[3] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
100 rgb_buffer[5] = CLIP_CHAR (yuv_buffer[3] + ((u * 33292 + 8192 ) >> 14));
101 }
102 }
103 }
104 else
105 {
106 unsigned yuv_step = image_md_->XRes() / width;
107 unsigned yuv_x_step = yuv_step << 1;
108 unsigned yuv_skip = (image_md_->YRes() / height - 1) * ( image_md_->XRes() << 1 );
109
110 for( unsigned yIdx = 0; yIdx < image_md_->YRes(); yIdx += yuv_step, yuv_buffer += yuv_skip, rgb_buffer += rgb_line_skip )
111 {
112 for( unsigned xIdx = 0; xIdx < image_md_->XRes(); xIdx += yuv_step, rgb_buffer += 3, yuv_buffer += yuv_x_step )
113 {
114 int v = yuv_buffer[2] - 128;
115 int u = yuv_buffer[0] - 128;
116
117 rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14));
118 rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
119 rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14));
120 }
121 }
122 }
123 }
124
fillGrayscale(unsigned width,unsigned height,unsigned char * gray_buffer,unsigned gray_line_step) const125 void ImageYUV422::fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step) const
126 {
127 // u y1 v y2
128 if (width > image_md_->XRes () || height > image_md_->YRes ())
129 THROW_OPENNI_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", image_md_->XRes (), image_md_->YRes (), width, height);
130
131 if (image_md_->XRes () % width != 0 || image_md_->YRes () % height != 0)
132 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);
133
134 unsigned gray_line_skip = 0;
135 if (gray_line_step != 0)
136 gray_line_skip = gray_line_step - width;
137
138 unsigned yuv_step = image_md_->XRes() / width;
139 unsigned yuv_x_step = yuv_step << 1;
140 unsigned yuv_skip = (image_md_->YRes() / height - 1) * ( image_md_->XRes() << 1 );
141 const XnUInt8* yuv_buffer = (image_md_->Data() + 1);
142
143 for( unsigned yIdx = 0; yIdx < image_md_->YRes(); yIdx += yuv_step, yuv_buffer += yuv_skip, gray_buffer += gray_line_skip )
144 {
145 for( unsigned xIdx = 0; xIdx < image_md_->XRes(); xIdx += yuv_step, ++gray_buffer, yuv_buffer += yuv_x_step )
146 {
147 *gray_buffer = *yuv_buffer;
148 }
149 }
150 }
151 }//namespace
152 #endif
153