organized_pointcloud_compression.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2012, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef ORGANIZED_COMPRESSION_HPP
#define ORGANIZED_COMPRESSION_HPP
#include <pcl/compression/organized_pointcloud_compression.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/common/boost.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <pcl/common/io.h>
#include <pcl/compression/libpng_wrapper.h>
#include <pcl/compression/organized_pointcloud_conversion.h>
#include <string>
#include <vector>
#include <limits>
#include <cassert>
namespace pcl
{
namespace io
{
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
OrganizedPointCloudCompression<PointT>::encodePointCloud (const PointCloudConstPtr &cloud_arg,
std::ostream& compressedDataOut_arg,
bool doColorEncoding,
bool convertToMono,
bool bShowStatistics_arg,
int pngLevel_arg)
{
std::uint32_t cloud_width = cloud_arg->width;
std::uint32_t cloud_height = cloud_arg->height;
float maxDepth, focalLength, disparityShift, disparityScale;
// no disparity scaling/shifting required during decoding
disparityScale = 1.0f;
disparityShift = 0.0f;
analyzeOrganizedCloud (cloud_arg, maxDepth, focalLength);
// encode header identifier
compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
// encode point cloud width
compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_width), sizeof (cloud_width));
// encode frame type height
compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_height), sizeof (cloud_height));
// encode frame max depth
compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth));
// encode frame focal length
compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength), sizeof (focalLength));
// encode frame disparity scale
compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale), sizeof (disparityScale));
// encode frame disparity shift
compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift), sizeof (disparityShift));
// disparity and rgb image data
std::vector<std::uint16_t> disparityData;
std::vector<std::uint8_t> colorData;
// compressed disparity and rgb image data
std::vector<std::uint8_t> compressedDisparity;
std::vector<std::uint8_t> compressedColor;
std::uint32_t compressedDisparitySize = 0;
std::uint32_t compressedColorSize = 0;
// Convert point cloud to disparity and rgb image
OrganizedConversion<PointT>::convert (*cloud_arg, focalLength, disparityShift, disparityScale, convertToMono, disparityData, colorData);
// Compress disparity information
encodeMonoImageToPNG (disparityData, cloud_width, cloud_height, compressedDisparity, pngLevel_arg);
compressedDisparitySize = static_cast<std::uint32_t>(compressedDisparity.size());
// Encode size of compressed disparity image data
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
// Output compressed disparity to ostream
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(std::uint8_t));
// Compress color information
if (CompressionPointTraits<PointT>::hasColor && doColorEncoding)
{
if (convertToMono)
{
encodeMonoImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/);
} else
{
encodeRGBImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/);
}
}
compressedColorSize = static_cast<std::uint32_t>(compressedColor.size ());
// Encode size of compressed Color image data
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
// Output compressed disparity to ostream
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(std::uint8_t));
if (bShowStatistics_arg)
{
std::uint64_t pointCount = cloud_width * cloud_height;
float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
PCL_INFO("*** POINTCLOUD ENCODING ***\n");
PCL_INFO("Number of encoded points: %ld\n", pointCount);
PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
}
// flush output stream
compressedDataOut_arg.flush();
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
OrganizedPointCloudCompression<PointT>::encodeRawDisparityMapWithColorImage ( std::vector<std::uint16_t>& disparityMap_arg,
std::vector<std::uint8_t>& colorImage_arg,
std::uint32_t width_arg,
std::uint32_t height_arg,
std::ostream& compressedDataOut_arg,
bool doColorEncoding,
bool convertToMono,
bool bShowStatistics_arg,
int pngLevel_arg,
float focalLength_arg,
float disparityShift_arg,
float disparityScale_arg)
{
float maxDepth = -1;
std::size_t cloud_size = width_arg*height_arg;
assert (disparityMap_arg.size()==cloud_size);
if (!colorImage_arg.empty ())
{
assert (colorImage_arg.size()==cloud_size*3);
}
// encode header identifier
compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
// encode point cloud width
compressedDataOut_arg.write (reinterpret_cast<const char*> (&width_arg), sizeof (width_arg));
// encode frame type height
compressedDataOut_arg.write (reinterpret_cast<const char*> (&height_arg), sizeof (height_arg));
// encode frame max depth
compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth));
// encode frame focal length
compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength_arg), sizeof (focalLength_arg));
// encode frame disparity scale
compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale_arg), sizeof (disparityScale_arg));
// encode frame disparity shift
compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift_arg), sizeof (disparityShift_arg));
// compressed disparity and rgb image data
std::vector<std::uint8_t> compressedDisparity;
std::vector<std::uint8_t> compressedColor;
std::uint32_t compressedDisparitySize = 0;
std::uint32_t compressedColorSize = 0;
// Remove color information of invalid points
std::uint16_t* depth_ptr = &disparityMap_arg[0];
std::uint8_t* color_ptr = &colorImage_arg[0];
for (std::size_t i = 0; i < cloud_size; ++i, ++depth_ptr, color_ptr += sizeof(std::uint8_t) * 3)
{
if (!(*depth_ptr) || (*depth_ptr==0x7FF))
memset(color_ptr, 0, sizeof(std::uint8_t)*3);
}
// Compress disparity information
encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg);
compressedDisparitySize = static_cast<std::uint32_t>(compressedDisparity.size());
// Encode size of compressed disparity image data
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
// Output compressed disparity to ostream
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(std::uint8_t));
// Compress color information
if (!colorImage_arg.empty () && doColorEncoding)
{
if (convertToMono)
{
std::vector<std::uint8_t> monoImage;
std::size_t size = width_arg*height_arg;
monoImage.reserve(size);
// grayscale conversion
for (std::size_t i = 0; i < size; ++i)
{
std::uint8_t grayvalue = static_cast<std::uint8_t>(0.2989 * static_cast<float>(colorImage_arg[i*3+0]) +
0.5870 * static_cast<float>(colorImage_arg[i*3+1]) +
0.1140 * static_cast<float>(colorImage_arg[i*3+2]));
monoImage.push_back(grayvalue);
}
encodeMonoImageToPNG (monoImage, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
} else
{
encodeRGBImageToPNG (colorImage_arg, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
}
}
compressedColorSize = static_cast<std::uint32_t>(compressedColor.size ());
// Encode size of compressed Color image data
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
// Output compressed disparity to ostream
compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(std::uint8_t));
if (bShowStatistics_arg)
{
std::uint64_t pointCount = width_arg * height_arg;
float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
PCL_INFO("*** POINTCLOUD ENCODING ***\n");
PCL_INFO("Number of encoded points: %ld\n", pointCount);
PCL_INFO("Size of uncompressed disparity map+color image: %.2f kBytes\n", (static_cast<float> (pointCount) * (sizeof(std::uint8_t)*3+sizeof(std::uint16_t))) / 1024.0f);
PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (sizeof(std::uint8_t)*3+sizeof(std::uint16_t)) * 100.0f);
PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
}
// flush output stream
compressedDataOut_arg.flush();
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> bool
OrganizedPointCloudCompression<PointT>::decodePointCloud (std::istream& compressedDataIn_arg,
PointCloudPtr &cloud_arg,
bool bShowStatistics_arg)
{
std::uint32_t cloud_width;
std::uint32_t cloud_height;
float maxDepth;
float focalLength;
float disparityShift = 0.0f;
float disparityScale;
// disparity and rgb image data
std::vector<std::uint16_t> disparityData;
std::vector<std::uint8_t> colorData;
// compressed disparity and rgb image data
std::vector<std::uint8_t> compressedDisparity;
std::vector<std::uint8_t> compressedColor;
std::uint32_t compressedDisparitySize;
std::uint32_t compressedColorSize;
// PNG decoded parameters
std::size_t png_width = 0;
std::size_t png_height = 0;
unsigned int png_channels = 1;
// sync to frame header
unsigned int headerIdPos = 0;
bool valid_stream = true;
while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
{
char readChar;
compressedDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
if (compressedDataIn_arg.gcount()!= sizeof (readChar))
valid_stream = false;
if (readChar != frameHeaderIdentifier_[headerIdPos++])
headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0;
valid_stream &= compressedDataIn_arg.good ();
}
if (valid_stream) {
//////////////
// reading frame header
compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_width), sizeof (cloud_width));
compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_height), sizeof (cloud_height));
compressedDataIn_arg.read (reinterpret_cast<char*> (&maxDepth), sizeof (maxDepth));
compressedDataIn_arg.read (reinterpret_cast<char*> (&focalLength), sizeof (focalLength));
compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityScale), sizeof (disparityScale));
compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityShift), sizeof (disparityShift));
// reading compressed disparity data
compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
compressedDisparity.resize (compressedDisparitySize);
compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparity[0]), compressedDisparitySize * sizeof(std::uint8_t));
// reading compressed rgb data
compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColorSize), sizeof (compressedColorSize));
compressedColor.resize (compressedColorSize);
compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColor[0]), compressedColorSize * sizeof(std::uint8_t));
// decode PNG compressed disparity data
decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels);
// decode PNG compressed rgb data
decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels);
}
if (disparityShift==0.0f)
{
// reconstruct point cloud
OrganizedConversion<PointT>::convert (disparityData,
colorData,
(png_channels == 1),
cloud_width,
cloud_height,
focalLength,
disparityShift,
disparityScale,
*cloud_arg);
} else
{
// we need to decode a raw shift image
std::size_t size = disparityData.size();
std::vector<float> depthData;
depthData.resize(size);
// initialize shift-to-depth converter
if (!sd_converter_.isInitialized())
sd_converter_.generateLookupTable();
// convert shift to depth image
for (std::size_t i=0; i<size; ++i)
depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
// reconstruct point cloud
OrganizedConversion<PointT>::convert (depthData,
colorData,
static_cast<bool>(png_channels==1),
cloud_width,
cloud_height,
focalLength,
*cloud_arg);
}
if (bShowStatistics_arg)
{
std::uint64_t pointCount = cloud_width * cloud_height;
float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
PCL_INFO("*** POINTCLOUD DECODING ***\n");
PCL_INFO("Number of encoded points: %ld\n", pointCount);
PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
}
return valid_stream;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
OrganizedPointCloudCompression<PointT>::analyzeOrganizedCloud (PointCloudConstPtr cloud_arg,
float& maxDepth_arg,
float& focalLength_arg) const
{
std::size_t width = cloud_arg->width;
std::size_t height = cloud_arg->height;
// Center of organized point cloud
int centerX = static_cast<int> (width / 2);
int centerY = static_cast<int> (height / 2);
// Ensure we have an organized point cloud
assert((width>1) && (height>1));
assert(width*height == cloud_arg->points.size());
float maxDepth = 0;
float focalLength = 0;
std::size_t it = 0;
for (int y = -centerY; y < centerY; ++y )
for (int x = -centerX; x < centerX; ++x )
{
const PointT& point = cloud_arg->points[it++];
if (pcl::isFinite (point))
{
if (maxDepth < point.z)
{
// Update maximum depth
maxDepth = point.z;
// Calculate focal length
focalLength = 2.0f / (point.x / (static_cast<float> (x) * point.z) + point.y / (static_cast<float> (y) * point.z));
}
}
}
// Update return values
maxDepth_arg = maxDepth;
focalLength_arg = focalLength;
}
}
}
#endif
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::io::CompressionPointTraits
Definition: organized_pointcloud_conversion.h:54
pcl::uint32_t
std::uint32_t uint32_t
Definition: pcl_macros.h:96
common.h
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
pcl::io::OrganizedPointCloudCompression::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: organized_pointcloud_compression.h:65
pcl::uint16_t
std::uint16_t uint16_t
Definition: pcl_macros.h:94
pcl::io::encodeRGBImageToPNG
PCL_EXPORTS void encodeRGBImageToPNG(std::vector< std::uint8_t > &image_arg, std::size_t width_arg, std::size_t height_arg, std::vector< std::uint8_t > &pngData_arg, int png_level_arg=-1)
Encodes 8-bit RGB image to PNG format.
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::io::decodePNGToImage
PCL_EXPORTS void decodePNGToImage(std::vector< std::uint8_t > &pngData_arg, std::vector< std::uint8_t > &imageData_arg, std::size_t &width_arg, std::size_t &heigh_argt, unsigned int &channels_arg)
Decode compressed PNG to 8-bit image.
pcl::io::OrganizedPointCloudCompression::decodePointCloud
bool decodePointCloud(std::istream &compressedDataIn_arg, PointCloudPtr &cloud_arg, bool bShowStatistics_arg=true)
Decode point cloud from input stream.
Definition: organized_pointcloud_compression.hpp:275
pcl::io::OrganizedPointCloudCompression::encodePointCloud
void encodePointCloud(const PointCloudConstPtr &cloud_arg, std::ostream &compressedDataOut_arg, bool doColorEncoding=false, bool convertToMono=false, bool bShowStatistics_arg=true, int pngLevel_arg=-1)
Encode point cloud to output stream.
Definition: organized_pointcloud_compression.hpp:65
pcl::io::OrganizedPointCloudCompression::encodeRawDisparityMapWithColorImage
void encodeRawDisparityMapWithColorImage(std::vector< std::uint16_t > &disparityMap_arg, std::vector< std::uint8_t > &colorImage_arg, std::uint32_t width_arg, std::uint32_t height_arg, std::ostream &compressedDataOut_arg, bool doColorEncoding=false, bool convertToMono=false, bool bShowStatistics_arg=true, int pngLevel_arg=-1, float focalLength_arg=525.0f, float disparityShift_arg=174.825f, float disparityScale_arg=-0.161175f)
Encode raw disparity map and color image.
Definition: organized_pointcloud_compression.hpp:159
pcl::io::encodeMonoImageToPNG
PCL_EXPORTS void encodeMonoImageToPNG(std::vector< std::uint8_t > &image_arg, std::size_t width_arg, std::size_t height_arg, std::vector< std::uint8_t > &pngData_arg, int png_level_arg=-1)
Encodes 8-bit mono image to PNG format.
pcl::io::OrganizedPointCloudCompression::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: organized_pointcloud_compression.h:64
pcl::uint8_t
std::uint8_t uint8_t
Definition: pcl_macros.h:92
pcl::io::OrganizedPointCloudCompression::analyzeOrganizedCloud
void analyzeOrganizedCloud(PointCloudConstPtr cloud_arg, float &maxDepth_arg, float &focalLength_arg) const
Analyze input point cloud and calculate the maximum depth and focal length.
Definition: organized_pointcloud_compression.hpp:402
pcl::io::OrganizedConversion
Definition: organized_pointcloud_conversion.h:79
pcl::uint64_t
std::uint64_t uint64_t
Definition: pcl_macros.h:98