Point Cloud Library (PCL)  1.13.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 template <typename PointT>
65 inline void
67 {
68  center_x_ = center_x;
69 }
70 
71 template <typename PointT>
72 inline float
74 {
75  return center_x_;
76 }
77 
78 template <typename PointT>
79 inline void
81 {
82  center_y_ = center_y;
83 }
84 
85 template <typename PointT>
86 inline float
88 {
89  return center_y_;
90 }
91 
92 template <typename PointT>
93 inline void
95 {
96  focal_length_ = focal_length;
97 }
98 
99 template <typename PointT>
100 inline float
102 {
103  return focal_length_;
104 }
105 
106 template <typename PointT>
107 inline void
109 {
110  baseline_ = baseline;
111 }
112 
113 template <typename PointT>
114 inline float
116 {
117  return baseline_;
118 }
119 
120 template <typename PointT>
121 inline void
123  const float disparity_threshold_min)
124 {
125  disparity_threshold_min_ = disparity_threshold_min;
126 }
127 
128 template <typename PointT>
129 inline float
131 {
132  return disparity_threshold_min_;
133 }
134 
135 template <typename PointT>
136 inline void
138  const float disparity_threshold_max)
139 {
140  disparity_threshold_max_ = disparity_threshold_max;
141 }
142 
143 template <typename PointT>
144 inline float
146 {
147  return disparity_threshold_max_;
148 }
149 
150 template <typename PointT>
151 void
154 {
155  image_ = image;
156 
157  // Set disparity map's size same with the image size.
158  disparity_map_width_ = image_->width;
159  disparity_map_height_ = image_->height;
160 
161  is_color_ = true;
162 }
163 
164 template <typename PointT>
167 {
169  *image_pointer = *image_;
170  return image_pointer;
171 }
172 
173 template <typename PointT>
174 bool
176 {
177  std::fstream disparity_file;
178 
179  // Open the disparity file
180  disparity_file.open(file_name.c_str(), std::fstream::in);
181  if (!disparity_file.is_open()) {
182  PCL_ERROR("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
183  return false;
184  }
185 
186  // Allocate memory for the disparity map.
187  disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
188 
189  // Reading the disparity map.
190  for (std::size_t row = 0; row < disparity_map_height_; ++row) {
191  for (std::size_t column = 0; column < disparity_map_width_; ++column) {
192  float disparity;
193  disparity_file >> disparity;
194 
195  disparity_map_[column + row * disparity_map_width_] = disparity;
196  } // column
197  } // row
198 
199  return true;
200 }
201 
202 template <typename PointT>
203 bool
205  const std::size_t width,
206  const std::size_t height)
207 {
208  // Initialize disparity map's size.
209  disparity_map_width_ = width;
210  disparity_map_height_ = height;
211 
212  // Load the disparity map.
213  return loadDisparityMap(file_name);
214 }
215 
216 template <typename PointT>
217 void
219  const std::vector<float>& disparity_map)
220 {
221  disparity_map_ = disparity_map;
222 }
223 
224 template <typename PointT>
225 void
227  const std::vector<float>& disparity_map,
228  const std::size_t width,
229  const std::size_t height)
230 {
231  disparity_map_width_ = width;
232  disparity_map_height_ = height;
233 
234  disparity_map_ = disparity_map;
235 }
236 
237 template <typename PointT>
238 std::vector<float>
240 {
241  return disparity_map_;
242 }
243 
244 template <typename PointT>
245 void
247 {
248  // Initialize the output cloud.
249  out_cloud.clear();
250  out_cloud.width = disparity_map_width_;
251  out_cloud.height = disparity_map_height_;
252  out_cloud.resize(out_cloud.width * out_cloud.height);
253 
254  if (is_color_ && !image_) {
255  PCL_ERROR("[pcl::DisparityMapConverter::compute] Memory for the image was not "
256  "allocated.\n");
257  return;
258  }
259 
260  for (std::size_t row = 0; row < disparity_map_height_; ++row) {
261  for (std::size_t column = 0; column < disparity_map_width_; ++column) {
262  // ID of current disparity point.
263  std::size_t disparity_point = column + row * disparity_map_width_;
264 
265  // Disparity value.
266  float disparity = disparity_map_[disparity_point];
267 
268  // New point for the output cloud.
269  PointT new_point;
270 
271  // Init color
272  if (is_color_) {
274  intensity_accessor.set(new_point,
275  static_cast<float>((*image_)[disparity_point].r +
276  (*image_)[disparity_point].g +
277  (*image_)[disparity_point].b) /
278  3.0f);
279  }
280 
281  // Init coordinates.
282  if (disparity_threshold_min_ < disparity &&
283  disparity < disparity_threshold_max_) {
284  // Compute new coordinates.
285  PointXYZ point_3D(translateCoordinates(row, column, disparity));
286  new_point.x = point_3D.x;
287  new_point.y = point_3D.y;
288  new_point.z = point_3D.z;
289  }
290  else {
291  new_point.x = std::numeric_limits<float>::quiet_NaN();
292  new_point.y = std::numeric_limits<float>::quiet_NaN();
293  new_point.z = std::numeric_limits<float>::quiet_NaN();
294  }
295  // Put the point to the output cloud.
296  out_cloud[disparity_point] = new_point;
297  } // column
298  } // row
299 }
300 
301 template <typename PointT>
304  std::size_t column,
305  float disparity) const
306 {
307  // Returning point.
308  PointXYZ point_3D;
309 
310  if (disparity != 0.0f) {
311  // Compute 3D-coordinates based on the image coordinates, the disparity and the
312  // camera parameters.
313  float z_value = focal_length_ * baseline_ / disparity;
314  point_3D.z = z_value;
315  point_3D.x = (static_cast<float>(column) - center_x_) * (z_value / focal_length_);
316  point_3D.y = (static_cast<float>(row) - center_y_) * (z_value / focal_length_);
317  }
318 
319  return point_3D;
320 }
321 
322 #define PCL_INSTANTIATE_DisparityMapConverter(T) \
323  template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
324 
325 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
float getBaseline() const
Get baseline.
std::vector< float > getDisparityMap()
Get the disparity map.
float getImageCenterY() const
Get y-coordinate of the image center.
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
float getImageCenterX() const
Get x-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.
PointXYZ translateCoordinates(std::size_t row, std::size_t column, float disparity) const
Translate point from image coordinates and disparity to 3D-coordinates.
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.
virtual void compute(PointCloud &out_cloud)
Compute the output cloud.
bool loadDisparityMap(const std::string &file_name)
Load the disparity map.
void setImageCenterX(const float center_x)
Set x-coordinate of the image center.
virtual ~DisparityMapConverter()
Empty destructor.
DisparityMapConverter()
DisparityMapConverter constructor.
void setDisparityMap(const std::vector< float > &disparity_map)
Set the disparity map.
void setDisparityThresholdMin(const float disparity_threshold_min)
Set min disparity threshold.
float getDisparityThresholdMax() const
Get max disparity threshold.
float getDisparityThresholdMin() const
Get min disparity threshold.
void setBaseline(const float baseline)
Set baseline.
float getFocalLength() const
Get focal length.
void setFocalLength(const float focal_length)
Set focal length.
void setImageCenterY(const float center_y)
Set y-coordinate of the image center.
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Defines all the PCL implemented PointT point type structures.
A point structure representing Euclidean xyz coordinates.
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