Point Cloud Library (PCL)  1.12.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 
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_
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:874
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