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/io/image_yuv422.h>
39
40 #include <pcl/io/io_exception.h>
41
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 using pcl::io::FrameWrapper;
48 using pcl::io::IOException;
49
ImageYUV422(FrameWrapper::Ptr image_metadata)50 pcl::io::ImageYUV422::ImageYUV422 (FrameWrapper::Ptr image_metadata)
51 : Image (std::move(image_metadata))
52 {}
53
54
ImageYUV422(FrameWrapper::Ptr image_metadata,Timestamp timestamp)55 pcl::io::ImageYUV422::ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp)
56 : Image (std::move(image_metadata), timestamp)
57 {}
58
59
~ImageYUV422()60 pcl::io::ImageYUV422::~ImageYUV422 () noexcept
61 {}
62
63 bool
isResizingSupported(unsigned input_width,unsigned input_height,unsigned output_width,unsigned output_height) const64 pcl::io::ImageYUV422::isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const
65 {
66 return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 );
67 }
68
69
70 void
fillRGB(unsigned width,unsigned height,unsigned char * rgb_buffer,unsigned rgb_line_step) const71 pcl::io::ImageYUV422::fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step) const
72 {
73 // 0 1 2 3
74 // u y1 v y2
75
76 if (wrapper_->getWidth () != width && wrapper_->getHeight () != height)
77 {
78 if (width > wrapper_->getWidth () || height > wrapper_->getHeight () )
79 THROW_IO_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height);
80
81 if ( wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0
82 || (wrapper_->getWidth () / width) & 0x01 || (wrapper_->getHeight () / height & 0x01) )
83 THROW_IO_EXCEPTION ("Downsampling only possible for power of two scale in both dimensions. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height);
84 }
85
86 const std::uint8_t* yuv_buffer = (std::uint8_t*) wrapper_->getData ();
87
88 unsigned rgb_line_skip = 0;
89 if (rgb_line_step != 0)
90 rgb_line_skip = rgb_line_step - width * 3;
91
92 if (wrapper_->getWidth () == width && wrapper_->getHeight () == height)
93 {
94 for (unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_buffer += rgb_line_skip)
95 {
96 for (unsigned xIdx = 0; xIdx < width; xIdx += 2, rgb_buffer += 6, yuv_buffer += 4)
97 {
98 int v = yuv_buffer[2] - 128;
99 int u = yuv_buffer[0] - 128;
100
101 rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14));
102 rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
103 rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14));
104
105 rgb_buffer[3] = CLIP_CHAR (yuv_buffer[3] + ((v * 18678 + 8192 ) >> 14));
106 rgb_buffer[4] = CLIP_CHAR (yuv_buffer[3] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
107 rgb_buffer[5] = CLIP_CHAR (yuv_buffer[3] + ((u * 33292 + 8192 ) >> 14));
108 }
109 }
110 }
111 else
112 {
113 unsigned yuv_step = wrapper_->getWidth () / width;
114 unsigned yuv_x_step = yuv_step << 1;
115 unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 );
116
117 for (unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, rgb_buffer += rgb_line_skip)
118 {
119 for (unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, rgb_buffer += 3, yuv_buffer += yuv_x_step)
120 {
121 int v = yuv_buffer[2] - 128;
122 int u = yuv_buffer[0] - 128;
123
124 rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14));
125 rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
126 rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14));
127 }
128 }
129 }
130 }
131
132
133 void
fillGrayscale(unsigned width,unsigned height,unsigned char * gray_buffer,unsigned gray_line_step) const134 pcl::io::ImageYUV422::fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step) const
135 {
136 // u y1 v y2
137 if (width > wrapper_->getWidth () || height > wrapper_->getHeight ())
138 THROW_IO_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height);
139
140 if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0)
141 THROW_IO_EXCEPTION ("Downsampling only possible for integer scales in both dimensions. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height);
142
143 unsigned gray_line_skip = 0;
144 if (gray_line_step != 0)
145 gray_line_skip = gray_line_step - width;
146
147 unsigned yuv_step = wrapper_->getWidth () / width;
148 unsigned yuv_x_step = yuv_step << 1;
149 unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 );
150 const std::uint8_t* yuv_buffer = ( (std::uint8_t*) wrapper_->getData () + 1);
151
152 for (unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, gray_buffer += gray_line_skip)
153 {
154 for (unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, ++gray_buffer, yuv_buffer += yuv_x_step)
155 {
156 *gray_buffer = *yuv_buffer;
157 }
158 }
159 }
160
161