Point Cloud Library (PCL)  1.11.1-dev
disparity_map_converter.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
37 #ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
38 #define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
39 
40 #include <pcl/common/intensity.h>
41 #include <pcl/console/print.h>
42 #include <pcl/stereo/disparity_map_converter.h>
43 #include <pcl/point_types.h>
44 
45 #include <fstream>
46 #include <limits>
47 
48 template <typename PointT>
50 : center_x_(0.0f)
51 , center_y_(0.0f)
52 , focal_length_(0.0f)
53 , baseline_(0.0f)
54 , is_color_(false)
55 , disparity_map_width_(640)
56 , disparity_map_height_(480)
57 , disparity_threshold_min_(0.0f)
58 , disparity_threshold_max_(std::numeric_limits<float>::max())
59 {}
60 
61 template <typename PointT>
63 {}
64 
65 template <typename PointT>
66 inline void
68 {
69  center_x_ = center_x;
70 }
71 
72 template <typename PointT>
73 inline float
75 {
76  return center_x_;
77 }
78 
79 template <typename PointT>
80 inline void
82 {
83  center_y_ = center_y;
84 }
85 
86 template <typename PointT>
87 inline float
89 {
90  return center_y_;
91 }
92 
93 template <typename PointT>
94 inline void
96 {
97  focal_length_ = focal_length;
98 }
99 
100 template <typename PointT>
101 inline float
103 {
104  return focal_length_;
105 }
106 
107 template <typename PointT>
108 inline void
110 {
111  baseline_ = baseline;
112 }
113 
114 template <typename PointT>
115 inline float
117 {
118  return baseline_;
119 }
120 
121 template <typename PointT>
122 inline void
124  const float disparity_threshold_min)
125 {
126  disparity_threshold_min_ = disparity_threshold_min;
127 }
128 
129 template <typename PointT>
130 inline float
132 {
133  return disparity_threshold_min_;
134 }
135 
136 template <typename PointT>
137 inline void
139  const float disparity_threshold_max)
140 {
141  disparity_threshold_max_ = disparity_threshold_max;
142 }
143 
144 template <typename PointT>
145 inline float
147 {
148  return disparity_threshold_max_;
149 }
150 
151 template <typename PointT>
152 void
155 {
156  image_ = image;
157 
158  // Set disparity map's size same with the image size.
159  disparity_map_width_ = image_->width;
160  disparity_map_height_ = image_->height;
161 
162  is_color_ = true;
163 }
164 
165 template <typename PointT>
168 {
170  *image_pointer = *image_;
171  return image_pointer;
172 }
173 
174 template <typename PointT>
175 bool
177 {
178  std::fstream disparity_file;
179 
180  // Open the disparity file
181  disparity_file.open(file_name.c_str(), std::fstream::in);
182  if (!disparity_file.is_open()) {
183  PCL_ERROR("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
184  return false;
185  }
186 
187  // Allocate memory for the disparity map.
188  disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
189 
190  // Reading the disparity map.
191  for (std::size_t row = 0; row < disparity_map_height_; ++row) {
192  for (std::size_t column = 0; column < disparity_map_width_; ++column) {
193  float disparity;
194  disparity_file >> disparity;
195 
196  disparity_map_[column + row * disparity_map_width_] = disparity;
197  } // column
198  } // row
199 
200  return true;
201 }
202 
203 template <typename PointT>
204 bool
206  const std::size_t width,
207  const std::size_t height)
208 {
209  // Initialize disparity map's size.
210  disparity_map_width_ = width;
211  disparity_map_height_ = height;
212 
213  // Load the disparity map.
214  return loadDisparityMap(file_name);
215 }
216 
217 template <typename PointT>
218 void
220  const std::vector<float>& disparity_map)
221 {
222  disparity_map_ = disparity_map;
223 }
224 
225 template <typename PointT>
226 void
228  const std::vector<float>& disparity_map,
229  const std::size_t width,
230  const std::size_t height)
231 {
232  disparity_map_width_ = width;
233  disparity_map_height_ = height;
234 
235  disparity_map_ = disparity_map;
236 }
237 
238 template <typename PointT>
239 std::vector<float>
241 {
242  return disparity_map_;
243 }
244 
245 template <typename PointT>
246 void
248 {
249  // Initialize the output cloud.
250  out_cloud.clear();
251  out_cloud.width = disparity_map_width_;
252  out_cloud.height = disparity_map_height_;
253  out_cloud.resize(out_cloud.width * out_cloud.height);
254 
255  if (is_color_ && !image_) {
256  PCL_ERROR("[pcl::DisparityMapConverter::compute] Memory for the image was not "
257  "allocated.\n");
258  return;
259  }
260 
261  for (std::size_t row = 0; row < disparity_map_height_; ++row) {
262  for (std::size_t column = 0; column < disparity_map_width_; ++column) {
263  // ID of current disparity point.
264  std::size_t disparity_point = column + row * disparity_map_width_;
265 
266  // Disparity value.
267  float disparity = disparity_map_[disparity_point];
268 
269  // New point for the output cloud.
270  PointT new_point;
271 
272  // Init color
273  if (is_color_) {
275  intensity_accessor.set(new_point,
276  static_cast<float>((*image_)[disparity_point].r +
277  (*image_)[disparity_point].g +
278  (*image_)[disparity_point].b) /
279  3.0f);
280  }
281 
282  // Init coordinates.
283  if (disparity_threshold_min_ < disparity &&
284  disparity < disparity_threshold_max_) {
285  // Compute new coordinates.
286  PointXYZ point_3D(translateCoordinates(row, column, disparity));
287  new_point.x = point_3D.x;
288  new_point.y = point_3D.y;
289  new_point.z = point_3D.z;
290  }
291  else {
292  new_point.x = std::numeric_limits<float>::quiet_NaN();
293  new_point.y = std::numeric_limits<float>::quiet_NaN();
294  new_point.z = std::numeric_limits<float>::quiet_NaN();
295  }
296  // Put the point to the output cloud.
297  out_cloud[disparity_point] = new_point;
298  } // column
299  } // row
300 }
301 
302 template <typename PointT>
305  std::size_t column,
306  float disparity) const
307 {
308  // Returning point.
309  PointXYZ point_3D;
310 
311  if (disparity != 0.0f) {
312  // Compute 3D-coordinates based on the image coordinates, the disparity and the
313  // camera parameters.
314  float z_value = focal_length_ * baseline_ / disparity;
315  point_3D.z = z_value;
316  point_3D.x = (static_cast<float>(column) - center_x_) * (z_value / focal_length_);
317  point_3D.y = (static_cast<float>(row) - center_y_) * (z_value / focal_length_);
318  }
319 
320  return point_3D;
321 }
322 
323 #define PCL_INSTANTIATE_DisparityMapConverter(T) \
324  template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
325 
326 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
point_types.h
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::DisparityMapConverter::setFocalLength
void setFocalLength(const float focal_length)
Set focal length.
Definition: disparity_map_converter.hpp:95
pcl::DisparityMapConverter::getFocalLength
float getFocalLength() const
Get focal length.
Definition: disparity_map_converter.hpp:102
pcl::DisparityMapConverter::setDisparityMap
void setDisparityMap(const std::vector< float > &disparity_map)
Set the disparity map.
Definition: disparity_map_converter.hpp:219
pcl::DisparityMapConverter::DisparityMapConverter
DisparityMapConverter()
DisparityMapConverter constructor.
Definition: disparity_map_converter.hpp:49
pcl::DisparityMapConverter::getDisparityMap
std::vector< float > getDisparityMap()
Get the disparity map.
Definition: disparity_map_converter.hpp:240
pcl::PointCloud< pcl::RGB >
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::DisparityMapConverter::getBaseline
float getBaseline() const
Get baseline.
Definition: disparity_map_converter.hpp:116
pcl::DisparityMapConverter::getDisparityThresholdMin
float getDisparityThresholdMin() const
Get min disparity threshold.
Definition: disparity_map_converter.hpp:131
pcl::DisparityMapConverter::getImageCenterY
float getImageCenterY() const
Get y-coordinate of the image center.
Definition: disparity_map_converter.hpp:88
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
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:153
pcl::DisparityMapConverter::setImageCenterX
void setImageCenterX(const float center_x)
Set x-coordinate of the image center.
Definition: disparity_map_converter.hpp:67
pcl::common::IntensityFieldAccessor::set
void set(PointT &p, float intensity) const
sets the intensity value of a point
Definition: intensity.h:75
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:300
pcl::DisparityMapConverter::loadDisparityMap
bool loadDisparityMap(const std::string &file_name)
Load the disparity map.
Definition: disparity_map_converter.hpp:176
pcl::DisparityMapConverter::getDisparityThresholdMax
float getDisparityThresholdMax() const
Get max disparity threshold.
Definition: disparity_map_converter.hpp:146
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
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:167
pcl::DisparityMapConverter::~DisparityMapConverter
virtual ~DisparityMapConverter()
Empty destructor.
Definition: disparity_map_converter.hpp:62
pcl::DisparityMapConverter::setImageCenterY
void setImageCenterY(const float center_y)
Set y-coordinate of the image center.
Definition: disparity_map_converter.hpp:81
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:864
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::DisparityMapConverter::getImageCenterX
float getImageCenterX() const
Get x-coordinate of the image center.
Definition: disparity_map_converter.hpp:74
pcl::DisparityMapConverter::setDisparityThresholdMax
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
Definition: disparity_map_converter.hpp:138
pcl::common::IntensityFieldAccessor
Definition: intensity.h:50
pcl::DisparityMapConverter::setBaseline
void setBaseline(const float baseline)
Set baseline.
Definition: disparity_map_converter.hpp:109
pcl::DisparityMapConverter::compute
virtual void compute(PointCloud &out_cloud)
Compute the output cloud.
Definition: disparity_map_converter.hpp:247
pcl::DisparityMapConverter::setDisparityThresholdMin
void setDisparityThresholdMin(const float disparity_threshold_min)
Set min disparity threshold.
Definition: disparity_map_converter.hpp:123
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:304