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) 2009-2012, Willow Garage, 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 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 00038 #ifndef ORGANIZED_COMPRESSION_HPP 00039 #define ORGANIZED_COMPRESSION_HPP 00040 00041 #include <pcl/compression/organized_pointcloud_compression.h> 00042 00043 #include <pcl/pcl_macros.h> 00044 #include <pcl/point_cloud.h> 00045 00046 #include <pcl/common/boost.h> 00047 #include <pcl/common/eigen.h> 00048 #include <pcl/common/common.h> 00049 #include <pcl/common/io.h> 00050 00051 #include <pcl/compression/libpng_wrapper.h> 00052 #include <pcl/compression/organized_pointcloud_conversion.h> 00053 00054 #include <string> 00055 #include <vector> 00056 #include <limits> 00057 #include <assert.h> 00058 00059 namespace pcl 00060 { 00061 namespace io 00062 { 00063 ////////////////////////////////////////////////////////////////////////////////////////////// 00064 template<typename PointT> void 00065 OrganizedPointCloudCompression<PointT>::encodePointCloud (const PointCloudConstPtr &cloud_arg, 00066 std::ostream& compressedDataOut_arg, 00067 bool doColorEncoding, 00068 bool convertToMono, 00069 bool bShowStatistics_arg, 00070 int pngLevel_arg) 00071 { 00072 uint32_t cloud_width = cloud_arg->width; 00073 uint32_t cloud_height = cloud_arg->height; 00074 00075 float maxDepth, focalLength, disparityShift, disparityScale; 00076 00077 // no disparity scaling/shifting required during decoding 00078 disparityScale = 1.0f; 00079 disparityShift = 0.0f; 00080 00081 analyzeOrganizedCloud (cloud_arg, maxDepth, focalLength); 00082 00083 // encode header identifier 00084 compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_)); 00085 // encode point cloud width 00086 compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_width), sizeof (cloud_width)); 00087 // encode frame type height 00088 compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_height), sizeof (cloud_height)); 00089 // encode frame max depth 00090 compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth)); 00091 // encode frame focal lenght 00092 compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength), sizeof (focalLength)); 00093 // encode frame disparity scale 00094 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale), sizeof (disparityScale)); 00095 // encode frame disparity shift 00096 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift), sizeof (disparityShift)); 00097 00098 // disparity and rgb image data 00099 std::vector<uint16_t> disparityData; 00100 std::vector<uint8_t> colorData; 00101 00102 // compressed disparity and rgb image data 00103 std::vector<uint8_t> compressedDisparity; 00104 std::vector<uint8_t> compressedColor; 00105 00106 uint32_t compressedDisparitySize = 0; 00107 uint32_t compressedColorSize = 0; 00108 00109 // Convert point cloud to disparity and rgb image 00110 OrganizedConversion<PointT>::convert (*cloud_arg, focalLength, disparityShift, disparityScale, convertToMono, disparityData, colorData); 00111 00112 // Compress disparity information 00113 encodeMonoImageToPNG (disparityData, cloud_width, cloud_height, compressedDisparity, pngLevel_arg); 00114 00115 compressedDisparitySize = static_cast<uint32_t>(compressedDisparity.size()); 00116 // Encode size of compressed disparity image data 00117 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize)); 00118 // Output compressed disparity to ostream 00119 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(uint8_t)); 00120 00121 // Compress color information 00122 if (CompressionPointTraits<PointT>::hasColor && doColorEncoding) 00123 { 00124 if (convertToMono) 00125 { 00126 encodeMonoImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/); 00127 } else 00128 { 00129 encodeRGBImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/); 00130 } 00131 } 00132 00133 compressedColorSize = static_cast<uint32_t>(compressedColor.size ()); 00134 // Encode size of compressed Color image data 00135 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize)); 00136 // Output compressed disparity to ostream 00137 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(uint8_t)); 00138 00139 if (bShowStatistics_arg) 00140 { 00141 uint64_t pointCount = cloud_width * cloud_height; 00142 float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount); 00143 00144 PCL_INFO("*** POINTCLOUD ENCODING ***\n"); 00145 PCL_INFO("Number of encoded points: %ld\n", pointCount); 00146 PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f); 00147 PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f); 00148 PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint)); 00149 PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f); 00150 PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint); 00151 } 00152 00153 // flush output stream 00154 compressedDataOut_arg.flush(); 00155 } 00156 00157 ////////////////////////////////////////////////////////////////////////////////////////////// 00158 template<typename PointT> void 00159 OrganizedPointCloudCompression<PointT>::encodeRawDisparityMapWithColorImage ( std::vector<uint16_t>& disparityMap_arg, 00160 std::vector<uint8_t>& colorImage_arg, 00161 uint32_t width_arg, 00162 uint32_t height_arg, 00163 std::ostream& compressedDataOut_arg, 00164 bool doColorEncoding, 00165 bool convertToMono, 00166 bool bShowStatistics_arg, 00167 int pngLevel_arg, 00168 float focalLength_arg, 00169 float disparityShift_arg, 00170 float disparityScale_arg) 00171 { 00172 float maxDepth = -1; 00173 00174 size_t cloud_size = width_arg*height_arg; 00175 assert (disparityMap_arg.size()==cloud_size); 00176 if (colorImage_arg.size()) 00177 { 00178 assert (colorImage_arg.size()==cloud_size*3); 00179 } 00180 00181 // encode header identifier 00182 compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_)); 00183 // encode point cloud width 00184 compressedDataOut_arg.write (reinterpret_cast<const char*> (&width_arg), sizeof (width_arg)); 00185 // encode frame type height 00186 compressedDataOut_arg.write (reinterpret_cast<const char*> (&height_arg), sizeof (height_arg)); 00187 // encode frame max depth 00188 compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth)); 00189 // encode frame focal lenght 00190 compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength_arg), sizeof (focalLength_arg)); 00191 // encode frame disparity scale 00192 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale_arg), sizeof (disparityScale_arg)); 00193 // encode frame disparity shift 00194 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift_arg), sizeof (disparityShift_arg)); 00195 00196 // compressed disparity and rgb image data 00197 std::vector<uint8_t> compressedDisparity; 00198 std::vector<uint8_t> compressedColor; 00199 00200 uint32_t compressedDisparitySize = 0; 00201 uint32_t compressedColorSize = 0; 00202 00203 // Remove color information of invalid points 00204 uint16_t* depth_ptr = &disparityMap_arg[0]; 00205 uint8_t* color_ptr = &colorImage_arg[0]; 00206 00207 size_t i; 00208 for (i=0; i<cloud_size; ++i, ++depth_ptr, color_ptr+=sizeof(uint8_t)*3) 00209 { 00210 if (!(*depth_ptr) || (*depth_ptr==0x7FF)) 00211 memset(color_ptr, 0, sizeof(uint8_t)*3); 00212 } 00213 00214 // Compress disparity information 00215 encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg); 00216 00217 compressedDisparitySize = static_cast<uint32_t>(compressedDisparity.size()); 00218 // Encode size of compressed disparity image data 00219 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize)); 00220 // Output compressed disparity to ostream 00221 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(uint8_t)); 00222 00223 // Compress color information 00224 if (colorImage_arg.size() && doColorEncoding) 00225 { 00226 if (convertToMono) 00227 { 00228 size_t i, size; 00229 vector<uint8_t> monoImage; 00230 size = width_arg*height_arg; 00231 00232 monoImage.reserve(size); 00233 00234 // grayscale conversion 00235 for (i=0; i<size; ++i) 00236 { 00237 uint8_t grayvalue = static_cast<uint8_t>(0.2989 * static_cast<float>(colorImage_arg[i*3+0]) + 00238 0.5870 * static_cast<float>(colorImage_arg[i*3+1]) + 00239 0.1140 * static_cast<float>(colorImage_arg[i*3+2])); 00240 monoImage.push_back(grayvalue); 00241 } 00242 encodeMonoImageToPNG (monoImage, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/); 00243 00244 } else 00245 { 00246 encodeRGBImageToPNG (colorImage_arg, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/); 00247 } 00248 00249 } 00250 00251 compressedColorSize = static_cast<uint32_t>(compressedColor.size ()); 00252 // Encode size of compressed Color image data 00253 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize)); 00254 // Output compressed disparity to ostream 00255 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(uint8_t)); 00256 00257 if (bShowStatistics_arg) 00258 { 00259 uint64_t pointCount = width_arg * height_arg; 00260 float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount); 00261 00262 PCL_INFO("*** POINTCLOUD ENCODING ***\n"); 00263 PCL_INFO("Number of encoded points: %ld\n", pointCount); 00264 PCL_INFO("Size of uncompressed disparity map+color image: %.2f kBytes\n", (static_cast<float> (pointCount) * (sizeof(uint8_t)*3+sizeof(uint16_t))) / 1024.0f); 00265 PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f); 00266 PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint)); 00267 PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (sizeof(uint8_t)*3+sizeof(uint16_t)) * 100.0f); 00268 PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint); 00269 } 00270 00271 // flush output stream 00272 compressedDataOut_arg.flush(); 00273 } 00274 00275 ////////////////////////////////////////////////////////////////////////////////////////////// 00276 template<typename PointT> bool 00277 OrganizedPointCloudCompression<PointT>::decodePointCloud (std::istream& compressedDataIn_arg, 00278 PointCloudPtr &cloud_arg, 00279 bool bShowStatistics_arg) 00280 { 00281 uint32_t cloud_width; 00282 uint32_t cloud_height; 00283 float maxDepth, focalLength, disparityShift, disparityScale; 00284 00285 // disparity and rgb image data 00286 std::vector<uint16_t> disparityData; 00287 std::vector<uint8_t> colorData; 00288 00289 // compressed disparity and rgb image data 00290 std::vector<uint8_t> compressedDisparity; 00291 std::vector<uint8_t> compressedColor; 00292 00293 uint32_t compressedDisparitySize; 00294 uint32_t compressedColorSize; 00295 00296 // PNG decoded parameters 00297 size_t png_width = 0; 00298 size_t png_height = 0; 00299 unsigned int png_channels = 1; 00300 00301 // sync to frame header 00302 unsigned int headerIdPos = 0; 00303 bool valid_stream = true; 00304 while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_))) 00305 { 00306 char readChar; 00307 compressedDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar)); 00308 if (compressedDataIn_arg.gcount()!= sizeof (readChar)) 00309 valid_stream = false; 00310 if (readChar != frameHeaderIdentifier_[headerIdPos++]) 00311 headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0; 00312 00313 valid_stream &= compressedDataIn_arg.good (); 00314 } 00315 00316 if (valid_stream) { 00317 00318 ////////////// 00319 // reading frame header 00320 compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_width), sizeof (cloud_width)); 00321 compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_height), sizeof (cloud_height)); 00322 compressedDataIn_arg.read (reinterpret_cast<char*> (&maxDepth), sizeof (maxDepth)); 00323 compressedDataIn_arg.read (reinterpret_cast<char*> (&focalLength), sizeof (focalLength)); 00324 compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityScale), sizeof (disparityScale)); 00325 compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityShift), sizeof (disparityShift)); 00326 00327 // reading compressed disparity data 00328 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparitySize), sizeof (compressedDisparitySize)); 00329 compressedDisparity.resize (compressedDisparitySize); 00330 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparity[0]), compressedDisparitySize * sizeof(uint8_t)); 00331 00332 // reading compressed rgb data 00333 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColorSize), sizeof (compressedColorSize)); 00334 compressedColor.resize (compressedColorSize); 00335 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColor[0]), compressedColorSize * sizeof(uint8_t)); 00336 00337 // decode PNG compressed disparity data 00338 decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels); 00339 00340 // decode PNG compressed rgb data 00341 decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels); 00342 } 00343 00344 if (disparityShift==0.0f) 00345 { 00346 // reconstruct point cloud 00347 OrganizedConversion<PointT>::convert (disparityData, 00348 colorData, 00349 static_cast<bool>(png_channels==1), 00350 cloud_width, 00351 cloud_height, 00352 focalLength, 00353 disparityShift, 00354 disparityScale, 00355 *cloud_arg); 00356 } else 00357 { 00358 00359 // we need to decode a raw shift image 00360 std::size_t size = disparityData.size(); 00361 std::vector<float> depthData; 00362 depthData.resize(size); 00363 00364 // initialize shift-to-depth converter 00365 if (!sd_converter_.isInitialized()) 00366 sd_converter_.generateLookupTable(); 00367 00368 // convert shift to depth image 00369 for (std::size_t i=0; i<size; ++i) 00370 depthData[i] = sd_converter_.shiftToDepth(disparityData[i]); 00371 00372 // reconstruct point cloud 00373 OrganizedConversion<PointT>::convert (depthData, 00374 colorData, 00375 static_cast<bool>(png_channels==1), 00376 cloud_width, 00377 cloud_height, 00378 focalLength, 00379 *cloud_arg); 00380 } 00381 00382 if (bShowStatistics_arg) 00383 { 00384 uint64_t pointCount = cloud_width * cloud_height; 00385 float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount); 00386 00387 PCL_INFO("*** POINTCLOUD DECODING ***\n"); 00388 PCL_INFO("Number of encoded points: %ld\n", pointCount); 00389 PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f); 00390 PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f); 00391 PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint)); 00392 PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f); 00393 PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint); 00394 } 00395 00396 return valid_stream; 00397 } 00398 00399 ////////////////////////////////////////////////////////////////////////////////////////////// 00400 template<typename PointT> void 00401 OrganizedPointCloudCompression<PointT>::analyzeOrganizedCloud (PointCloudConstPtr cloud_arg, 00402 float& maxDepth_arg, 00403 float& focalLength_arg) const 00404 { 00405 size_t width, height, it; 00406 int centerX, centerY; 00407 int x, y; 00408 float maxDepth; 00409 float focalLength; 00410 00411 width = cloud_arg->width; 00412 height = cloud_arg->height; 00413 00414 // Center of organized point cloud 00415 centerX = static_cast<int> (width / 2); 00416 centerY = static_cast<int> (height / 2); 00417 00418 // Ensure we have an organized point cloud 00419 assert((width>1) && (height>1)); 00420 assert(width*height == cloud_arg->points.size()); 00421 00422 maxDepth = 0; 00423 focalLength = 0; 00424 00425 it = 0; 00426 for (y = -centerY; y < +centerY; ++y) 00427 for (x = -centerX; x < +centerX; ++x) 00428 { 00429 const PointT& point = cloud_arg->points[it++]; 00430 00431 if (pcl::isFinite (point)) 00432 { 00433 if (maxDepth < point.z) 00434 { 00435 // Update maximum depth 00436 maxDepth = point.z; 00437 00438 // Calculate focal length 00439 focalLength = 2.0f / (point.x / (static_cast<float> (x) * point.z) + point.y / (static_cast<float> (y) * point.z)); 00440 } 00441 } 00442 } 00443 00444 // Update return values 00445 maxDepth_arg = maxDepth; 00446 focalLength_arg = focalLength; 00447 } 00448 00449 } 00450 } 00451 00452 #endif 00453