40 #ifndef PCL_ORGANIZED_CONVERSION_H_ 41 #define PCL_ORGANIZED_CONVERSION_H_ 43 #include <pcl/pcl_macros.h> 44 #include <pcl/point_cloud.h> 67 static const size_t bytesPerPoint = 3 *
sizeof(float) + 3 *
sizeof(uint8_t);
73 static const bool hasColor =
true;
74 static const unsigned int channels = 4;
75 static const size_t bytesPerPoint = 3 *
sizeof(float) + 3 *
sizeof(uint8_t);
79 template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor >
85 template<
typename Po
intT>
97 float focalLength_arg,
98 float disparityShift_arg,
99 float disparityScale_arg,
101 typename std::vector<uint16_t>& disparityData_arg,
102 typename std::vector<uint8_t>&)
104 size_t cloud_size, i;
106 cloud_size = cloud_arg.
points.size ();
109 disparityData_arg.clear ();
111 disparityData_arg.reserve (cloud_size);
113 for (i = 0; i < cloud_size; ++i)
121 uint16_t disparity =
static_cast<uint16_t
> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
122 disparityData_arg.push_back (disparity);
127 disparityData_arg.push_back (0);
142 static void convert(
typename std::vector<uint16_t>& disparityData_arg,
143 typename std::vector<uint8_t>&,
147 float focalLength_arg,
148 float disparityShift_arg,
149 float disparityScale_arg,
153 size_t cloud_size = width_arg * height_arg;
154 int x, y, centerX, centerY;
156 assert(disparityData_arg.size()==cloud_size);
159 cloud_arg.
points.clear ();
160 cloud_arg.
points.reserve (cloud_size);
163 cloud_arg.
width =
static_cast<uint32_t
> (width_arg);
164 cloud_arg.
height =
static_cast<uint32_t
> (height_arg);
168 centerX =
static_cast<int> (width_arg / 2);
169 centerY =
static_cast<int> (height_arg / 2);
171 const float fl_const = 1.0f / focalLength_arg;
172 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
175 for (y = -centerY; y < +centerY; ++y)
176 for (x = -centerX; x < +centerX; ++x)
179 const uint16_t& pixel_disparity = disparityData_arg[i];
185 float depth = focalLength_arg / (
static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
188 newPoint.x =
static_cast<float> (x) * depth * fl_const;
189 newPoint.y =
static_cast<float> (y) * depth * fl_const;
196 newPoint.x = newPoint.y = newPoint.z = bad_point;
199 cloud_arg.
points.push_back (newPoint);
213 static void convert(
typename std::vector<float>& depthData_arg,
214 typename std::vector<uint8_t>&,
218 float focalLength_arg,
222 size_t cloud_size = width_arg * height_arg;
223 int x, y, centerX, centerY;
225 assert(depthData_arg.size()==cloud_size);
228 cloud_arg.
points.clear ();
229 cloud_arg.
points.reserve (cloud_size);
232 cloud_arg.
width =
static_cast<uint32_t
> (width_arg);
233 cloud_arg.
height =
static_cast<uint32_t
> (height_arg);
237 centerX =
static_cast<int> (width_arg / 2);
238 centerY =
static_cast<int> (height_arg / 2);
240 const float fl_const = 1.0f / focalLength_arg;
241 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
244 for (y = -centerY; y < +centerY; ++y)
245 for (x = -centerX; x < +centerX; ++x)
248 const float& pixel_depth = depthData_arg[i];
254 float depth = focalLength_arg / pixel_depth;
257 newPoint.x =
static_cast<float> (x) * depth * fl_const;
258 newPoint.y =
static_cast<float> (y) * depth * fl_const;
265 newPoint.x = newPoint.y = newPoint.z = bad_point;
268 cloud_arg.
points.push_back (newPoint);
278 template <
typename Po
intT>
292 float focalLength_arg,
293 float disparityShift_arg,
294 float disparityScale_arg,
296 typename std::vector<uint16_t>& disparityData_arg,
297 typename std::vector<uint8_t>& rgbData_arg)
299 size_t cloud_size, i;
301 cloud_size = cloud_arg.
points.size ();
304 disparityData_arg.clear ();
305 rgbData_arg.clear ();
308 disparityData_arg.reserve (cloud_size);
311 rgbData_arg.reserve (cloud_size);
314 rgbData_arg.reserve (cloud_size * 3);
318 for (i = 0; i < cloud_size; ++i)
327 const uint32_t rgb = *
reinterpret_cast<const int*
> (&point.rgb);
328 uint8_t grayvalue =
static_cast<uint8_t
>(0.2989 *
static_cast<float>((rgb >> 16) & 0x0000ff) +
329 0.5870 *
static_cast<float>((rgb >> 8) & 0x0000ff) +
330 0.1140 *
static_cast<float>((rgb >> 0) & 0x0000ff));
332 rgbData_arg.push_back (grayvalue);
336 const uint32_t rgb = *
reinterpret_cast<const int*
> (&point.rgb);
338 rgbData_arg.push_back ( (rgb >> 16) & 0x0000ff);
339 rgbData_arg.push_back ( (rgb >> 8) & 0x0000ff);
340 rgbData_arg.push_back ( (rgb >> 0) & 0x0000ff);
345 uint16_t disparity =
static_cast<uint16_t
> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
348 disparityData_arg.push_back (disparity);
356 rgbData_arg.push_back (0);
359 rgbData_arg.push_back (0);
360 rgbData_arg.push_back (0);
361 rgbData_arg.push_back (0);
365 disparityData_arg.push_back (0);
383 static void convert(
typename std::vector<uint16_t>& disparityData_arg,
384 typename std::vector<uint8_t>& rgbData_arg,
388 float focalLength_arg,
389 float disparityShift_arg,
390 float disparityScale_arg,
394 size_t cloud_size = width_arg*height_arg;
395 bool hasColor = (rgbData_arg.size () > 0);
398 assert (disparityData_arg.size()==cloud_size);
403 assert (rgbData_arg.size()==cloud_size);
406 assert (rgbData_arg.size()==cloud_size*3);
410 int x, y, centerX, centerY;
414 cloud_arg.
points.reserve(cloud_size);
417 cloud_arg.
width =
static_cast<uint32_t
>(width_arg);
418 cloud_arg.
height =
static_cast<uint32_t
>(height_arg);
422 centerX =
static_cast<int>(width_arg/2);
423 centerY =
static_cast<int>(height_arg/2);
425 const float fl_const = 1.0f/focalLength_arg;
426 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
429 for (y=-centerY; y<+centerY; ++y )
430 for (x=-centerX; x<+centerX; ++x )
434 const uint16_t& pixel_disparity = disparityData_arg[i];
436 if (pixel_disparity && (pixel_disparity!=0x7FF))
438 float depth = focalLength_arg / (
static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
442 newPoint.x =
static_cast<float> (x) * depth * fl_const;
443 newPoint.y =
static_cast<float> (y) * depth * fl_const;
449 const uint8_t& pixel_r = rgbData_arg[i];
450 const uint8_t& pixel_g = rgbData_arg[i];
451 const uint8_t& pixel_b = rgbData_arg[i];
454 uint32_t rgb = (
static_cast<uint32_t
>(pixel_r) << 16
455 | static_cast<uint32_t>(pixel_g) << 8
456 |
static_cast<uint32_t
>(pixel_b));
457 newPoint.rgb = *
reinterpret_cast<float*
>(&rgb);
460 const uint8_t& pixel_r = rgbData_arg[i*3+0];
461 const uint8_t& pixel_g = rgbData_arg[i*3+1];
462 const uint8_t& pixel_b = rgbData_arg[i*3+2];
465 uint32_t rgb = (
static_cast<uint32_t
>(pixel_r) << 16
466 | static_cast<uint32_t>(pixel_g) << 8
467 |
static_cast<uint32_t
>(pixel_b));
468 newPoint.rgb = *
reinterpret_cast<float*
>(&rgb);
474 uint32_t rgb = (
static_cast<uint32_t
>(255) << 16
475 | static_cast<uint32_t>(255) << 8
476 |
static_cast<uint32_t
>(255));
477 newPoint.rgb = *
reinterpret_cast<float*
>(&rgb);
482 newPoint.x = newPoint.y = newPoint.z = bad_point;
487 cloud_arg.
points.push_back(newPoint);
503 static void convert(
typename std::vector<float>& depthData_arg,
504 typename std::vector<uint8_t>& rgbData_arg,
508 float focalLength_arg,
512 size_t cloud_size = width_arg*height_arg;
513 bool hasColor = (rgbData_arg.size () > 0);
516 assert (depthData_arg.size()==cloud_size);
521 assert (rgbData_arg.size()==cloud_size);
524 assert (rgbData_arg.size()==cloud_size*3);
528 int x, y, centerX, centerY;
532 cloud_arg.
points.reserve(cloud_size);
535 cloud_arg.
width =
static_cast<uint32_t
>(width_arg);
536 cloud_arg.
height =
static_cast<uint32_t
>(height_arg);
540 centerX =
static_cast<int>(width_arg/2);
541 centerY =
static_cast<int>(height_arg/2);
543 const float fl_const = 1.0f/focalLength_arg;
544 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
547 for (y=-centerY; y<+centerY; ++y )
548 for (x=-centerX; x<+centerX; ++x )
552 const float& pixel_depth = depthData_arg[i];
554 if (pixel_depth==pixel_depth)
556 float depth = focalLength_arg / pixel_depth;
560 newPoint.x =
static_cast<float> (x) * depth * fl_const;
561 newPoint.y =
static_cast<float> (y) * depth * fl_const;
567 const uint8_t& pixel_r = rgbData_arg[i];
568 const uint8_t& pixel_g = rgbData_arg[i];
569 const uint8_t& pixel_b = rgbData_arg[i];
572 uint32_t rgb = (
static_cast<uint32_t
>(pixel_r) << 16
573 | static_cast<uint32_t>(pixel_g) << 8
574 |
static_cast<uint32_t
>(pixel_b));
575 newPoint.rgb = *
reinterpret_cast<float*
>(&rgb);
578 const uint8_t& pixel_r = rgbData_arg[i*3+0];
579 const uint8_t& pixel_g = rgbData_arg[i*3+1];
580 const uint8_t& pixel_b = rgbData_arg[i*3+2];
583 uint32_t rgb = (
static_cast<uint32_t
>(pixel_r) << 16
584 | static_cast<uint32_t>(pixel_g) << 8
585 |
static_cast<uint32_t
>(pixel_b));
586 newPoint.rgb = *
reinterpret_cast<float*
>(&rgb);
592 uint32_t rgb = (
static_cast<uint32_t
>(255) << 16
593 | static_cast<uint32_t>(255) << 8
594 |
static_cast<uint32_t
>(255));
595 newPoint.rgb = *
reinterpret_cast<float*
>(&rgb);
600 newPoint.x = newPoint.y = newPoint.z = bad_point;
605 cloud_arg.
points.push_back(newPoint);
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool, typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &)
Convert point cloud to disparity image.
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
static const bool hasColor
static void convert(typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &, bool, size_t width_arg, size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
uint32_t height
The point cloud height (if organized as an image-structure).
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< uint8_t > &, bool, size_t width_arg, size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
uint32_t width
The point cloud width (if organized as an image-structure).
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool convertToMono, typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &rgbData_arg)
Convert point cloud to disparity image and rgb image.
static void convert(typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &rgbData_arg, bool monoImage_arg, size_t width_arg, size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
static const size_t bytesPerPoint
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< uint8_t > &rgbData_arg, bool monoImage_arg, size_t width_arg, size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
A point structure representing Euclidean xyz coordinates, and the RGB color.
static const unsigned int channels