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
48template <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
61template <typename PointT>
63{}
64
65template <typename PointT>
66inline void
68{
69 center_x_ = center_x;
70}
71
72template <typename PointT>
73inline float
75{
76 return center_x_;
77}
78
79template <typename PointT>
80inline void
82{
83 center_y_ = center_y;
84}
85
86template <typename PointT>
87inline float
89{
90 return center_y_;
91}
93template <typename PointT>
94inline void
96{
97 focal_length_ = focal_length;
99
100template <typename PointT>
101inline float
103{
104 return focal_length_;
105}
106
107template <typename PointT>
108inline void
111 baseline_ = baseline;
112}
113
114template <typename PointT>
115inline float
117{
118 return baseline_;
119}
120
121template <typename PointT>
122inline void
124 const float disparity_threshold_min)
125{
126 disparity_threshold_min_ = disparity_threshold_min;
127}
129template <typename PointT>
130inline float
132{
133 return disparity_threshold_min_;
135
136template <typename PointT>
137inline void
139 const float disparity_threshold_max)
141 disparity_threshold_max_ = disparity_threshold_max;
142}
143
144template <typename PointT>
145inline float
147{
148 return disparity_threshold_max_;
149}
150
151template <typename PointT>
152void
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}
165template <typename PointT>
168{
170 *image_pointer = *image_;
171 return image_pointer;
172}
173
174template <typename PointT>
175bool
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 }
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
203template <typename PointT>
204bool
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
217template <typename PointT>
218void
220 const std::vector<float>& disparity_map)
221{
222 disparity_map_ = disparity_map;
223}
224
225template <typename PointT>
226void
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
238template <typename PointT>
239std::vector<float>
241{
242 return disparity_map_;
243}
244
245template <typename PointT>
246void
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
302template <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