38 #ifndef PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
39 #define PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
46 #include <pcl/common/io.h>
49 template <
typename Po
intT>
bool
55 bool result = this->extractImpl (cloud, img);
57 if (paint_nans_with_black_ && result)
59 size_t size = img.
encoding ==
"mono16" ? 2 : 3;
60 for (
size_t i = 0; i < cloud.
points.size (); ++i)
62 std::memset (&img.
data[i * size], 0, size);
69 template <
typename Po
intT>
bool
72 std::vector<pcl::PCLPointField> fields;
76 if (field_x_idx == -1 || field_y_idx == -1 || field_z_idx == -1)
78 const size_t offset_x = fields[field_x_idx].offset;
79 const size_t offset_y = fields[field_y_idx].offset;
80 const size_t offset_z = fields[field_z_idx].offset;
85 img.
step = img.
width *
sizeof (
unsigned char) * 3;
88 for (
size_t i = 0; i < cloud.
points.size (); ++i)
93 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset_x, x);
94 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset_y, y);
95 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset_z, z);
96 img.
data[i * 3 + 0] =
static_cast<unsigned char>((x + 1.0) * 127);
97 img.
data[i * 3 + 1] =
static_cast<unsigned char>((y + 1.0) * 127);
98 img.
data[i * 3 + 2] =
static_cast<unsigned char>((z + 1.0) * 127);
105 template <
typename Po
intT>
bool
108 std::vector<pcl::PCLPointField> fields;
116 const size_t offset = fields[field_idx].offset;
121 img.
step = img.
width *
sizeof (
unsigned char) * 3;
124 for (
size_t i = 0; i < cloud.
points.size (); ++i)
127 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
128 img.
data[i * 3 + 0] = (val >> 16) & 0x0000ff;
129 img.
data[i * 3 + 1] = (val >> 8) & 0x0000ff;
130 img.
data[i * 3 + 2] = (val) & 0x0000ff;
138 const uint8_t GLASBEY_LUT[] =
397 const size_t GLASBEY_LUT_SIZE =
sizeof (GLASBEY_LUT) /
sizeof (GLASBEY_LUT[0]);
400 template <
typename Po
intT>
bool
403 std::vector<pcl::PCLPointField> fields;
407 const size_t offset = fields[field_idx].offset;
416 img.
step = img.
width *
sizeof (
unsigned short);
418 unsigned short* data =
reinterpret_cast<unsigned short*
>(&img.
data[0]);
419 for (
size_t i = 0; i < cloud.
points.size (); ++i)
422 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
423 data[i] =
static_cast<unsigned short>(val);
427 case COLORS_RGB_RANDOM:
432 img.
step = img.
width *
sizeof (
unsigned char) * 3;
435 std::srand(std::time(0));
436 std::map<uint32_t, size_t> colormap;
438 for (
size_t i = 0; i < cloud.
points.size (); ++i)
441 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
442 if (colormap.count (val) == 0)
444 colormap[val] = i * 3;
445 img.
data[i * 3 + 0] =
static_cast<uint8_t
> ((std::rand () % 256));
446 img.
data[i * 3 + 1] =
static_cast<uint8_t
> ((std::rand () % 256));
447 img.
data[i * 3 + 2] =
static_cast<uint8_t
> ((std::rand () % 256));
451 memcpy (&img.
data[i * 3], &img.
data[colormap[val]], 3);
456 case COLORS_RGB_GLASBEY:
461 img.
step = img.
width *
sizeof (
unsigned char) * 3;
464 std::srand(std::time(0));
465 std::set<uint32_t> labels;
466 std::map<uint32_t, size_t> colormap;
469 for (
size_t i = 0; i < cloud.
points.size (); ++i)
472 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
478 for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter)
480 colormap[*iter] = color % GLASBEY_LUT_SIZE;
485 for (
size_t i = 0; i < cloud.
points.size (); ++i)
488 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
489 memcpy (&img.
data[i * 3], &GLASBEY_LUT[colormap[val] * 3], 3);
500 template <
typename Po
intT>
bool
503 std::vector<pcl::PCLPointField> fields;
507 const size_t offset = fields[field_idx].offset;
512 img.
step = img.
width *
sizeof (
unsigned short);
514 unsigned short* data =
reinterpret_cast<unsigned short*
>(&img.
data[0]);
516 float scaling_factor = scaling_factor_;
517 float data_min = 0.0f;
518 if (scaling_method_ == SCALING_FULL_RANGE)
520 float min = std::numeric_limits<float>::infinity();
521 float max = -std::numeric_limits<float>::infinity();
522 for (
size_t i = 0; i < cloud.
points.size (); ++i)
525 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset, val);
531 scaling_factor = min == max ? 0 : std::numeric_limits<unsigned short>::max() / (max - min);
535 for (
size_t i = 0; i < cloud.
points.size (); ++i)
538 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset, val);
539 if (scaling_method_ == SCALING_NO)
543 else if (scaling_method_ == SCALING_FULL_RANGE)
545 data[i] = (val - data_min) * scaling_factor;
547 else if (scaling_method_ == SCALING_FIXED_FACTOR)
549 data[i] = val * scaling_factor;
556 #endif // PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
uint32_t width
The point cloud width (if organized as an image-structure).
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
uint32_t height
The point cloud height (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::vector< pcl::uint8_t > data