Point Cloud Library (PCL)  1.9.0-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 #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 {
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 {
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.
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_
bool is_color_
Is color image is set.
void setFocalLength(const float focal_length)
Set focal length.
PointXYZ translateCoordinates(size_t row, size_t column, float disparity) const
Translate point from image coordinates and disparity to 3D-coordinates.
float getDisparityThresholdMin() const
Get min disparity threshold.
float getFocalLength() const
Get focal length.
bool loadDisparityMap(const std::string &file_name)
Load the disparity map.
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.
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
float getDisparityThresholdMax() const
Get max disparity threshold.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::PointCloud< pcl::RGB >::ConstPtr image_
Color image of the scene.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
float getImageCenterX() const
Get x-coordinate of the image center.
virtual ~DisparityMapConverter()
Empty destructor.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
size_t disparity_map_width_
X-size of the disparity map.
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:576
void set(PointT &p, float intensity) const
sets the intensity value of a point
Definition: intensity.h:76
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float getBaseline() const
Get baseline.
float center_x_
X-coordinate of the image center.
void setBaseline(const float baseline)
Set baseline.
float getImageCenterY() const
Get y-coordinate of the image center.
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:455
void setImageCenterX(const float center_x)
Set x-coordinate of the image center.
A point structure representing Euclidean xyz coordinates, and the RGB color.
std::vector< float > disparity_map_
Vector for the disparity map.
size_t disparity_map_height_
Y-size of the disparity map.
float disparity_threshold_min_
Thresholds of the disparity.
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.
float center_y_
Y-coordinate of the image center.