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