Point Cloud Library (PCL) 1.13.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
64template <typename PointT>
65inline void
67{
68 center_x_ = center_x;
69}
70
71template <typename PointT>
72inline float
74{
75 return center_x_;
76}
77
78template <typename PointT>
79inline void
81{
82 center_y_ = center_y;
84
85template <typename PointT>
86inline float
88{
89 return center_y_;
90}
91
92template <typename PointT>
93inline void
95{
96 focal_length_ = focal_length;
97}
99template <typename PointT>
100inline float
102{
103 return focal_length_;
105
106template <typename PointT>
107inline void
109{
110 baseline_ = baseline;
111}
112
113template <typename PointT>
114inline float
117 return baseline_;
118}
119
120template <typename PointT>
121inline void
123 const float disparity_threshold_min)
124{
125 disparity_threshold_min_ = disparity_threshold_min;
126}
127
128template <typename PointT>
129inline float
131{
132 return disparity_threshold_min_;
133}
135template <typename PointT>
136inline void
138 const float disparity_threshold_max)
139{
140 disparity_threshold_max_ = disparity_threshold_max;
141}
142
143template <typename PointT>
144inline float
147 return disparity_threshold_max_;
148}
149
150template <typename PointT>
151void
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
164template <typename PointT>
167{
169 *image_pointer = *image_;
170 return image_pointer;
171}
172
173template <typename PointT>
174bool
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;
195 disparity_map_[column + row * disparity_map_width_] = disparity;
196 } // column
197 } // row
198
199 return true;
200}
201
202template <typename PointT>
203bool
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;
212 // Load the disparity map.
213 return loadDisparityMap(file_name);
214}
215
216template <typename PointT>
217void
219 const std::vector<float>& disparity_map)
220{
221 disparity_map_ = disparity_map;
222}
223
224template <typename PointT>
225void
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
237template <typename PointT>
238std::vector<float>
240{
241 return disparity_map_;
242}
243
244template <typename PointT>
245void
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
301template <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