38 #ifndef PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ 39 #define PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ 46 #include <pcl/common/io.h> 47 #include <pcl/common/colors.h> 50 template <
typename Po
intT>
bool 56 bool result = this->extractImpl (cloud, img);
58 if (paint_nans_with_black_ && result)
60 size_t size = img.
encoding ==
"mono16" ? 2 : 3;
61 for (
size_t i = 0; i < cloud.
points.size (); ++i)
63 std::memset (&img.
data[i * size], 0, size);
70 template <
typename Po
intT>
bool 73 std::vector<pcl::PCLPointField> fields;
77 if (field_x_idx == -1 || field_y_idx == -1 || field_z_idx == -1)
79 const size_t offset_x = fields[field_x_idx].offset;
80 const size_t offset_y = fields[field_y_idx].offset;
81 const size_t offset_z = fields[field_z_idx].offset;
86 img.
step = img.
width *
sizeof (
unsigned char) * 3;
89 for (
size_t i = 0; i < cloud.
points.size (); ++i)
94 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset_x, x);
95 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset_y, y);
96 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset_z, z);
97 img.
data[i * 3 + 0] =
static_cast<unsigned char>((x + 1.0) * 127);
98 img.
data[i * 3 + 1] =
static_cast<unsigned char>((y + 1.0) * 127);
99 img.
data[i * 3 + 2] =
static_cast<unsigned char>((z + 1.0) * 127);
106 template <
typename Po
intT>
bool 109 std::vector<pcl::PCLPointField> fields;
117 const size_t offset = fields[field_idx].offset;
122 img.
step = img.
width *
sizeof (
unsigned char) * 3;
125 for (
size_t i = 0; i < cloud.
points.size (); ++i)
128 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
129 img.
data[i * 3 + 0] = (val >> 16) & 0x0000ff;
130 img.
data[i * 3 + 1] = (val >> 8) & 0x0000ff;
131 img.
data[i * 3 + 2] = (val) & 0x0000ff;
138 template <
typename Po
intT>
bool 141 std::vector<pcl::PCLPointField> fields;
145 const size_t offset = fields[field_idx].offset;
154 img.
step = img.
width *
sizeof (
unsigned short);
156 unsigned short* data =
reinterpret_cast<unsigned short*
>(&img.
data[0]);
157 for (
size_t i = 0; i < cloud.
points.size (); ++i)
160 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
161 data[i] =
static_cast<unsigned short>(val);
165 case COLORS_RGB_RANDOM:
170 img.
step = img.
width *
sizeof (
unsigned char) * 3;
173 std::srand(std::time(0));
174 std::map<uint32_t, size_t> colormap;
176 for (
size_t i = 0; i < cloud.
points.size (); ++i)
179 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
180 if (colormap.count (val) == 0)
182 colormap[val] = i * 3;
183 img.
data[i * 3 + 0] =
static_cast<uint8_t
> ((std::rand () % 256));
184 img.
data[i * 3 + 1] =
static_cast<uint8_t
> ((std::rand () % 256));
185 img.
data[i * 3 + 2] =
static_cast<uint8_t
> ((std::rand () % 256));
189 memcpy (&img.
data[i * 3], &img.
data[colormap[val]], 3);
194 case COLORS_RGB_GLASBEY:
199 img.
step = img.
width *
sizeof (
unsigned char) * 3;
202 std::srand(std::time(0));
203 std::set<uint32_t> labels;
204 std::map<uint32_t, size_t> colormap;
207 for (
size_t i = 0; i < cloud.
points.size (); ++i)
213 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
221 for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter)
223 colormap[*iter] = color % GlasbeyLUT::size ();
228 for (
size_t i = 0; i < cloud.
points.size (); ++i)
231 pcl::getFieldValue<PointT, uint32_t> (cloud.
points[i], offset, val);
232 memcpy (&img.
data[i * 3], GlasbeyLUT::data () + colormap[val] * 3, 3);
243 template <
typename Po
intT>
bool 246 std::vector<pcl::PCLPointField> fields;
250 const size_t offset = fields[field_idx].offset;
255 img.
step = img.
width *
sizeof (
unsigned short);
257 unsigned short* data =
reinterpret_cast<unsigned short*
>(&img.
data[0]);
259 float scaling_factor = scaling_factor_;
260 float data_min = 0.0f;
261 if (scaling_method_ == SCALING_FULL_RANGE)
263 float min = std::numeric_limits<float>::infinity();
264 float max = -std::numeric_limits<float>::infinity();
265 for (
size_t i = 0; i < cloud.
points.size (); ++i)
268 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset, val);
274 scaling_factor = min == max ? 0 : std::numeric_limits<unsigned short>::max() / (max - min);
278 for (
size_t i = 0; i < cloud.
points.size (); ++i)
281 pcl::getFieldValue<PointT, float> (cloud.
points[i], offset, val);
282 if (scaling_method_ == SCALING_NO)
286 else if (scaling_method_ == SCALING_FULL_RANGE)
288 data[i] = (val - data_min) * scaling_factor;
290 else if (scaling_method_ == SCALING_FIXED_FACTOR)
292 data[i] = val * scaling_factor;
299 #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 return true if f...
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
uint32_t height
The point cloud height (if organized as an image-structure).
uint32_t width
The point cloud width (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
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).