openni_image_yuv_422.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011 2011 Willow Garage, Inc.
00005  *    Suat Gedikli <gedikli@willowgarage.com>
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 #include <pcl/pcl_config.h>
00038 #ifdef HAVE_OPENNI
00039 
00040 #include <pcl/io/openni_camera/openni_image_yuv_422.h>
00041 #include <sstream>
00042 #include <iostream>
00043 
00044 #define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
00045 
00046 using namespace std;
00047 namespace openni_wrapper
00048 {
00049 
00050 ImageYUV422::ImageYUV422 (boost::shared_ptr<xn::ImageMetaData> image_meta_data) throw ()
00051 : Image (image_meta_data)
00052 {
00053 }
00054 
00055 ImageYUV422::~ImageYUV422 () throw ()
00056 {
00057 }
00058 
00059 bool ImageYUV422::isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const
00060 {
00061   return ImageYUV422::resizingSupported (input_width, input_height, output_width, output_height);
00062 }
00063 
00064 void ImageYUV422::fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step) const
00065 {
00066   // 0  1   2  3
00067   // u  y1  v  y2
00068 
00069   if (image_md_->XRes() != width && image_md_->YRes() != height)
00070   {
00071     if (width > image_md_->XRes () || height > image_md_->YRes ())
00072       THROW_OPENNI_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", image_md_->XRes (), image_md_->YRes (), width, height);
00073 
00074     if ( image_md_->XRes () % width != 0 || image_md_->YRes () % height != 0
00075         || (image_md_->XRes () / width) & 0x01 || (image_md_->YRes () / height & 0x01) )
00076         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);
00077   }
00078 
00079   register const XnUInt8* yuv_buffer = image_md_->Data();
00080 
00081   unsigned rgb_line_skip = 0;
00082   if (rgb_line_step != 0)
00083     rgb_line_skip = rgb_line_step - width * 3;
00084 
00085   if (image_md_->XRes() == width && image_md_->YRes() == height)
00086   {
00087     for( register unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_buffer += rgb_line_skip )
00088     {
00089       for( register unsigned xIdx = 0; xIdx < width; xIdx += 2, rgb_buffer += 6, yuv_buffer += 4 )
00090       {
00091         int v = yuv_buffer[2] - 128;
00092         int u = yuv_buffer[0] - 128;
00093 
00094         rgb_buffer[0] =  CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14));
00095         rgb_buffer[1] =  CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
00096         rgb_buffer[2] =  CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14));
00097 
00098         rgb_buffer[3] =  CLIP_CHAR (yuv_buffer[3] + ((v * 18678 + 8192 ) >> 14));
00099         rgb_buffer[4] =  CLIP_CHAR (yuv_buffer[3] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
00100         rgb_buffer[5] =  CLIP_CHAR (yuv_buffer[3] + ((u * 33292 + 8192 ) >> 14));
00101       }
00102     }
00103   }
00104   else
00105   {
00106     register unsigned yuv_step = image_md_->XRes() / width;
00107     register unsigned yuv_x_step = yuv_step << 1;
00108     register unsigned yuv_skip = (image_md_->YRes() / height - 1) * ( image_md_->XRes() << 1 );
00109 
00110     for( register unsigned yIdx = 0; yIdx < image_md_->YRes(); yIdx += yuv_step, yuv_buffer += yuv_skip, rgb_buffer += rgb_line_skip )
00111     {
00112       for( register unsigned xIdx = 0; xIdx < image_md_->XRes(); xIdx += yuv_step, rgb_buffer += 3, yuv_buffer += yuv_x_step )
00113       {
00114         int v = yuv_buffer[2] - 128;
00115         int u = yuv_buffer[0] - 128;
00116 
00117         rgb_buffer[0] =  CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14));
00118         rgb_buffer[1] =  CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14));
00119         rgb_buffer[2] =  CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14));
00120       }
00121     }
00122   }
00123 }
00124 
00125 void ImageYUV422::fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step) const
00126 {
00127   // u y1 v y2
00128   if (width > image_md_->XRes () || height > image_md_->YRes ())
00129     THROW_OPENNI_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", image_md_->XRes (), image_md_->YRes (), width, height);
00130 
00131   if (image_md_->XRes () % width != 0 || image_md_->YRes () % height != 0)
00132       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);
00133 
00134   unsigned gray_line_skip = 0;
00135   if (gray_line_step != 0)
00136     gray_line_skip = gray_line_step - width;
00137 
00138   register unsigned yuv_step = image_md_->XRes() / width;
00139   register unsigned yuv_x_step = yuv_step << 1;
00140   register unsigned yuv_skip = (image_md_->YRes() / height - 1) * ( image_md_->XRes() << 1 );
00141   register const XnUInt8* yuv_buffer = (image_md_->Data() + 1);
00142 
00143   for( register unsigned yIdx = 0; yIdx < image_md_->YRes(); yIdx += yuv_step, yuv_buffer += yuv_skip, gray_buffer += gray_line_skip )
00144   {
00145     for( register unsigned xIdx = 0; xIdx < image_md_->XRes(); xIdx += yuv_step, ++gray_buffer, yuv_buffer += yuv_x_step )
00146     {
00147       *gray_buffer = *yuv_buffer;
00148     }
00149   }
00150 }
00151 }//namespace
00152 #endif


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:26:29