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);
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 uint8_t grayvalue =
static_cast<uint8_t
>(0.2989 * point.r
331 rgbData_arg.push_back (grayvalue);
335 rgbData_arg.push_back (point.r);
336 rgbData_arg.push_back (point.g);
337 rgbData_arg.push_back (point.b);
342 uint16_t disparity =
static_cast<uint16_t
> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
345 disparityData_arg.push_back (disparity);
353 rgbData_arg.push_back (0);
356 rgbData_arg.push_back (0);
357 rgbData_arg.push_back (0);
358 rgbData_arg.push_back (0);
362 disparityData_arg.push_back (0);
380 static void convert(
typename std::vector<uint16_t>& disparityData_arg,
381 typename std::vector<uint8_t>& rgbData_arg,
385 float focalLength_arg,
386 float disparityShift_arg,
387 float disparityScale_arg,
391 size_t cloud_size = width_arg*height_arg;
392 bool hasColor = (rgbData_arg.size () > 0);
395 assert (disparityData_arg.size()==cloud_size);
400 assert (rgbData_arg.size()==cloud_size);
403 assert (rgbData_arg.size()==cloud_size*3);
407 int x, y, centerX, centerY;
411 cloud_arg.
points.reserve(cloud_size);
414 cloud_arg.
width =
static_cast<uint32_t
>(width_arg);
415 cloud_arg.
height =
static_cast<uint32_t
>(height_arg);
419 centerX =
static_cast<int>(width_arg/2);
420 centerY =
static_cast<int>(height_arg/2);
422 const float fl_const = 1.0f/focalLength_arg;
423 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
426 for (y=-centerY; y<+centerY; ++y )
427 for (x=-centerX; x<+centerX; ++x )
431 const uint16_t& pixel_disparity = disparityData_arg[i];
433 if (pixel_disparity && (pixel_disparity!=0x7FF))
435 float depth = focalLength_arg / (
static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
439 newPoint.x =
static_cast<float> (x) * depth * fl_const;
440 newPoint.y =
static_cast<float> (y) * depth * fl_const;
447 newPoint.r = rgbData_arg[i];
448 newPoint.g = rgbData_arg[i];
449 newPoint.b = rgbData_arg[i];
453 newPoint.r = rgbData_arg[i*3+0];
454 newPoint.g = rgbData_arg[i*3+1];
455 newPoint.b = rgbData_arg[i*3+2];
461 newPoint.rgba = 0xffffffffu;
466 newPoint.x = newPoint.y = newPoint.z = bad_point;
471 cloud_arg.
points.push_back(newPoint);
487 static void convert(
typename std::vector<float>& depthData_arg,
488 typename std::vector<uint8_t>& rgbData_arg,
492 float focalLength_arg,
496 size_t cloud_size = width_arg*height_arg;
497 bool hasColor = (rgbData_arg.size () > 0);
500 assert (depthData_arg.size()==cloud_size);
505 assert (rgbData_arg.size()==cloud_size);
508 assert (rgbData_arg.size()==cloud_size*3);
512 int x, y, centerX, centerY;
516 cloud_arg.
points.reserve(cloud_size);
519 cloud_arg.
width =
static_cast<uint32_t
>(width_arg);
520 cloud_arg.
height =
static_cast<uint32_t
>(height_arg);
524 centerX =
static_cast<int>(width_arg/2);
525 centerY =
static_cast<int>(height_arg/2);
527 const float fl_const = 1.0f/focalLength_arg;
528 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
531 for (y=-centerY; y<+centerY; ++y )
532 for (x=-centerX; x<+centerX; ++x )
536 const float& pixel_depth = depthData_arg[i];
538 if (pixel_depth==pixel_depth)
540 float depth = focalLength_arg / pixel_depth;
544 newPoint.x =
static_cast<float> (x) * depth * fl_const;
545 newPoint.y =
static_cast<float> (y) * depth * fl_const;
552 newPoint.r = rgbData_arg[i];
553 newPoint.g = rgbData_arg[i];
554 newPoint.b = rgbData_arg[i];
558 newPoint.r = rgbData_arg[i*3+0];
559 newPoint.g = rgbData_arg[i*3+1];
560 newPoint.b = rgbData_arg[i*3+2];
566 newPoint.rgba = 0xffffffffu;
571 newPoint.x = newPoint.y = newPoint.z = bad_point;
576 cloud_arg.
points.push_back(newPoint);