Loading...
Searching...
No Matches
disparity_map_converter.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2014-, Open Perception, 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 the copyright holder(s) 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 PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
#define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
#include <pcl/common/intensity.h>
#include <pcl/console/print.h>
#include <pcl/stereo/disparity_map_converter.h>
#include <pcl/point_types.h>
#include <fstream>
#include <limits>
template <typename PointT>
pcl::DisparityMapConverter<PointT>::DisparityMapConverter()
: disparity_threshold_max_(std::numeric_limits<float>::max())
{}
template <typename PointT>
pcl::DisparityMapConverter<PointT>::~DisparityMapConverter() = default;
template <typename PointT>
inline void
pcl::DisparityMapConverter<PointT>::setImageCenterX(const float center_x)
{
center_x_ = center_x;
}
template <typename PointT>
inline float
pcl::DisparityMapConverter<PointT>::getImageCenterX() const
{
return center_x_;
}
template <typename PointT>
inline void
pcl::DisparityMapConverter<PointT>::setImageCenterY(const float center_y)
{
center_y_ = center_y;
}
template <typename PointT>
inline float
pcl::DisparityMapConverter<PointT>::getImageCenterY() const
{
return center_y_;
}
template <typename PointT>
inline void
pcl::DisparityMapConverter<PointT>::setFocalLength(const float focal_length)
{
focal_length_ = focal_length;
}
template <typename PointT>
inline float
pcl::DisparityMapConverter<PointT>::getFocalLength() const
{
return focal_length_;
}
template <typename PointT>
inline void
pcl::DisparityMapConverter<PointT>::setBaseline(const float baseline)
{
baseline_ = baseline;
}
template <typename PointT>
inline float
pcl::DisparityMapConverter<PointT>::getBaseline() const
{
return baseline_;
}
template <typename PointT>
inline void
pcl::DisparityMapConverter<PointT>::setDisparityThresholdMin(
const float disparity_threshold_min)
{
disparity_threshold_min_ = disparity_threshold_min;
}
template <typename PointT>
inline float
pcl::DisparityMapConverter<PointT>::getDisparityThresholdMin() const
{
return disparity_threshold_min_;
}
template <typename PointT>
inline void
pcl::DisparityMapConverter<PointT>::setDisparityThresholdMax(
const float disparity_threshold_max)
{
disparity_threshold_max_ = disparity_threshold_max;
}
template <typename PointT>
inline float
pcl::DisparityMapConverter<PointT>::getDisparityThresholdMax() const
{
return disparity_threshold_max_;
}
template <typename PointT>
void
pcl::DisparityMapConverter<PointT>::setImage(
const pcl::PointCloud<pcl::RGB>::ConstPtr& image)
{
image_ = image;
// Set disparity map's size same with the image size.
disparity_map_width_ = image_->width;
disparity_map_height_ = image_->height;
is_color_ = true;
}
template <typename PointT>
pcl::PointCloud<pcl::RGB>::Ptr
pcl::DisparityMapConverter<PointT>::getImage()
{
pcl::PointCloud<pcl::RGB>::Ptr image_pointer(new pcl::PointCloud<pcl::RGB>);
*image_pointer = *image_;
return image_pointer;
}
template <typename PointT>
bool
pcl::DisparityMapConverter<PointT>::loadDisparityMap(const std::string& file_name)
{
std::fstream disparity_file;
// Open the disparity file
disparity_file.open(file_name.c_str(), std::fstream::in);
if (!disparity_file.is_open()) {
PCL_ERROR("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
return false;
}
// Allocate memory for the disparity map.
disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
// Reading the disparity map.
for (std::size_t row = 0; row < disparity_map_height_; ++row) {
for (std::size_t column = 0; column < disparity_map_width_; ++column) {
float disparity;
disparity_file >> disparity;
disparity_map_[column + row * disparity_map_width_] = disparity;
} // column
} // row
return true;
}
template <typename PointT>
bool
pcl::DisparityMapConverter<PointT>::loadDisparityMap(const std::string& file_name,
const std::size_t width,
const std::size_t height)
{
// Initialize disparity map's size.
disparity_map_width_ = width;
disparity_map_height_ = height;
// Load the disparity map.
return loadDisparityMap(file_name);
}
template <typename PointT>
void
pcl::DisparityMapConverter<PointT>::setDisparityMap(
const std::vector<float>& disparity_map)
{
disparity_map_ = disparity_map;
}
template <typename PointT>
void
pcl::DisparityMapConverter<PointT>::setDisparityMap(
const std::vector<float>& disparity_map,
const std::size_t width,
const std::size_t height)
{
disparity_map_width_ = width;
disparity_map_height_ = height;
disparity_map_ = disparity_map;
}
template <typename PointT>
std::vector<float>
pcl::DisparityMapConverter<PointT>::getDisparityMap()
{
return disparity_map_;
}
template <typename PointT>
void
pcl::DisparityMapConverter<PointT>::compute(PointCloud& out_cloud)
{
// Initialize the output cloud.
out_cloud.clear();
out_cloud.width = disparity_map_width_;
out_cloud.height = disparity_map_height_;
out_cloud.resize(out_cloud.width * out_cloud.height);
if (is_color_ && !image_) {
PCL_ERROR("[pcl::DisparityMapConverter::compute] Memory for the image was not "
"allocated.\n");
return;
}
for (std::size_t row = 0; row < disparity_map_height_; ++row) {
for (std::size_t column = 0; column < disparity_map_width_; ++column) {
// ID of current disparity point.
std::size_t disparity_point = column + row * disparity_map_width_;
// Disparity value.
float disparity = disparity_map_[disparity_point];
// New point for the output cloud.
PointT new_point;
// Init color
if (is_color_) {
pcl::common::IntensityFieldAccessor<PointT> intensity_accessor;
intensity_accessor.set(new_point,
static_cast<float>((*image_)[disparity_point].r +
(*image_)[disparity_point].g +
(*image_)[disparity_point].b) /
3.0f);
}
// Init coordinates.
if (disparity_threshold_min_ < disparity &&
disparity < disparity_threshold_max_) {
// Compute new coordinates.
PointXYZ point_3D(translateCoordinates(row, column, disparity));
new_point.x = point_3D.x;
new_point.y = point_3D.y;
new_point.z = point_3D.z;
}
else {
new_point.x = std::numeric_limits<float>::quiet_NaN();
new_point.y = std::numeric_limits<float>::quiet_NaN();
new_point.z = std::numeric_limits<float>::quiet_NaN();
}
// Put the point to the output cloud.
out_cloud[disparity_point] = new_point;
} // column
} // row
}
template <typename PointT>
pcl::PointXYZ
pcl::DisparityMapConverter<PointT>::translateCoordinates(std::size_t row,
std::size_t column,
float disparity) const
{
// Returning point.
PointXYZ point_3D;
if (disparity != 0.0f) {
// Compute 3D-coordinates based on the image coordinates, the disparity and the
// camera parameters.
float z_value = focal_length_ * baseline_ / disparity;
point_3D.z = z_value;
point_3D.x = (static_cast<float>(column) - center_x_) * (z_value / focal_length_);
point_3D.y = (static_cast<float>(row) - center_y_) * (z_value / focal_length_);
}
return point_3D;
}
#define PCL_INSTANTIATE_DisparityMapConverter(T) \
template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
#endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
pcl::DisparityMapConverter::getBaseline
float getBaseline() const
Get baseline.
Definition disparity_map_converter.hpp:107
pcl::DisparityMapConverter::getDisparityMap
std::vector< float > getDisparityMap()
Get the disparity map.
Definition disparity_map_converter.hpp:231
pcl::DisparityMapConverter::getImageCenterY
float getImageCenterY() const
Get y-coordinate of the image center.
Definition disparity_map_converter.hpp:79
pcl::DisparityMapConverter::setDisparityThresholdMax
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
Definition disparity_map_converter.hpp:129
pcl::DisparityMapConverter::getImageCenterX
float getImageCenterX() const
Get x-coordinate of the image center.
Definition disparity_map_converter.hpp:65
pcl::DisparityMapConverter::setImage
void setImage(const pcl::PointCloud< pcl::RGB >::ConstPtr &image)
Set an image, that will be used for coloring of the output cloud.
Definition disparity_map_converter.hpp:144
pcl::DisparityMapConverter::translateCoordinates
PointXYZ translateCoordinates(std::size_t row, std::size_t column, float disparity) const
Translate point from image coordinates and disparity to 3D-coordinates.
Definition disparity_map_converter.hpp:295
pcl::DisparityMapConverter::getImage
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.
Definition disparity_map_converter.hpp:158
pcl::DisparityMapConverter::compute
virtual void compute(PointCloud &out_cloud)
Compute the output cloud.
Definition disparity_map_converter.hpp:238
pcl::DisparityMapConverter::loadDisparityMap
bool loadDisparityMap(const std::string &file_name)
Load the disparity map.
Definition disparity_map_converter.hpp:167
pcl::DisparityMapConverter::setImageCenterX
void setImageCenterX(const float center_x)
Set x-coordinate of the image center.
Definition disparity_map_converter.hpp:58
pcl::DisparityMapConverter::~DisparityMapConverter
virtual ~DisparityMapConverter()
Empty destructor.
pcl::DisparityMapConverter::DisparityMapConverter
DisparityMapConverter()
DisparityMapConverter constructor.
Definition disparity_map_converter.hpp:49
pcl::DisparityMapConverter::setDisparityMap
void setDisparityMap(const std::vector< float > &disparity_map)
Set the disparity map.
Definition disparity_map_converter.hpp:210
pcl::DisparityMapConverter::setDisparityThresholdMin
void setDisparityThresholdMin(const float disparity_threshold_min)
Set min disparity threshold.
Definition disparity_map_converter.hpp:114
pcl::DisparityMapConverter::getDisparityThresholdMax
float getDisparityThresholdMax() const
Get max disparity threshold.
Definition disparity_map_converter.hpp:137
pcl::DisparityMapConverter::getDisparityThresholdMin
float getDisparityThresholdMin() const
Get min disparity threshold.
Definition disparity_map_converter.hpp:122
pcl::DisparityMapConverter::setBaseline
void setBaseline(const float baseline)
Set baseline.
Definition disparity_map_converter.hpp:100
pcl::DisparityMapConverter::getFocalLength
float getFocalLength() const
Get focal length.
Definition disparity_map_converter.hpp:93
pcl::DisparityMapConverter::setFocalLength
void setFocalLength(const float focal_length)
Set focal length.
Definition disparity_map_converter.hpp:86
pcl::DisparityMapConverter::setImageCenterY
void setImageCenterY(const float center_y)
Set y-coordinate of the image center.
Definition disparity_map_converter.hpp:72
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition point_cloud.h:462
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition point_cloud.h:398
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition point_cloud.h:400
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition point_cloud.h:885
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
point_types.h
Defines all the PCL implemented PointT point type structures.
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition point_types.hpp:351
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:623
pcl::common::IntensityFieldAccessor
Definition intensity.h:51
pcl::common::IntensityFieldAccessor::set
void set(PointT &p, float intensity) const
sets the intensity value of a point
Definition intensity.h:75