43 #include <pcl/point_cloud.h>
44 #include <pcl/common/point_tests.h>
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);
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 )
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);
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)
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);
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 )
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);
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);