Point Cloud Library (PCL)  1.8.0
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 #ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
37 #define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
38 
39 #include <pcl/stereo/disparity_map_converter.h>
40 
41 #include <fstream>
42 #include <limits>
43 
44 #include <pcl/point_types.h>
45 #include <pcl/console/print.h>
46 #include <pcl/common/intensity.h>
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 
62 template <typename PointT>
64 {
65 }
66 
67 template <typename PointT> inline void
69 {
70  center_x_ = center_x;
71 }
72 
73 template <typename PointT> inline float
75 {
76  return center_x_;
77 }
78 
79 template <typename PointT> inline void
81 {
82  center_y_ = center_y;
83 }
84 
85 template <typename PointT> inline float
87 {
88  return center_y_;
89 }
90 
91 template <typename PointT> inline void
93 {
94  focal_length_ = focal_length;
95 }
96 
97 template <typename PointT> inline float
99 {
100  return focal_length_;
101 }
102 
103 template <typename PointT> inline void
105 {
106  baseline_ = baseline;
107 }
108 
109 template <typename PointT> inline float
111 {
112  return baseline_;
113 }
114 
115 template <typename PointT> inline void
117  const float disparity_threshold_min)
118 {
119  disparity_threshold_min_ = disparity_threshold_min;
120 }
121 
122 template <typename PointT> inline float
124 {
125  return disparity_threshold_min_;
126 }
127 
128 template <typename PointT> inline void
130  const float disparity_threshold_max)
131 {
132  disparity_threshold_max_ = disparity_threshold_max;
133 }
134 
135 template <typename PointT> inline float
137 {
138  return disparity_threshold_max_;
139 }
140 
141 template <typename PointT> void
144 {
145  image_ = image;
146 
147  // Set disparity map's size same with the image size.
148  disparity_map_width_ = image_->width;
149  disparity_map_height_ = image_->height;
150 
151  is_color_ = true;
152 }
153 
154 template <typename PointT> pcl::PointCloud<pcl::RGB>::Ptr
156 {
158  *image_pointer = *image_;
159  return image_pointer;
160 }
161 
162 template <typename PointT> bool
164  const std::string &file_name)
165 {
166  std::fstream disparity_file;
167 
168  // Open the disparity file
169  disparity_file.open (file_name.c_str (), std::fstream::in);
170  if (!disparity_file.is_open ())
171  {
172  PCL_ERROR ("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
173  return false;
174  }
175 
176  // Allocate memory for the disparity map.
177  disparity_map_.resize (disparity_map_width_ * disparity_map_height_);
178 
179  // Reading the disparity map.
180  for (size_t row = 0; row < disparity_map_height_; ++row)
181  {
182  for (size_t column = 0; column < disparity_map_width_; ++column)
183  {
184  float disparity;
185  disparity_file >> disparity;
186 
187  disparity_map_[column + row * disparity_map_width_] = disparity;
188  } // column
189  } // row
190 
191  return true;
192 }
193 
194 template <typename PointT> bool
196  const std::string &file_name, const size_t width, const size_t height)
197 {
198  // Initialize disparity map's size.
199  disparity_map_width_ = width;
200  disparity_map_height_ = height;
201 
202  // Load the disparity map.
203  return loadDisparityMap (file_name);
204 }
205 
206 template <typename PointT> void
208  const std::vector<float> &disparity_map)
209 {
210  disparity_map_ = disparity_map;
211 }
212 
213 template <typename PointT> void
215  const std::vector<float> &disparity_map,
216  const size_t width, const size_t height)
217 {
218  disparity_map_width_ = width;
219  disparity_map_height_ = height;
220 
221  disparity_map_ = disparity_map;
222 }
223 
224 template <typename PointT> std::vector<float>
226 {
227  return disparity_map_;
228 }
229 
230 template <typename PointT> void
232 {
233  // Initialize the output cloud.
234  out_cloud.clear ();
235  out_cloud.width = disparity_map_width_;
236  out_cloud.height = disparity_map_height_;
237  out_cloud.resize (out_cloud.width * out_cloud.height);
238 
239  if (is_color_ && !image_)
240  {
241  PCL_ERROR ("[pcl::DisparityMapConverter::compute] Memory for the image was not allocated.\n");
242  return;
243  }
244 
245  for (size_t row = 0; row < disparity_map_height_; ++row)
246  {
247  for (size_t column = 0; column < disparity_map_width_; ++column)
248  {
249  // ID of current disparity point.
250  size_t disparity_point = column + row * disparity_map_width_;
251 
252  // Disparity value.
253  float disparity = disparity_map_[disparity_point];
254 
255  // New point for the output cloud.
256  PointT new_point;
257 
258  // Init color
259  if (is_color_)
260  {
262  intensity_accessor.set (new_point, static_cast<float> (
263  image_->points[disparity_point].r +
264  image_->points[disparity_point].g +
265  image_->points[disparity_point].b) / 3.0f);
266  }
267 
268  // Init coordinates.
269  if (disparity_threshold_min_ < disparity && disparity < disparity_threshold_max_)
270  {
271  // Compute new coordinates.
272  PointXYZ point_3D (translateCoordinates (row, column, disparity));
273  new_point.x = point_3D.x;
274  new_point.y = point_3D.y;
275  new_point.z = point_3D.z;
276  }
277  else
278  {
279  new_point.x = std::numeric_limits<float>::quiet_NaN ();
280  new_point.y = std::numeric_limits<float>::quiet_NaN ();
281  new_point.z = std::numeric_limits<float>::quiet_NaN ();
282  }
283  // Put the point to the output cloud.
284  out_cloud[disparity_point] = new_point;
285  } // column
286  } // row
287 }
288 
289 template <typename PointT> pcl::PointXYZ
291  size_t row, size_t column, float disparity) const
292 {
293  // Returning point.
294  PointXYZ point_3D;
295 
296  if (disparity != 0.0f)
297  {
298  // Compute 3D-coordinates based on the image coordinates, the disparity and the camera parameters.
299  float z_value = focal_length_ * baseline_ / disparity;
300  point_3D.z = z_value;
301  point_3D.x = (static_cast<float> (column) - center_x_) * (z_value / focal_length_);
302  point_3D.y = (static_cast<float> (row) - center_y_) * (z_value / focal_length_);
303  }
304 
305  return point_3D;
306 }
307 
308 #define PCL_INSTANTIATE_DisparityMapConverter(T) template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
309 
310 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
void setFocalLength(const float focal_length)
Set focal length.
float getImageCenterX() const
Get x-coordinate of the image center.
float getBaseline() const
Get baseline.
float getFocalLength() const
Get focal length.
bool loadDisparityMap(const std::string &file_name)
Load the disparity map.
float getDisparityThresholdMin() const
Get min disparity threshold.
void setDisparityMap(const std::vector< float > &disparity_map)
Set the disparity map.
virtual void compute(PointCloud &out_cloud)
Compute the output cloud.
DisparityMapConverter()
DisparityMapConverter constructor.
std::vector< float > getDisparityMap()
Get the disparity map.
PointXYZ translateCoordinates(size_t row, size_t column, float disparity) const
Translate point from image coordinates and disparity to 3D-coordinates.
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
virtual ~DisparityMapConverter()
Empty destructor.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
Defines all the PCL implemented PointT point type structures.
void setDisparityThresholdMin(const float disparity_threshold_min)
Set min disparity threshold.
A point structure representing Euclidean xyz coordinates.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:567
float getImageCenterY() const
Get y-coordinate of the image center.
float getDisparityThresholdMax() const
Get max disparity threshold.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setBaseline(const float baseline)
Set baseline.
void setImageCenterY(const float center_y)
Set y-coordinate of the image center.
void setImage(const pcl::PointCloud< pcl::RGB >::ConstPtr &image)
Set an image, that will be used for coloring of the output cloud.
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
void setImageCenterX(const float center_x)
Set x-coordinate of the image center.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void set(PointT &p, float intensity) const
sets the intensity value of a point
Definition: intensity.h:76
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.