43 #include <pcl/point_cloud.h>
66 static const std::size_t
bytesPerPoint = 3 *
sizeof(float) + 3 *
sizeof(std::uint8_t);
74 static const std::size_t
bytesPerPoint = 3 *
sizeof(float) + 3 *
sizeof(std::uint8_t);
78 template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor >
84 template<
typename Po
intT>
96 float focalLength_arg,
97 float disparityShift_arg,
98 float disparityScale_arg,
100 typename std::vector<std::uint16_t>& disparityData_arg,
101 typename std::vector<std::uint8_t>&)
103 std::size_t cloud_size = cloud_arg.
points.size ();
106 disparityData_arg.clear ();
108 disparityData_arg.reserve (cloud_size);
110 for (std::size_t i = 0; i < cloud_size; ++i)
118 std::uint16_t disparity = static_cast<std::uint16_t> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
119 disparityData_arg.push_back (disparity);
124 disparityData_arg.push_back (0);
139 static void convert(
typename std::vector<std::uint16_t>& disparityData_arg,
140 typename std::vector<std::uint8_t>&,
142 std::size_t width_arg,
143 std::size_t height_arg,
144 float focalLength_arg,
145 float disparityShift_arg,
146 float disparityScale_arg,
149 std::size_t cloud_size = width_arg * height_arg;
151 assert(disparityData_arg.size()==cloud_size);
154 cloud_arg.
points.clear ();
155 cloud_arg.
points.reserve (cloud_size);
158 cloud_arg.
width = static_cast<std::uint32_t> (width_arg);
159 cloud_arg.
height = static_cast<std::uint32_t> (height_arg);
163 int centerX = static_cast<int> (width_arg / 2);
164 int centerY = static_cast<int> (height_arg / 2);
166 const float fl_const = 1.0f / focalLength_arg;
167 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
170 for (
int y = -centerY; y < centerY; ++y )
171 for (
int x = -centerX; x < centerX; ++x )
174 const std::uint16_t& pixel_disparity = disparityData_arg[i];
180 float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
183 newPoint.x = static_cast<float> (x) * depth * fl_const;
184 newPoint.y = static_cast<float> (y) * depth * fl_const;
191 newPoint.x = newPoint.y = newPoint.z = bad_point;
194 cloud_arg.
points.push_back (newPoint);
208 static void convert(
typename std::vector<float>& depthData_arg,
209 typename std::vector<std::uint8_t>&,
211 std::size_t width_arg,
212 std::size_t height_arg,
213 float focalLength_arg,
216 std::size_t cloud_size = width_arg * height_arg;
218 assert(depthData_arg.size()==cloud_size);
221 cloud_arg.
points.clear ();
222 cloud_arg.
points.reserve (cloud_size);
225 cloud_arg.
width = static_cast<std::uint32_t> (width_arg);
226 cloud_arg.
height = static_cast<std::uint32_t> (height_arg);
230 int centerX = static_cast<int> (width_arg / 2);
231 int centerY = static_cast<int> (height_arg / 2);
233 const float fl_const = 1.0f / focalLength_arg;
234 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
237 for (
int y = -centerY; y < centerY; ++y )
238 for (
int x = -centerX; x < centerX; ++x )
241 const float& pixel_depth = depthData_arg[i];
247 float depth = focalLength_arg / pixel_depth;
250 newPoint.x = static_cast<float> (x) * depth * fl_const;
251 newPoint.y = static_cast<float> (y) * depth * fl_const;
258 newPoint.x = newPoint.y = newPoint.z = bad_point;
261 cloud_arg.
points.push_back (newPoint);
271 template <
typename Po
intT>
285 float focalLength_arg,
286 float disparityShift_arg,
287 float disparityScale_arg,
289 typename std::vector<std::uint16_t>& disparityData_arg,
290 typename std::vector<std::uint8_t>& rgbData_arg)
292 std::size_t cloud_size = cloud_arg.
points.size ();
295 disparityData_arg.clear ();
296 rgbData_arg.clear ();
299 disparityData_arg.reserve (cloud_size);
302 rgbData_arg.reserve (cloud_size);
305 rgbData_arg.reserve (cloud_size * 3);
309 for (std::size_t i = 0; i < cloud_size; ++i)
318 std::uint8_t grayvalue = static_cast<std::uint8_t>(0.2989 * point.r
322 rgbData_arg.push_back (grayvalue);
326 rgbData_arg.push_back (point.r);
327 rgbData_arg.push_back (point.g);
328 rgbData_arg.push_back (point.b);
333 std::uint16_t disparity = static_cast<std::uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
336 disparityData_arg.push_back (disparity);
344 rgbData_arg.push_back (0);
347 rgbData_arg.push_back (0);
348 rgbData_arg.push_back (0);
349 rgbData_arg.push_back (0);
353 disparityData_arg.push_back (0);
371 static void convert(
typename std::vector<std::uint16_t>& disparityData_arg,
372 typename std::vector<std::uint8_t>& rgbData_arg,
374 std::size_t width_arg,
375 std::size_t height_arg,
376 float focalLength_arg,
377 float disparityShift_arg,
378 float disparityScale_arg,
381 std::size_t cloud_size = width_arg*height_arg;
382 bool hasColor = (!rgbData_arg.empty ());
385 assert (disparityData_arg.size()==cloud_size);
390 assert (rgbData_arg.size()==cloud_size);
393 assert (rgbData_arg.size()==cloud_size*3);
399 cloud_arg.
points.reserve(cloud_size);
402 cloud_arg.
width = static_cast<std::uint32_t>(width_arg);
403 cloud_arg.
height = static_cast<std::uint32_t>(height_arg);
407 int centerX = static_cast<int>(width_arg/2);
408 int centerY = static_cast<int>(height_arg/2);
410 const float fl_const = 1.0f/focalLength_arg;
411 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
414 for (
int y = -centerY; y < centerY; ++y )
415 for (
int x = -centerX; x < centerX; ++x )
419 const std::uint16_t& pixel_disparity = disparityData_arg[i];
421 if (pixel_disparity && (pixel_disparity!=0x7FF))
423 float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
427 newPoint.x = static_cast<float> (x) * depth * fl_const;
428 newPoint.y = static_cast<float> (y) * depth * fl_const;
435 newPoint.r = rgbData_arg[i];
436 newPoint.g = rgbData_arg[i];
437 newPoint.b = rgbData_arg[i];
441 newPoint.r = rgbData_arg[i*3+0];
442 newPoint.g = rgbData_arg[i*3+1];
443 newPoint.b = rgbData_arg[i*3+2];
449 newPoint.rgba = 0xffffffffu;
454 newPoint.x = newPoint.y = newPoint.z = bad_point;
459 cloud_arg.
points.push_back(newPoint);
475 static void convert(
typename std::vector<float>& depthData_arg,
476 typename std::vector<std::uint8_t>& rgbData_arg,
478 std::size_t width_arg,
479 std::size_t height_arg,
480 float focalLength_arg,
483 std::size_t cloud_size = width_arg*height_arg;
484 bool hasColor = (!rgbData_arg.empty ());
487 assert (depthData_arg.size()==cloud_size);
492 assert (rgbData_arg.size()==cloud_size);
495 assert (rgbData_arg.size()==cloud_size*3);
501 cloud_arg.
points.reserve(cloud_size);
504 cloud_arg.
width = static_cast<std::uint32_t>(width_arg);
505 cloud_arg.
height = static_cast<std::uint32_t>(height_arg);
509 int centerX = static_cast<int>(width_arg/2);
510 int centerY = static_cast<int>(height_arg/2);
512 const float fl_const = 1.0f/focalLength_arg;
513 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
516 for (
int y = -centerY; y < centerY; ++y )
517 for (
int x = -centerX; x < centerX; ++x )
521 const float& pixel_depth = depthData_arg[i];
523 if (pixel_depth==pixel_depth)
525 float depth = focalLength_arg / pixel_depth;
529 newPoint.x = static_cast<float> (x) * depth * fl_const;
530 newPoint.y = static_cast<float> (y) * depth * fl_const;
537 newPoint.r = rgbData_arg[i];
538 newPoint.g = rgbData_arg[i];
539 newPoint.b = rgbData_arg[i];
543 newPoint.r = rgbData_arg[i*3+0];
544 newPoint.g = rgbData_arg[i*3+1];
545 newPoint.b = rgbData_arg[i*3+2];
551 newPoint.rgba = 0xffffffffu;
556 newPoint.x = newPoint.y = newPoint.z = bad_point;
561 cloud_arg.
points.push_back(newPoint);