42 #include <pcl/pcl_macros.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/common/point_tests.h>
67 static const std::size_t
bytesPerPoint = 3 *
sizeof(float) + 3 *
sizeof(std::uint8_t);
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);
78 template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor >
82 template<
typename Po
intT>
94 float focalLength_arg,
95 float disparityShift_arg,
96 float disparityScale_arg,
98 typename std::vector<std::uint16_t>& disparityData_arg,
99 typename std::vector<std::uint8_t>&)
101 std::size_t cloud_size = cloud_arg.
points.size ();
104 disparityData_arg.clear ();
106 disparityData_arg.reserve (cloud_size);
108 for (std::size_t i = 0; i < cloud_size; ++i)
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);
122 disparityData_arg.push_back (0);
137 static void convert(
typename std::vector<std::uint16_t>& disparityData_arg,
138 typename std::vector<std::uint8_t>&,
140 std::size_t width_arg,
141 std::size_t height_arg,
142 float focalLength_arg,
143 float disparityShift_arg,
144 float disparityScale_arg,
147 std::size_t cloud_size = width_arg * height_arg;
149 assert(disparityData_arg.size()==cloud_size);
152 cloud_arg.
points.clear ();
153 cloud_arg.
points.reserve (cloud_size);
156 cloud_arg.
width =
static_cast<std::uint32_t
> (width_arg);
157 cloud_arg.
height =
static_cast<std::uint32_t
> (height_arg);
161 int centerX =
static_cast<int> (width_arg / 2);
162 int centerY =
static_cast<int> (height_arg / 2);
164 const float fl_const = 1.0f / focalLength_arg;
165 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
168 for (
int y = -centerY; y < centerY; ++y )
169 for (
int x = -centerX; x < centerX; ++x )
172 const std::uint16_t& pixel_disparity = disparityData_arg[i];
178 float depth = focalLength_arg / (
static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
181 newPoint.x =
static_cast<float> (x) * depth * fl_const;
182 newPoint.y =
static_cast<float> (y) * depth * fl_const;
189 newPoint.x = newPoint.y = newPoint.z = bad_point;
192 cloud_arg.
points.push_back (newPoint);
204 static void convert(
typename std::vector<float>& depthData_arg,
205 typename std::vector<std::uint8_t>&,
207 std::size_t width_arg,
208 std::size_t height_arg,
209 float focalLength_arg,
212 std::size_t cloud_size = width_arg * height_arg;
214 assert(depthData_arg.size()==cloud_size);
217 cloud_arg.
points.clear ();
218 cloud_arg.
points.reserve (cloud_size);
221 cloud_arg.
width =
static_cast<std::uint32_t
> (width_arg);
222 cloud_arg.
height =
static_cast<std::uint32_t
> (height_arg);
226 int centerX =
static_cast<int> (width_arg / 2);
227 int centerY =
static_cast<int> (height_arg / 2);
229 const float fl_const = 1.0f / focalLength_arg;
230 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
233 for (
int y = -centerY; y < centerY; ++y )
234 for (
int x = -centerX; x < centerX; ++x )
237 const float& pixel_depth = depthData_arg[i];
243 float depth = focalLength_arg / pixel_depth;
246 newPoint.x =
static_cast<float> (x) * depth * fl_const;
247 newPoint.y =
static_cast<float> (y) * depth * fl_const;
254 newPoint.x = newPoint.y = newPoint.z = bad_point;
257 cloud_arg.
points.push_back (newPoint);
263 template <
typename Po
intT>
277 float focalLength_arg,
278 float disparityShift_arg,
279 float disparityScale_arg,
281 typename std::vector<std::uint16_t>& disparityData_arg,
282 typename std::vector<std::uint8_t>& rgbData_arg)
284 std::size_t cloud_size = cloud_arg.
points.size ();
287 disparityData_arg.clear ();
288 rgbData_arg.clear ();
291 disparityData_arg.reserve (cloud_size);
294 rgbData_arg.reserve (cloud_size);
297 rgbData_arg.reserve (cloud_size * 3);
300 for (std::size_t i = 0; i < cloud_size; ++i)
309 std::uint8_t grayvalue =
static_cast<std::uint8_t
>(0.2989 * point.r
313 rgbData_arg.push_back (grayvalue);
317 rgbData_arg.push_back (point.r);
318 rgbData_arg.push_back (point.g);
319 rgbData_arg.push_back (point.b);
323 std::uint16_t disparity =
static_cast<std::uint16_t
> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
326 disparityData_arg.push_back (disparity);
333 rgbData_arg.push_back (0);
336 rgbData_arg.push_back (0);
337 rgbData_arg.push_back (0);
338 rgbData_arg.push_back (0);
342 disparityData_arg.push_back (0);
359 static void convert(
typename std::vector<std::uint16_t>& disparityData_arg,
360 typename std::vector<std::uint8_t>& rgbData_arg,
362 std::size_t width_arg,
363 std::size_t height_arg,
364 float focalLength_arg,
365 float disparityShift_arg,
366 float disparityScale_arg,
369 std::size_t cloud_size = width_arg*height_arg;
370 bool hasColor = (!rgbData_arg.empty ());
373 assert (disparityData_arg.size()==cloud_size);
378 assert (rgbData_arg.size()==cloud_size);
381 assert (rgbData_arg.size()==cloud_size*3);
387 cloud_arg.
points.reserve(cloud_size);
390 cloud_arg.
width =
static_cast<std::uint32_t
>(width_arg);
391 cloud_arg.
height =
static_cast<std::uint32_t
>(height_arg);
395 int centerX =
static_cast<int>(width_arg/2);
396 int centerY =
static_cast<int>(height_arg/2);
398 const float fl_const = 1.0f/focalLength_arg;
399 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
402 for (
int y = -centerY; y < centerY; ++y )
403 for (
int x = -centerX; x < centerX; ++x )
407 const std::uint16_t& pixel_disparity = disparityData_arg[i];
409 if (pixel_disparity && (pixel_disparity!=0x7FF))
411 float depth = focalLength_arg / (
static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
415 newPoint.x =
static_cast<float> (x) * depth * fl_const;
416 newPoint.y =
static_cast<float> (y) * depth * fl_const;
423 newPoint.r = rgbData_arg[i];
424 newPoint.g = rgbData_arg[i];
425 newPoint.b = rgbData_arg[i];
429 newPoint.r = rgbData_arg[i*3+0];
430 newPoint.g = rgbData_arg[i*3+1];
431 newPoint.b = rgbData_arg[i*3+2];
437 newPoint.rgba = 0xffffffffu;
442 newPoint.x = newPoint.y = newPoint.z = bad_point;
447 cloud_arg.
points.push_back(newPoint);
463 static void convert(
typename std::vector<float>& depthData_arg,
464 typename std::vector<std::uint8_t>& rgbData_arg,
466 std::size_t width_arg,
467 std::size_t height_arg,
468 float focalLength_arg,
471 std::size_t cloud_size = width_arg*height_arg;
472 bool hasColor = (!rgbData_arg.empty ());
475 assert (depthData_arg.size()==cloud_size);
480 assert (rgbData_arg.size()==cloud_size);
483 assert (rgbData_arg.size()==cloud_size*3);
489 cloud_arg.
points.reserve(cloud_size);
492 cloud_arg.
width =
static_cast<std::uint32_t
>(width_arg);
493 cloud_arg.
height =
static_cast<std::uint32_t
>(height_arg);
497 int centerX =
static_cast<int>(width_arg/2);
498 int centerY =
static_cast<int>(height_arg/2);
500 const float fl_const = 1.0f/focalLength_arg;
501 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
504 for (
int y = -centerY; y < centerY; ++y )
505 for (
int x = -centerX; x < centerX; ++x )
509 const float& pixel_depth = depthData_arg[i];
511 if (pixel_depth==pixel_depth)
513 float depth = focalLength_arg / pixel_depth;
517 newPoint.x =
static_cast<float> (x) * depth * fl_const;
518 newPoint.y =
static_cast<float> (y) * depth * fl_const;
525 newPoint.r = rgbData_arg[i];
526 newPoint.g = rgbData_arg[i];
527 newPoint.b = rgbData_arg[i];
531 newPoint.r = rgbData_arg[i*3+0];
532 newPoint.g = rgbData_arg[i*3+1];
533 newPoint.b = rgbData_arg[i*3+2];
539 newPoint.rgba = 0xffffffffu;
544 newPoint.x = newPoint.y = newPoint.z = bad_point;
549 cloud_arg.
points.push_back(newPoint);
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...
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 const bool hasColor
std::uint32_t width
The point cloud width (if organized as an image-structure).
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.
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.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
static const std::size_t bytesPerPoint
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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(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.
std::uint32_t height
The point cloud height (if organized as an image-structure).
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 const unsigned int channels
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...