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