Point Cloud Library (PCL)  1.9.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/stereo/disparity_map_converter.h>
41 
42 #include <fstream>
43 #include <limits>
44 
45 #include <pcl/common/intensity.h>
46 #include <pcl/console/print.h>
47 #include <pcl/point_types.h>
48 
49 template <typename PointT>
51 : center_x_(0.0f)
52 , center_y_(0.0f)
53 , focal_length_(0.0f)
54 , baseline_(0.0f)
55 , is_color_(false)
56 , disparity_map_width_(640)
57 , disparity_map_height_(480)
58 , disparity_threshold_min_(0.0f)
59 , disparity_threshold_max_(std::numeric_limits<float>::max())
60 {}
61 
62 template <typename PointT>
64 {}
65 
66 template <typename PointT>
67 inline void
69 {
70  center_x_ = center_x;
71 }
72 
73 template <typename PointT>
74 inline float
76 {
77  return center_x_;
78 }
79 
80 template <typename PointT>
81 inline void
83 {
84  center_y_ = center_y;
85 }
86 
87 template <typename PointT>
88 inline float
90 {
91  return center_y_;
92 }
93 
94 template <typename PointT>
95 inline void
97 {
98  focal_length_ = focal_length;
99 }
100 
101 template <typename PointT>
102 inline float
104 {
105  return focal_length_;
106 }
107 
108 template <typename PointT>
109 inline void
111 {
112  baseline_ = baseline;
113 }
114 
115 template <typename PointT>
116 inline float
118 {
119  return baseline_;
120 }
121 
122 template <typename PointT>
123 inline void
125  const float disparity_threshold_min)
126 {
127  disparity_threshold_min_ = disparity_threshold_min;
128 }
129 
130 template <typename PointT>
131 inline float
133 {
134  return disparity_threshold_min_;
135 }
136 
137 template <typename PointT>
138 inline void
140  const float disparity_threshold_max)
141 {
142  disparity_threshold_max_ = disparity_threshold_max;
143 }
144 
145 template <typename PointT>
146 inline float
148 {
149  return disparity_threshold_max_;
150 }
151 
152 template <typename PointT>
153 void
156 {
157  image_ = image;
158 
159  // Set disparity map's size same with the image size.
160  disparity_map_width_ = image_->width;
161  disparity_map_height_ = image_->height;
162 
163  is_color_ = true;
164 }
165 
166 template <typename PointT>
169 {
171  *image_pointer = *image_;
172  return image_pointer;
173 }
174 
175 template <typename PointT>
176 bool
178 {
179  std::fstream disparity_file;
180 
181  // Open the disparity file
182  disparity_file.open(file_name.c_str(), std::fstream::in);
183  if (!disparity_file.is_open()) {
184  PCL_ERROR("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
185  return false;
186  }
187 
188  // Allocate memory for the disparity map.
189  disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
190 
191  // Reading the disparity map.
192  for (std::size_t row = 0; row < disparity_map_height_; ++row) {
193  for (std::size_t column = 0; column < disparity_map_width_; ++column) {
194  float disparity;
195  disparity_file >> disparity;
196 
197  disparity_map_[column + row * disparity_map_width_] = disparity;
198  } // column
199  } // row
200 
201  return true;
202 }
203 
204 template <typename PointT>
205 bool
207  const std::size_t width,
208  const std::size_t height)
209 {
210  // Initialize disparity map's size.
211  disparity_map_width_ = width;
212  disparity_map_height_ = height;
213 
214  // Load the disparity map.
215  return loadDisparityMap(file_name);
216 }
217 
218 template <typename PointT>
219 void
221  const std::vector<float>& disparity_map)
222 {
223  disparity_map_ = disparity_map;
224 }
225 
226 template <typename PointT>
227 void
229  const std::vector<float>& disparity_map,
230  const std::size_t width,
231  const std::size_t height)
232 {
233  disparity_map_width_ = width;
234  disparity_map_height_ = height;
235 
236  disparity_map_ = disparity_map;
237 }
238 
239 template <typename PointT>
240 std::vector<float>
242 {
243  return disparity_map_;
244 }
245 
246 template <typename PointT>
247 void
249 {
250  // Initialize the output cloud.
251  out_cloud.clear();
252  out_cloud.width = disparity_map_width_;
253  out_cloud.height = disparity_map_height_;
254  out_cloud.resize(out_cloud.width * out_cloud.height);
255 
256  if (is_color_ && !image_) {
257  PCL_ERROR("[pcl::DisparityMapConverter::compute] Memory for the image was not "
258  "allocated.\n");
259  return;
260  }
261 
262  for (std::size_t row = 0; row < disparity_map_height_; ++row) {
263  for (std::size_t column = 0; column < disparity_map_width_; ++column) {
264  // ID of current disparity point.
265  std::size_t disparity_point = column + row * disparity_map_width_;
266 
267  // Disparity value.
268  float disparity = disparity_map_[disparity_point];
269 
270  // New point for the output cloud.
271  PointT new_point;
272 
273  // Init color
274  if (is_color_) {
276  intensity_accessor.set(new_point,
277  static_cast<float>(image_->points[disparity_point].r +
278  image_->points[disparity_point].g +
279  image_->points[disparity_point].b) /
280  3.0f);
281  }
282 
283  // Init coordinates.
284  if (disparity_threshold_min_ < disparity &&
285  disparity < disparity_threshold_max_) {
286  // Compute new coordinates.
287  PointXYZ point_3D(translateCoordinates(row, column, disparity));
288  new_point.x = point_3D.x;
289  new_point.y = point_3D.y;
290  new_point.z = point_3D.z;
291  }
292  else {
293  new_point.x = std::numeric_limits<float>::quiet_NaN();
294  new_point.y = std::numeric_limits<float>::quiet_NaN();
295  new_point.z = std::numeric_limits<float>::quiet_NaN();
296  }
297  // Put the point to the output cloud.
298  out_cloud[disparity_point] = new_point;
299  } // column
300  } // row
301 }
302 
303 template <typename PointT>
306  std::size_t column,
307  float disparity) const
308 {
309  // Returning point.
310  PointXYZ point_3D;
311 
312  if (disparity != 0.0f) {
313  // Compute 3D-coordinates based on the image coordinates, the disparity and the
314  // camera parameters.
315  float z_value = focal_length_ * baseline_ / disparity;
316  point_3D.z = z_value;
317  point_3D.x = (static_cast<float>(column) - center_x_) * (z_value / focal_length_);
318  point_3D.y = (static_cast<float>(row) - center_y_) * (z_value / focal_length_);
319  }
320 
321  return point_3D;
322 }
323 
324 #define PCL_INSTANTIATE_DisparityMapConverter(T) \
325  template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
326 
327 #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.
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:439
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.
PointXYZ translateCoordinates(std::size_t row, std::size_t column, float disparity) const
Translate point from image coordinates and disparity to 3D-coordinates.
std::vector< float > getDisparityMap()
Get the disparity map.
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:397
virtual ~DisparityMapConverter()
Empty destructor.
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< PointCloud< PointT > > Ptr
Definition: point_cloud.h:412
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:589
float getImageCenterY() const
Get y-coordinate of the image center.
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:399
float getDisparityThresholdMax() const
Get max disparity threshold.
void setBaseline(const float baseline)
Set baseline.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:413
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 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:75
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.