Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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: disparity_threshold_max_(std::numeric_limits<float>::max())
51{}
52
53template <typename PointT>
55
56template <typename PointT>
57inline void
59{
60 center_x_ = center_x;
61}
62
63template <typename PointT>
64inline float
66{
67 return center_x_;
68}
69
70template <typename PointT>
71inline void
73{
74 center_y_ = center_y;
75}
76
77template <typename PointT>
78inline float
80{
81 return center_y_;
82}
84template <typename PointT>
85inline void
87{
88 focal_length_ = focal_length;
89}
90
91template <typename PointT>
92inline float
94{
95 return focal_length_;
96}
97
98template <typename PointT>
99inline void
101{
102 baseline_ = baseline;
103}
105template <typename PointT>
106inline float
108{
109 return baseline_;
111
112template <typename PointT>
113inline void
115 const float disparity_threshold_min)
117 disparity_threshold_min_ = disparity_threshold_min;
118}
119
120template <typename PointT>
121inline float
123{
124 return disparity_threshold_min_;
125}
126
127template <typename PointT>
128inline void
130 const float disparity_threshold_max)
131{
132 disparity_threshold_max_ = disparity_threshold_max;
133}
135template <typename PointT>
136inline float
138{
139 return disparity_threshold_max_;
141
142template <typename PointT>
143void
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;
153 is_color_ = true;
154}
155
156template <typename PointT>
159{
161 *image_pointer = *image_;
162 return image_pointer;
163}
165template <typename PointT>
166bool
168{
169 std::fstream disparity_file;
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 }
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;
187 disparity_map_[column + row * disparity_map_width_] = disparity;
188 } // column
189 } // row
190
191 return true;
192}
193
194template <typename PointT>
195bool
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;
204 // Load the disparity map.
205 return loadDisparityMap(file_name);
206}
207
208template <typename PointT>
209void
211 const std::vector<float>& disparity_map)
212{
213 disparity_map_ = disparity_map;
214}
215
216template <typename PointT>
217void
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}
229template <typename PointT>
230std::vector<float>
232{
233 return disparity_map_;
234}
235
236template <typename PointT>
237void
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
293template <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.
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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