Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
organized_pointcloud_conversion.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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 * $Id$
37 * Authors: Julius Kammerl (julius@kammerl.de)
38 */
39
40#pragma once
41
42#include <pcl/pcl_macros.h>
43#include <pcl/point_cloud.h>
44#include <pcl/common/point_tests.h> // for pcl::isFinite
45
46#include <vector>
47#include <limits>
48#include <cassert>
49
50namespace pcl
51{
52namespace io
53{
54
55template<typename PointT> struct CompressionPointTraits
56{
57 static const bool hasColor = false;
58 static const unsigned int channels = 1;
59 static const std::size_t bytesPerPoint = 3 * sizeof(float);
60};
61
62template<>
64{
65 static const bool hasColor = true;
66 static const unsigned int channels = 4;
67 static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
68};
69
70template<>
72{
73 static const bool hasColor = true;
74 static const unsigned int channels = 4;
75 static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t);
76};
77
80
81// Uncolored point cloud specialization
82template<typename PointT>
84{
85 /** \brief Convert point cloud to disparity image
86 * \param[in] cloud_arg input point cloud
87 * \param[in] focalLength_arg focal length
88 * \param[in] disparityShift_arg disparity shift
89 * \param[in] disparityScale_arg disparity scaling
90 * \param[out] disparityData_arg output disparity image
91 * \ingroup io
92 */
94 float focalLength_arg,
97 bool ,
98 typename std::vector<std::uint16_t>& disparityData_arg,
99 typename std::vector<std::uint8_t>&)
100 {
101 const auto cloud_size = cloud_arg.size ();
102
103 // Clear image data
104 disparityData_arg.clear ();
105
106 disparityData_arg.reserve (cloud_size);
107
108 for (std::size_t i = 0; i < cloud_size; ++i)
109 {
110 // Get point from cloud
111 const PointT& point = cloud_arg[i];
112
113 if (pcl::isFinite (point))
114 {
115 // Inverse depth quantization
116 std::uint16_t disparity = static_cast<std::uint16_t> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
117 disparityData_arg.push_back (disparity);
118 }
119 else
120 {
121 // Non-valid points are encoded with zeros
122 disparityData_arg.push_back (0);
123 }
124 }
125 }
126
127 /** \brief Convert disparity image to point cloud
128 * \param[in] disparityData_arg input depth image
129 * \param[in] width_arg width of disparity image
130 * \param[in] height_arg height of disparity image
131 * \param[in] focalLength_arg focal length
132 * \param[in] disparityShift_arg disparity shift
133 * \param[in] disparityScale_arg disparity scaling
134 * \param[out] cloud_arg output point cloud
135 * \ingroup io
136 */
137 static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
138 typename std::vector<std::uint8_t>&,
139 bool,
140 std::size_t width_arg,
141 std::size_t height_arg,
142 float focalLength_arg,
143 float disparityShift_arg,
144 float disparityScale_arg,
146 {
147 std::size_t cloud_size = width_arg * height_arg;
148
149 assert(disparityData_arg.size()==cloud_size);
150
151 // Reset point cloud
152 cloud_arg.clear ();
153 cloud_arg.reserve (cloud_size);
154
155 // Define point cloud parameters
156 cloud_arg.width = static_cast<std::uint32_t> (width_arg);
157 cloud_arg.height = static_cast<std::uint32_t> (height_arg);
158 cloud_arg.is_dense = false;
159
160 // Calculate center of disparity image
161 int centerX = static_cast<int> (width_arg / 2);
162 int centerY = static_cast<int> (height_arg / 2);
163
164 const float fl_const = 1.0f / focalLength_arg;
165 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
166
167 std::size_t i = 0;
168 for (int y = -centerY; y < centerY; ++y )
169 for (int x = -centerX; x < centerX; ++x )
170 {
172 const std::uint16_t& pixel_disparity = disparityData_arg[i];
173 ++i;
174
175 if (pixel_disparity)
176 {
177 // Inverse depth decoding
178 float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
179
180 // Generate new points
181 newPoint.x = static_cast<float> (x) * depth * fl_const;
182 newPoint.y = static_cast<float> (y) * depth * fl_const;
183 newPoint.z = depth;
184
185 }
186 else
187 {
188 // Generate bad point
190 }
191
192 cloud_arg.push_back (newPoint);
193 }
194 }
195
196 /** \brief Convert disparity image to point cloud
197 * \param[in] depthData_arg input depth image
198 * \param[in] width_arg width of disparity image
199 * \param[in] height_arg height of disparity image
200 * \param[in] focalLength_arg focal length
201 * \param[out] cloud_arg output point cloud
202 * \ingroup io
203 */
204 static void convert(typename std::vector<float>& depthData_arg,
205 typename std::vector<std::uint8_t>&,
206 bool,
207 std::size_t width_arg,
208 std::size_t height_arg,
209 float focalLength_arg,
211 {
212 std::size_t cloud_size = width_arg * height_arg;
213
214 assert(depthData_arg.size()==cloud_size);
215
216 // Reset point cloud
217 cloud_arg.clear ();
218 cloud_arg.reserve (cloud_size);
219
220 // Define point cloud parameters
221 cloud_arg.width = static_cast<std::uint32_t> (width_arg);
222 cloud_arg.height = static_cast<std::uint32_t> (height_arg);
223 cloud_arg.is_dense = false;
224
225 // Calculate center of disparity image
226 int centerX = static_cast<int> (width_arg / 2);
227 int centerY = static_cast<int> (height_arg / 2);
228
229 const float fl_const = 1.0f / focalLength_arg;
230 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
231
232 std::size_t i = 0;
233 for (int y = -centerY; y < centerY; ++y )
234 for (int x = -centerX; x < centerX; ++x )
235 {
237 const float& pixel_depth = depthData_arg[i];
238 ++i;
239
240 if (pixel_depth)
241 {
242 // Generate new points
243 newPoint.x = static_cast<float> (x) * pixel_depth * fl_const;
244 newPoint.y = static_cast<float> (y) * pixel_depth * fl_const;
246
247 }
248 else
249 {
250 // Generate bad point
252 }
253
254 cloud_arg.push_back (newPoint);
255 }
256 }
257};
258
259// Colored point cloud specialization
260template <typename PointT>
262{
263 /** \brief Convert point cloud to disparity image and rgb image
264 * \param[in] cloud_arg input point cloud
265 * \param[in] focalLength_arg focal length
266 * \param[in] disparityShift_arg disparity shift
267 * \param[in] disparityScale_arg disparity scaling
268 * \param[in] convertToMono convert color to mono/grayscale
269 * \param[out] disparityData_arg output disparity image
270 * \param[out] rgbData_arg output rgb image
271 * \ingroup io
272 */
274 float focalLength_arg,
275 float disparityShift_arg,
276 float disparityScale_arg,
277 bool convertToMono,
278 typename std::vector<std::uint16_t>& disparityData_arg,
279 typename std::vector<std::uint8_t>& rgbData_arg)
280 {
281 const auto cloud_size = cloud_arg.size ();
282
283 // Reset output vectors
284 disparityData_arg.clear ();
285 rgbData_arg.clear ();
286
287 // Allocate memory
288 disparityData_arg.reserve (cloud_size);
289 if (convertToMono)
290 {
291 rgbData_arg.reserve (cloud_size);
292 } else
293 {
294 rgbData_arg.reserve (cloud_size * 3);
295 }
296
297 for (std::size_t i = 0; i < cloud_size; ++i)
298 {
299 const PointT& point = cloud_arg[i];
300
301 if (pcl::isFinite (point))
302 {
303 if (convertToMono)
304 {
305 // Encode point color
306 std::uint8_t grayvalue = static_cast<std::uint8_t>(0.2989 * point.r
307 + 0.5870 * point.g
308 + 0.1140 * point.b);
309
310 rgbData_arg.push_back (grayvalue);
311 } else
312 {
313 // Encode point color
314 rgbData_arg.push_back (point.r);
315 rgbData_arg.push_back (point.g);
316 rgbData_arg.push_back (point.b);
317 }
318
319 // Inverse depth quantization
320 std::uint16_t disparity = static_cast<std::uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
321
322 // Encode disparity
323 disparityData_arg.push_back (disparity);
324 }
325 else
326 {
327 // Encode black point
328 if (convertToMono)
329 {
330 rgbData_arg.push_back (0);
331 } else
332 {
333 rgbData_arg.push_back (0);
334 rgbData_arg.push_back (0);
335 rgbData_arg.push_back (0);
336 }
337
338 // Encode bad point
339 disparityData_arg.push_back (0);
340 }
341 }
342 }
343
344 /** \brief Convert disparity image to point cloud
345 * \param[in] disparityData_arg output disparity image
346 * \param[in] rgbData_arg output rgb image
347 * \param[in] monoImage_arg input image is a single-channel mono image
348 * \param[in] width_arg width of disparity image
349 * \param[in] height_arg height of disparity image
350 * \param[in] focalLength_arg focal length
351 * \param[in] disparityShift_arg disparity shift
352 * \param[in] disparityScale_arg disparity scaling
353 * \param[out] cloud_arg output point cloud
354 * \ingroup io
355 */
356 static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
357 typename std::vector<std::uint8_t>& rgbData_arg,
358 bool monoImage_arg,
359 std::size_t width_arg,
360 std::size_t height_arg,
361 float focalLength_arg,
362 float disparityShift_arg,
363 float disparityScale_arg,
365 {
366 std::size_t cloud_size = width_arg*height_arg;
367 bool hasColor = (!rgbData_arg.empty ());
368
369 // Check size of input data
370 assert (disparityData_arg.size()==cloud_size);
371 if (hasColor)
372 {
373 if (monoImage_arg)
374 {
375 assert (rgbData_arg.size()==cloud_size);
376 } else
377 {
378 assert (rgbData_arg.size()==cloud_size*3);
379 }
380 }
381
382 // Reset point cloud
383 cloud_arg.clear();
384 cloud_arg.reserve(cloud_size);
385
386 // Define point cloud parameters
387 cloud_arg.width = static_cast<std::uint32_t>(width_arg);
388 cloud_arg.height = static_cast<std::uint32_t>(height_arg);
389 cloud_arg.is_dense = false;
390
391 // Calculate center of disparity image
392 int centerX = static_cast<int>(width_arg/2);
393 int centerY = static_cast<int>(height_arg/2);
394
395 const float fl_const = 1.0f/focalLength_arg;
396 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
397
398 std::size_t i = 0;
399 for (int y = -centerY; y < centerY; ++y )
400 for (int x = -centerX; x < centerX; ++x )
401 {
403
404 const std::uint16_t& pixel_disparity = disparityData_arg[i];
405
406 if (pixel_disparity && (pixel_disparity!=0x7FF))
407 {
408 float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
409
410 // Define point location
411 newPoint.z = depth;
412 newPoint.x = static_cast<float> (x) * depth * fl_const;
413 newPoint.y = static_cast<float> (y) * depth * fl_const;
414
415 if (hasColor)
416 {
417 if (monoImage_arg)
418 {
419 // Define point color
420 newPoint.r = rgbData_arg[i];
421 newPoint.g = rgbData_arg[i];
422 newPoint.b = rgbData_arg[i];
423 } else
424 {
425 // Define point color
426 newPoint.r = rgbData_arg[i*3+0];
427 newPoint.g = rgbData_arg[i*3+1];
428 newPoint.b = rgbData_arg[i*3+2];
429 }
430
431 } else
432 {
433 // Set white point color
434 newPoint.rgba = 0xffffffffu;
435 }
436 } else
437 {
438 // Define bad point
440 newPoint.rgb = 0.0f;
441 }
442
443 // Add point to cloud
444 cloud_arg.push_back(newPoint);
445 // Increment point iterator
446 ++i;
447 }
448 }
449
450 /** \brief Convert disparity image to point cloud
451 * \param[in] depthData_arg output disparity image
452 * \param[in] rgbData_arg output rgb image
453 * \param[in] monoImage_arg input image is a single-channel mono image
454 * \param[in] width_arg width of disparity image
455 * \param[in] height_arg height of disparity image
456 * \param[in] focalLength_arg focal length
457 * \param[out] cloud_arg output point cloud
458 * \ingroup io
459 */
460 static void convert(typename std::vector<float>& depthData_arg,
461 typename std::vector<std::uint8_t>& rgbData_arg,
462 bool monoImage_arg,
463 std::size_t width_arg,
464 std::size_t height_arg,
465 float focalLength_arg,
467 {
468 std::size_t cloud_size = width_arg*height_arg;
469 bool hasColor = (!rgbData_arg.empty ());
470
471 // Check size of input data
472 assert (depthData_arg.size()==cloud_size);
473 if (hasColor)
474 {
475 if (monoImage_arg)
476 {
477 assert (rgbData_arg.size()==cloud_size);
478 } else
479 {
480 assert (rgbData_arg.size()==cloud_size*3);
481 }
482 }
483
484 // Reset point cloud
485 cloud_arg.clear();
486 cloud_arg.reserve(cloud_size);
487
488 // Define point cloud parameters
489 cloud_arg.width = static_cast<std::uint32_t>(width_arg);
490 cloud_arg.height = static_cast<std::uint32_t>(height_arg);
491 cloud_arg.is_dense = false;
492
493 // Calculate center of disparity image
494 int centerX = static_cast<int>(width_arg/2);
495 int centerY = static_cast<int>(height_arg/2);
496
497 const float fl_const = 1.0f/focalLength_arg;
498 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
499
500 std::size_t i = 0;
501 for (int y = -centerY; y < centerY; ++y )
502 for (int x = -centerX; x < centerX; ++x )
503 {
505
506 const float& pixel_depth = depthData_arg[i];
507
508 if (pixel_depth)
509 {
510 // Define point location
512 newPoint.x = static_cast<float> (x) * pixel_depth * fl_const;
513 newPoint.y = static_cast<float> (y) * pixel_depth * fl_const;
514
515 if (hasColor)
516 {
517 if (monoImage_arg)
518 {
519 // Define point color
520 newPoint.r = rgbData_arg[i];
521 newPoint.g = rgbData_arg[i];
522 newPoint.b = rgbData_arg[i];
523 } else
524 {
525 // Define point color
526 newPoint.r = rgbData_arg[i*3+0];
527 newPoint.g = rgbData_arg[i*3+1];
528 newPoint.b = rgbData_arg[i*3+2];
529 }
530
531 } else
532 {
533 // Set white point color
534 newPoint.rgba = 0xffffffffu;
535 }
536 } else
537 {
538 // Define bad point
540 newPoint.rgb = 0.0f;
541 }
542
543 // Add point to cloud
544 cloud_arg.push_back(newPoint);
545 // Increment point iterator
546 ++i;
547 }
548 }
549};
550
551} // namespace io
552} // namespace pcl
553
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< std::uint8_t > &, bool, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool convertToMono, typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &rgbData_arg)
Convert point cloud to disparity image and rgb image.
static void convert(typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &, bool, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
static void convert(typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &rgbData_arg, bool monoImage_arg, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool, typename std::vector< std::uint16_t > &disparityData_arg, typename std::vector< std::uint8_t > &)
Convert point cloud to disparity image.
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< std::uint8_t > &rgbData_arg, bool monoImage_arg, std::size_t width_arg, std::size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.