lzf_image_io.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception, Inc.
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 the copyright holder(s) 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 
00038 #ifndef PCL_LZF_IMAGE_IO_HPP_
00039 #define PCL_LZF_IMAGE_IO_HPP_
00040 
00041 #include <pcl/console/print.h>
00042 #include <pcl/io/debayer.h>
00043 
00044 #define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
00045 
00047 template <typename PointT> bool
00048 pcl::io::LZFDepth16ImageReader::read (
00049     const std::string &filename, pcl::PointCloud<PointT> &cloud)
00050 {
00051   uint32_t uncompressed_size;
00052   std::vector<char> compressed_data;
00053   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00054   {
00055     PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00056     return (false);
00057   }
00058 
00059   if (uncompressed_size != getWidth () * getHeight () * 2)
00060   {
00061     PCL_DEBUG ("[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 2, filename.c_str (), getImageType ().c_str ());
00062     return (false);
00063   }
00064 
00065   std::vector<char> uncompressed_data (uncompressed_size);
00066   decompress (compressed_data, uncompressed_data);
00067 
00068   if (uncompressed_data.empty ())
00069   {
00070     PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00071     return (false);
00072   }
00073 
00074   // Copy to PointT
00075   cloud.width    = getWidth ();
00076   cloud.height   = getHeight ();
00077   cloud.is_dense = true;
00078   cloud.resize (getWidth () * getHeight ());
00079   register int depth_idx = 0, point_idx = 0;
00080   double constant_x = 1.0 / parameters_.focal_length_x,
00081          constant_y = 1.0 / parameters_.focal_length_y;
00082   for (int v = 0; v < cloud.height; ++v)
00083   {
00084     for (register int u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2)
00085     {
00086       PointT &pt = cloud.points[point_idx];
00087       unsigned short val;
00088       memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short));
00089       if (val == 0)
00090       {
00091         pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
00092         cloud.is_dense = false;
00093         continue;
00094       }
00095 
00096       pt.z = static_cast<float> (val * z_multiplication_factor_);
00097       pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x)) 
00098         * pt.z * static_cast<float> (constant_x);
00099       pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y)) 
00100         * pt.z * static_cast<float> (constant_y);
00101     }
00102   }
00103   cloud.sensor_origin_.setZero ();
00104   cloud.sensor_orientation_.w () = 1.0f;
00105   cloud.sensor_orientation_.x () = 0.0f;
00106   cloud.sensor_orientation_.y () = 0.0f;
00107   cloud.sensor_orientation_.z () = 0.0f;
00108   return (true);
00109 }
00110         
00112 template <typename PointT> bool
00113 pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename, 
00114                                          pcl::PointCloud<PointT> &cloud, 
00115                                          unsigned int num_threads)
00116 {
00117   uint32_t uncompressed_size;
00118   std::vector<char> compressed_data;
00119   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00120   {
00121     PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00122     return (false);
00123   }
00124 
00125   if (uncompressed_size != getWidth () * getHeight () * 2)
00126   {
00127     PCL_DEBUG ("[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 2, filename.c_str (), getImageType ().c_str ());
00128     return (false);
00129   }
00130 
00131   std::vector<char> uncompressed_data (uncompressed_size);
00132   decompress (compressed_data, uncompressed_data);
00133 
00134   if (uncompressed_data.empty ())
00135   {
00136     PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00137     return (false);
00138   }
00139 
00140   // Copy to PointT
00141   cloud.width    = getWidth ();
00142   cloud.height   = getHeight ();
00143   cloud.is_dense = true;
00144   cloud.resize (getWidth () * getHeight ());
00145   double constant_x = 1.0 / parameters_.focal_length_x,
00146          constant_y = 1.0 / parameters_.focal_length_y;
00147 #ifdef _OPENMP
00148 #pragma omp parallel for num_threads (num_threads)
00149 #endif
00150   for (int i = 0; i < static_cast< int> (cloud.size ()); ++i)
00151   {
00152     int u = i % cloud.width;
00153     int v = i / cloud.width;
00154     PointT &pt = cloud.points[i];
00155     int depth_idx = 2*i;
00156     unsigned short val;
00157     memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short));
00158     if (val == 0)
00159     {
00160       pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
00161       if (cloud.is_dense)
00162       {
00163 #ifdef _OPENMP
00164 #pragma omp critical
00165 #endif
00166       {
00167       if (cloud.is_dense)
00168         cloud.is_dense = false;
00169       }
00170       }
00171       continue;
00172     }
00173 
00174     pt.z = static_cast<float> (val * z_multiplication_factor_);
00175     pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x)) 
00176       * pt.z * static_cast<float> (constant_x);
00177     pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y)) 
00178       * pt.z * static_cast<float> (constant_y);
00179     
00180   }
00181   cloud.sensor_origin_.setZero ();
00182   cloud.sensor_orientation_.w () = 1.0f;
00183   cloud.sensor_orientation_.x () = 0.0f;
00184   cloud.sensor_orientation_.y () = 0.0f;
00185   cloud.sensor_orientation_.z () = 0.0f;
00186   return (true);
00187 
00188 }
00189 
00191 template <typename PointT> bool
00192 pcl::io::LZFRGB24ImageReader::read (
00193     const std::string &filename, pcl::PointCloud<PointT> &cloud)
00194 {
00195   uint32_t uncompressed_size;
00196   std::vector<char> compressed_data;
00197   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00198   {
00199     PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00200     return (false);
00201   }
00202 
00203   if (uncompressed_size != getWidth () * getHeight () * 3)
00204   {
00205     PCL_DEBUG ("[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 3, filename.c_str (), getImageType ().c_str ());
00206     return (false);
00207   }
00208 
00209   std::vector<char> uncompressed_data (uncompressed_size);
00210   decompress (compressed_data, uncompressed_data);
00211 
00212   if (uncompressed_data.empty ())
00213   {
00214     PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00215     return (false);
00216   }
00217 
00218   // Copy to PointT
00219   cloud.width  = getWidth ();
00220   cloud.height = getHeight ();
00221   cloud.resize (getWidth () * getHeight ());
00222 
00223   register int rgb_idx = 0;
00224   unsigned char *color_r = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
00225   unsigned char *color_g = reinterpret_cast<unsigned char*> (&uncompressed_data[getWidth () * getHeight ()]);
00226   unsigned char *color_b = reinterpret_cast<unsigned char*> (&uncompressed_data[2 * getWidth () * getHeight ()]);
00227 
00228   for (size_t i = 0; i < cloud.size (); ++i, ++rgb_idx)
00229   {
00230     PointT &pt = cloud.points[i];
00231 
00232     pt.b = color_b[rgb_idx];
00233     pt.g = color_g[rgb_idx];
00234     pt.r = color_r[rgb_idx];
00235   }
00236   return (true);
00237 }
00238 
00240 template <typename PointT> bool
00241 pcl::io::LZFRGB24ImageReader::readOMP (
00242     const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
00243 {
00244   uint32_t uncompressed_size;
00245   std::vector<char> compressed_data;
00246   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00247   {
00248     PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00249     return (false);
00250   }
00251 
00252   if (uncompressed_size != getWidth () * getHeight () * 3)
00253   {
00254     PCL_DEBUG ("[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 3, filename.c_str (), getImageType ().c_str ());
00255     return (false);
00256   }
00257 
00258   std::vector<char> uncompressed_data (uncompressed_size);
00259   decompress (compressed_data, uncompressed_data);
00260 
00261   if (uncompressed_data.empty ())
00262   {
00263     PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00264     return (false);
00265   }
00266 
00267   // Copy to PointT
00268   cloud.width  = getWidth ();
00269   cloud.height = getHeight ();
00270   cloud.resize (getWidth () * getHeight ());
00271 
00272   unsigned char *color_r = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
00273   unsigned char *color_g = reinterpret_cast<unsigned char*> (&uncompressed_data[getWidth () * getHeight ()]);
00274   unsigned char *color_b = reinterpret_cast<unsigned char*> (&uncompressed_data[2 * getWidth () * getHeight ()]);
00275 
00276 #ifdef _OPENMP
00277 #pragma omp parallel for num_threads (num_threads)
00278 #endif//_OPENMP
00279   for (long int i = 0; i < cloud.size (); ++i)
00280   {
00281     PointT &pt = cloud.points[i];
00282 
00283     pt.b = color_b[i];
00284     pt.g = color_g[i];
00285     pt.r = color_r[i];
00286   }
00287   return (true);
00288 }
00289 
00291 template <typename PointT> bool
00292 pcl::io::LZFYUV422ImageReader::read (
00293     const std::string &filename, pcl::PointCloud<PointT> &cloud)
00294 {
00295   uint32_t uncompressed_size;
00296   std::vector<char> compressed_data;
00297   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00298   {
00299     PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00300     return (false);
00301   }
00302 
00303   if (uncompressed_size != getWidth () * getHeight () * 2)
00304   {
00305     PCL_DEBUG ("[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
00306     return (false);
00307   }
00308 
00309   std::vector<char> uncompressed_data (uncompressed_size);
00310   decompress (compressed_data, uncompressed_data);
00311 
00312   if (uncompressed_data.empty ())
00313   {
00314     PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00315     return (false);
00316   }
00317 
00318   // Convert YUV422 to RGB24 and copy to PointT
00319   cloud.width  = getWidth ();
00320   cloud.height = getHeight ();
00321   cloud.resize (getWidth () * getHeight ());
00322 
00323   int wh2 = getWidth () * getHeight () / 2;
00324   unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
00325   unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
00326   unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
00327   
00328   register int y_idx = 0;
00329   for (size_t i = 0; i < wh2; ++i, y_idx += 2)
00330   {
00331     int v = color_v[i] - 128;
00332     int u = color_u[i] - 128;
00333 
00334     PointT &pt1 = cloud.points[y_idx + 0];
00335     pt1.r =  CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
00336     pt1.g =  CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
00337     pt1.b =  CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
00338 
00339     PointT &pt2 = cloud.points[y_idx + 1];
00340     pt2.r =  CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
00341     pt2.g =  CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
00342     pt2.b =  CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
00343   }
00344 
00345   return (true);
00346 }
00347 
00349 template <typename PointT> bool
00350 pcl::io::LZFYUV422ImageReader::readOMP (
00351     const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
00352 {
00353   uint32_t uncompressed_size;
00354   std::vector<char> compressed_data;
00355   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00356   {
00357     PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00358     return (false);
00359   }
00360 
00361   if (uncompressed_size != getWidth () * getHeight () * 2)
00362   {
00363     PCL_DEBUG ("[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
00364     return (false);
00365   }
00366 
00367   std::vector<char> uncompressed_data (uncompressed_size);
00368   decompress (compressed_data, uncompressed_data);
00369 
00370   if (uncompressed_data.empty ())
00371   {
00372     PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00373     return (false);
00374   }
00375 
00376   // Convert YUV422 to RGB24 and copy to PointT
00377   cloud.width  = getWidth ();
00378   cloud.height = getHeight ();
00379   cloud.resize (getWidth () * getHeight ());
00380 
00381   int wh2 = getWidth () * getHeight () / 2;
00382   unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
00383   unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
00384   unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
00385   
00386 #ifdef _OPENMP
00387 #pragma omp parallel for num_threads (num_threads)
00388 #endif//_OPENMP
00389   for (int i = 0; i < wh2; ++i)
00390   {
00391     int y_idx = 2*i;
00392     int v = color_v[i] - 128;
00393     int u = color_u[i] - 128;
00394 
00395     PointT &pt1 = cloud.points[y_idx + 0];
00396     pt1.r =  CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
00397     pt1.g =  CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
00398     pt1.b =  CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
00399 
00400     PointT &pt2 = cloud.points[y_idx + 1];
00401     pt2.r =  CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
00402     pt2.g =  CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
00403     pt2.b =  CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
00404   }
00405 
00406   return (true);
00407 }
00408 
00410 template <typename PointT> bool
00411 pcl::io::LZFBayer8ImageReader::read (
00412     const std::string &filename, pcl::PointCloud<PointT> &cloud)
00413 {
00414   uint32_t uncompressed_size;
00415   std::vector<char> compressed_data;
00416   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00417   {
00418     PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00419     return (false);
00420   }
00421 
00422   if (uncompressed_size != getWidth () * getHeight ())
00423   {
00424     PCL_DEBUG ("[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
00425     return (false);
00426   }
00427 
00428   std::vector<char> uncompressed_data (uncompressed_size);
00429   decompress (compressed_data, uncompressed_data);
00430 
00431   if (uncompressed_data.empty ())
00432   {
00433     PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00434     return (false);
00435   }
00436 
00437   // Convert Bayer8 to RGB24
00438   std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
00439   pcl::io::DeBayer i;
00440   i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]), 
00441                      static_cast<unsigned char*> (&rgb_buffer[0]), 
00442                      getWidth (), getHeight ());
00443   // Copy to PointT
00444   cloud.width  = getWidth ();
00445   cloud.height = getHeight ();
00446   cloud.resize (getWidth () * getHeight ());
00447   register int rgb_idx = 0;
00448   for (size_t i = 0; i < cloud.size (); ++i, rgb_idx += 3)
00449   {
00450     PointT &pt = cloud.points[i];
00451 
00452     pt.b = rgb_buffer[rgb_idx + 2];
00453     pt.g = rgb_buffer[rgb_idx + 1];
00454     pt.r = rgb_buffer[rgb_idx + 0];
00455   }
00456   return (true);
00457 }
00458 
00460 template <typename PointT> bool
00461 pcl::io::LZFBayer8ImageReader::readOMP (
00462     const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
00463 {
00464   uint32_t uncompressed_size;
00465   std::vector<char> compressed_data;
00466   if (!loadImageBlob (filename, compressed_data, uncompressed_size))
00467   {
00468     PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
00469     return (false);
00470   }
00471 
00472   if (uncompressed_size != getWidth () * getHeight ())
00473   {
00474     PCL_DEBUG ("[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
00475     return (false);
00476   }
00477 
00478   std::vector<char> uncompressed_data (uncompressed_size);
00479   decompress (compressed_data, uncompressed_data);
00480 
00481   if (uncompressed_data.empty ())
00482   {
00483     PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
00484     return (false);
00485   }
00486 
00487   // Convert Bayer8 to RGB24
00488   std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
00489   pcl::io::DeBayer i;
00490   i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]), 
00491                      static_cast<unsigned char*> (&rgb_buffer[0]), 
00492                      getWidth (), getHeight ());
00493   // Copy to PointT
00494   cloud.width  = getWidth ();
00495   cloud.height = getHeight ();
00496   cloud.resize (getWidth () * getHeight ());
00497 #ifdef _OPENMP
00498 #pragma omp parallel for num_threads (num_threads)
00499 #endif//_OPENMP
00500   for (long int i = 0; i < cloud.size (); ++i)
00501   {
00502     PointT &pt = cloud.points[i];
00503     long int rgb_idx = 3*i;
00504     pt.b = rgb_buffer[rgb_idx + 2];
00505     pt.g = rgb_buffer[rgb_idx + 1];
00506     pt.r = rgb_buffer[rgb_idx + 0];
00507   }
00508   return (true);
00509 }
00510 
00511 #endif  //#ifndef PCL_LZF_IMAGE_IO_HPP_
00512 


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