Loading...
Searching...
No Matches
organized_pointcloud_conversion.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, 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.
*
* $Id$
* Authors: Julius Kammerl (julius@kammerl.de)
*/
#pragma once
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/common/point_tests.h> // for pcl::isFinite
#include <vector>
#include <limits>
#include <cassert>
namespace pcl
{
namespace io
{
template<typename PointT> struct CompressionPointTraits
{
static const bool hasColor = false;
static const unsigned int channels = 1;
static const std::size_t bytesPerPoint = 3 * sizeof(float);
};
template<>
struct CompressionPointTraits<PointXYZRGB>
{
static const bool hasColor = true;
static const unsigned int channels = 4;
static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
};
template<>
struct CompressionPointTraits<PointXYZRGBA>
{
static const bool hasColor = true;
static const unsigned int channels = 4;
static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
};
template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor >
struct OrganizedConversion;
// Uncolored point cloud specialization
template<typename PointT>
struct OrganizedConversion<PointT, false>
{
/** \brief Convert point cloud to disparity image
* \param[in] cloud_arg input point cloud
* \param[in] focalLength_arg focal length
* \param[in] disparityShift_arg disparity shift
* \param[in] disparityScale_arg disparity scaling
* \param[out] disparityData_arg output disparity image
* \ingroup io
*/
static void convert(const pcl::PointCloud<PointT>& cloud_arg,
float focalLength_arg,
float disparityShift_arg,
float disparityScale_arg,
bool ,
typename std::vector<std::uint16_t>& disparityData_arg,
typename std::vector<std::uint8_t>&)
{
const auto cloud_size = cloud_arg.size ();
// Clear image data
disparityData_arg.clear ();
disparityData_arg.reserve (cloud_size);
for (std::size_t i = 0; i < cloud_size; ++i)
{
// Get point from cloud
const PointT& point = cloud_arg[i];
if (pcl::isFinite (point))
{
// Inverse depth quantization
auto disparity = static_cast<std::uint16_t> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
disparityData_arg.push_back (disparity);
}
else
{
// Non-valid points are encoded with zeros
disparityData_arg.push_back (0);
}
}
}
/** \brief Convert disparity image to point cloud
* \param[in] disparityData_arg input depth image
* \param[in] width_arg width of disparity image
* \param[in] height_arg height of disparity image
* \param[in] focalLength_arg focal length
* \param[in] disparityShift_arg disparity shift
* \param[in] disparityScale_arg disparity scaling
* \param[out] cloud_arg output point cloud
* \ingroup io
*/
static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
typename std::vector<std::uint8_t>&,
bool,
std::size_t width_arg,
std::size_t height_arg,
float focalLength_arg,
float disparityShift_arg,
float disparityScale_arg,
pcl::PointCloud<PointT>& cloud_arg)
{
std::size_t cloud_size = width_arg * height_arg;
assert(disparityData_arg.size()==cloud_size);
// Reset point cloud
cloud_arg.clear ();
cloud_arg.reserve (cloud_size);
// Define point cloud parameters
cloud_arg.width = static_cast<std::uint32_t> (width_arg);
cloud_arg.height = static_cast<std::uint32_t> (height_arg);
cloud_arg.is_dense = false;
// Calculate center of disparity image
int centerX = static_cast<int> (width_arg / 2);
int centerY = static_cast<int> (height_arg / 2);
const float fl_const = 1.0f / focalLength_arg;
static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
std::size_t i = 0;
for (int y = -centerY; y < centerY; ++y )
for (int x = -centerX; x < centerX; ++x )
{
PointT newPoint;
const std::uint16_t& pixel_disparity = disparityData_arg[i];
++i;
if (pixel_disparity)
{
// Inverse depth decoding
float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
// Generate new points
newPoint.x = static_cast<float> (x) * depth * fl_const;
newPoint.y = static_cast<float> (y) * depth * fl_const;
newPoint.z = depth;
}
else
{
// Generate bad point
newPoint.x = newPoint.y = newPoint.z = bad_point;
}
cloud_arg.push_back (newPoint);
}
}
/** \brief Convert disparity image to point cloud
* \param[in] depthData_arg input depth image
* \param[in] width_arg width of disparity image
* \param[in] height_arg height of disparity image
* \param[in] focalLength_arg focal length
* \param[out] cloud_arg output point cloud
* \ingroup io
*/
static void convert(typename std::vector<float>& depthData_arg,
typename std::vector<std::uint8_t>&,
bool,
std::size_t width_arg,
std::size_t height_arg,
float focalLength_arg,
pcl::PointCloud<PointT>& cloud_arg)
{
std::size_t cloud_size = width_arg * height_arg;
assert(depthData_arg.size()==cloud_size);
// Reset point cloud
cloud_arg.clear ();
cloud_arg.reserve (cloud_size);
// Define point cloud parameters
cloud_arg.width = static_cast<std::uint32_t> (width_arg);
cloud_arg.height = static_cast<std::uint32_t> (height_arg);
cloud_arg.is_dense = false;
// Calculate center of disparity image
int centerX = static_cast<int> (width_arg / 2);
int centerY = static_cast<int> (height_arg / 2);
const float fl_const = 1.0f / focalLength_arg;
static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
std::size_t i = 0;
for (int y = -centerY; y < centerY; ++y )
for (int x = -centerX; x < centerX; ++x )
{
PointT newPoint;
const float& pixel_depth = depthData_arg[i];
++i;
if (pixel_depth)
{
// Generate new points
newPoint.x = static_cast<float> (x) * pixel_depth * fl_const;
newPoint.y = static_cast<float> (y) * pixel_depth * fl_const;
newPoint.z = pixel_depth;
}
else
{
// Generate bad point
newPoint.x = newPoint.y = newPoint.z = bad_point;
}
cloud_arg.push_back (newPoint);
}
}
};
// Colored point cloud specialization
template <typename PointT>
struct OrganizedConversion<PointT, true>
{
/** \brief Convert point cloud to disparity image and rgb image
* \param[in] cloud_arg input point cloud
* \param[in] focalLength_arg focal length
* \param[in] disparityShift_arg disparity shift
* \param[in] disparityScale_arg disparity scaling
* \param[in] convertToMono convert color to mono/grayscale
* \param[out] disparityData_arg output disparity image
* \param[out] rgbData_arg output rgb image
* \ingroup io
*/
static void convert(const pcl::PointCloud<PointT>& cloud_arg,
float focalLength_arg,
float disparityShift_arg,
float disparityScale_arg,
bool convertToMono,
typename std::vector<std::uint16_t>& disparityData_arg,
typename std::vector<std::uint8_t>& rgbData_arg)
{
const auto cloud_size = cloud_arg.size ();
// Reset output vectors
disparityData_arg.clear ();
rgbData_arg.clear ();
// Allocate memory
disparityData_arg.reserve (cloud_size);
if (convertToMono)
{
rgbData_arg.reserve (cloud_size);
} else
{
rgbData_arg.reserve (cloud_size * 3);
}
for (std::size_t i = 0; i < cloud_size; ++i)
{
const PointT& point = cloud_arg[i];
if (pcl::isFinite (point))
{
if (convertToMono)
{
// Encode point color
auto grayvalue = static_cast<std::uint8_t>(0.2989 * point.r
+ 0.5870 * point.g
+ 0.1140 * point.b);
rgbData_arg.push_back (grayvalue);
} else
{
// Encode point color
rgbData_arg.push_back (point.r);
rgbData_arg.push_back (point.g);
rgbData_arg.push_back (point.b);
}
// Inverse depth quantization
auto disparity = static_cast<std::uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
// Encode disparity
disparityData_arg.push_back (disparity);
}
else
{
// Encode black point
if (convertToMono)
{
rgbData_arg.push_back (0);
} else
{
rgbData_arg.push_back (0);
rgbData_arg.push_back (0);
rgbData_arg.push_back (0);
}
// Encode bad point
disparityData_arg.push_back (0);
}
}
}
/** \brief Convert disparity image to point cloud
* \param[in] disparityData_arg output disparity image
* \param[in] rgbData_arg output rgb image
* \param[in] monoImage_arg input image is a single-channel mono image
* \param[in] width_arg width of disparity image
* \param[in] height_arg height of disparity image
* \param[in] focalLength_arg focal length
* \param[in] disparityShift_arg disparity shift
* \param[in] disparityScale_arg disparity scaling
* \param[out] cloud_arg output point cloud
* \ingroup io
*/
static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
typename std::vector<std::uint8_t>& rgbData_arg,
bool monoImage_arg,
std::size_t width_arg,
std::size_t height_arg,
float focalLength_arg,
float disparityShift_arg,
float disparityScale_arg,
pcl::PointCloud<PointT>& cloud_arg)
{
std::size_t cloud_size = width_arg*height_arg;
bool hasColor = (!rgbData_arg.empty ());
// Check size of input data
assert (disparityData_arg.size()==cloud_size);
if (hasColor)
{
if (monoImage_arg)
{
assert (rgbData_arg.size()==cloud_size);
} else
{
assert (rgbData_arg.size()==cloud_size*3);
}
}
// Reset point cloud
cloud_arg.clear();
cloud_arg.reserve(cloud_size);
// Define point cloud parameters
cloud_arg.width = static_cast<std::uint32_t>(width_arg);
cloud_arg.height = static_cast<std::uint32_t>(height_arg);
cloud_arg.is_dense = false;
// Calculate center of disparity image
int centerX = static_cast<int>(width_arg/2);
int centerY = static_cast<int>(height_arg/2);
const float fl_const = 1.0f/focalLength_arg;
static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
std::size_t i = 0;
for (int y = -centerY; y < centerY; ++y )
for (int x = -centerX; x < centerX; ++x )
{
PointT newPoint;
const std::uint16_t& pixel_disparity = disparityData_arg[i];
if (pixel_disparity && (pixel_disparity!=0x7FF))
{
float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
// Define point location
newPoint.z = depth;
newPoint.x = static_cast<float> (x) * depth * fl_const;
newPoint.y = static_cast<float> (y) * depth * fl_const;
if (hasColor)
{
if (monoImage_arg)
{
// Define point color
newPoint.r = rgbData_arg[i];
newPoint.g = rgbData_arg[i];
newPoint.b = rgbData_arg[i];
} else
{
// Define point color
newPoint.r = rgbData_arg[i*3+0];
newPoint.g = rgbData_arg[i*3+1];
newPoint.b = rgbData_arg[i*3+2];
}
} else
{
// Set white point color
newPoint.rgba = 0xffffffffu;
}
} else
{
// Define bad point
newPoint.x = newPoint.y = newPoint.z = bad_point;
newPoint.rgb = 0.0f;
}
// Add point to cloud
cloud_arg.push_back(newPoint);
// Increment point iterator
++i;
}
}
/** \brief Convert disparity image to point cloud
* \param[in] depthData_arg output disparity image
* \param[in] rgbData_arg output rgb image
* \param[in] monoImage_arg input image is a single-channel mono image
* \param[in] width_arg width of disparity image
* \param[in] height_arg height of disparity image
* \param[in] focalLength_arg focal length
* \param[out] cloud_arg output point cloud
* \ingroup io
*/
static void convert(typename std::vector<float>& depthData_arg,
typename std::vector<std::uint8_t>& rgbData_arg,
bool monoImage_arg,
std::size_t width_arg,
std::size_t height_arg,
float focalLength_arg,
pcl::PointCloud<PointT>& cloud_arg)
{
std::size_t cloud_size = width_arg*height_arg;
bool hasColor = (!rgbData_arg.empty ());
// Check size of input data
assert (depthData_arg.size()==cloud_size);
if (hasColor)
{
if (monoImage_arg)
{
assert (rgbData_arg.size()==cloud_size);
} else
{
assert (rgbData_arg.size()==cloud_size*3);
}
}
// Reset point cloud
cloud_arg.clear();
cloud_arg.reserve(cloud_size);
// Define point cloud parameters
cloud_arg.width = static_cast<std::uint32_t>(width_arg);
cloud_arg.height = static_cast<std::uint32_t>(height_arg);
cloud_arg.is_dense = false;
// Calculate center of disparity image
int centerX = static_cast<int>(width_arg/2);
int centerY = static_cast<int>(height_arg/2);
const float fl_const = 1.0f/focalLength_arg;
static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
std::size_t i = 0;
for (int y = -centerY; y < centerY; ++y )
for (int x = -centerX; x < centerX; ++x )
{
PointT newPoint;
const float& pixel_depth = depthData_arg[i];
if (pixel_depth)
{
// Define point location
newPoint.z = pixel_depth;
newPoint.x = static_cast<float> (x) * pixel_depth * fl_const;
newPoint.y = static_cast<float> (y) * pixel_depth * fl_const;
if (hasColor)
{
if (monoImage_arg)
{
// Define point color
newPoint.r = rgbData_arg[i];
newPoint.g = rgbData_arg[i];
newPoint.b = rgbData_arg[i];
} else
{
// Define point color
newPoint.r = rgbData_arg[i*3+0];
newPoint.g = rgbData_arg[i*3+1];
newPoint.b = rgbData_arg[i*3+2];
}
} else
{
// Set white point color
newPoint.rgba = 0xffffffffu;
}
} else
{
// Define bad point
newPoint.x = newPoint.y = newPoint.z = bad_point;
newPoint.rgb = 0.0f;
}
// Add point to cloud
cloud_arg.push_back(newPoint);
// Increment point iterator
++i;
}
}
};
} // namespace io
} // namespace pcl
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:174
pcl::PointCloud::push_back
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition point_cloud.h:664
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition point_cloud.h:404
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition point_cloud.h:399
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition point_cloud.h:401
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition point_cloud.h:886
pcl::PointCloud::size
std::size_t size() const
Definition point_cloud.h:444
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition point_cloud.h:446
pcl::io::OrganizedConversion< PointT, false >::convert
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< std::uint8_t > &, bool, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
Definition organized_pointcloud_conversion.h:204
pcl::io::OrganizedConversion< PointT, true >::convert
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool convertToMono, typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &rgbData_arg)
Convert point cloud to disparity image and rgb image.
Definition organized_pointcloud_conversion.h:273
pcl::io::OrganizedConversion< PointT, false >::convert
static void convert(typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &, bool, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
Definition organized_pointcloud_conversion.h:137
pcl::io::OrganizedConversion< PointT, true >::convert
static void convert(typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &rgbData_arg, bool monoImage_arg, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
Definition organized_pointcloud_conversion.h:356
pcl::io::OrganizedConversion< PointT, false >::convert
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool, typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &)
Convert point cloud to disparity image.
Definition organized_pointcloud_conversion.h:93
pcl::io::OrganizedConversion< PointT, true >::convert
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< std::uint8_t > &rgbData_arg, bool monoImage_arg, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
Definition organized_pointcloud_conversion.h:460
pcl::io
Definition io.h:517
pcl
Definition convolution.h:46
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_macros.h
Defines all the PCL and non-PCL macros used.
pcl::PointXYZRGBA
A point structure representing Euclidean xyz coordinates, and the RGBA color.
Definition point_types.hpp:528
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:594
pcl::io::CompressionPointTraits< PointXYZRGB >::bytesPerPoint
static const std::size_t bytesPerPoint
Definition organized_pointcloud_conversion.h:67
pcl::io::CompressionPointTraits< PointXYZRGB >::hasColor
static const bool hasColor
Definition organized_pointcloud_conversion.h:65
pcl::io::CompressionPointTraits< PointXYZRGB >::channels
static const unsigned int channels
Definition organized_pointcloud_conversion.h:66
pcl::io::CompressionPointTraits< PointXYZRGBA >::bytesPerPoint
static const std::size_t bytesPerPoint
Definition organized_pointcloud_conversion.h:75
pcl::io::CompressionPointTraits< PointXYZRGBA >::channels
static const unsigned int channels
Definition organized_pointcloud_conversion.h:74
pcl::io::CompressionPointTraits< PointXYZRGBA >::hasColor
static const bool hasColor
Definition organized_pointcloud_conversion.h:73
pcl::io::CompressionPointTraits
Definition organized_pointcloud_conversion.h:56
pcl::io::CompressionPointTraits::channels
static const unsigned int channels
Definition organized_pointcloud_conversion.h:58
pcl::io::CompressionPointTraits::hasColor
static const bool hasColor
Definition organized_pointcloud_conversion.h:57
pcl::io::CompressionPointTraits::bytesPerPoint
static const std::size_t bytesPerPoint
Definition organized_pointcloud_conversion.h:59
pcl::io::OrganizedConversion
Definition organized_pointcloud_conversion.h:79