Point Cloud Library (PCL)
1.7.0
|
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 00046 ////////////////////////////////////////////////////////////////////////////// 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 00111 /////////////////////////////////////////////////////////////////////////////// 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 00190 ////////////////////////////////////////////////////////////////////////////// 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 00239 ////////////////////////////////////////////////////////////////////////////// 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 00290 ////////////////////////////////////////////////////////////////////////////// 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 00348 ////////////////////////////////////////////////////////////////////////////// 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 00409 ////////////////////////////////////////////////////////////////////////////// 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 00459 ////////////////////////////////////////////////////////////////////////////// 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