45 #include <pcl/common/io.h>
46 #include <pcl/common/colors.h>
47 #include <pcl/common/point_tests.h>
50 template <
typename Po
intT>
bool
56 bool result = this->extractImpl (cloud, img);
58 if (paint_nans_with_black_ && result)
60 std::size_t size = img.
encoding ==
"mono16" ? 2 : 3;
61 for (std::size_t i = 0; i < cloud.
size (); ++i)
63 std::memset (&img.
data[i * size], 0, size);
70 template <
typename Po
intT>
bool
73 std::vector<pcl::PCLPointField> fields;
74 int field_x_idx = pcl::getFieldIndex<PointT> (
"normal_x", fields);
75 int field_y_idx = pcl::getFieldIndex<PointT> (
"normal_y", fields);
76 int field_z_idx = pcl::getFieldIndex<PointT> (
"normal_z", fields);
77 if (field_x_idx == -1 || field_y_idx == -1 || field_z_idx == -1)
79 const std::size_t offset_x = fields[field_x_idx].offset;
80 const std::size_t offset_y = fields[field_y_idx].offset;
81 const std::size_t offset_z = fields[field_z_idx].offset;
86 img.
step = img.
width *
sizeof (
unsigned char) * 3;
89 for (std::size_t i = 0; i < cloud.
size (); ++i)
94 pcl::getFieldValue<PointT, float> (cloud[i], offset_x, x);
95 pcl::getFieldValue<PointT, float> (cloud[i], offset_y, y);
96 pcl::getFieldValue<PointT, float> (cloud[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;
110 int field_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
113 field_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
117 const std::size_t offset = fields[field_idx].offset;
122 img.
step = img.
width *
sizeof (
unsigned char) * 3;
125 for (std::size_t i = 0; i < cloud.
size (); ++i)
128 pcl::getFieldValue<PointT, std::uint32_t> (cloud[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;
142 int field_idx = pcl::getFieldIndex<PointT> (
"label", fields);
145 const std::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 (std::size_t i = 0; i < cloud.
size (); ++i)
160 pcl::getFieldValue<PointT, std::uint32_t> (cloud[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(
nullptr));
174 std::map<std::uint32_t, std::size_t> colormap;
176 for (std::size_t i = 0; i < cloud.
size (); ++i)
179 pcl::getFieldValue<PointT, std::uint32_t> (cloud[i], offset, val);
180 if (colormap.count (val) == 0)
182 colormap[val] = i * 3;
183 img.
data[i * 3 + 0] =
static_cast<std::uint8_t
> ((std::rand () % 256));
184 img.
data[i * 3 + 1] =
static_cast<std::uint8_t
> ((std::rand () % 256));
185 img.
data[i * 3 + 2] =
static_cast<std::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(
nullptr));
203 std::set<std::uint32_t> labels;
204 std::map<std::uint32_t, std::size_t> colormap;
207 for (
const auto& point: cloud)
213 pcl::getFieldValue<PointT, std::uint32_t> (point, offset, val);
220 std::size_t color = 0;
221 for (
const std::uint32_t &label : labels)
223 colormap[label] = color % GlasbeyLUT::size ();
228 for (std::size_t i = 0; i < cloud.size (); ++i)
231 pcl::getFieldValue<PointT, std::uint32_t> (cloud[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;
247 int field_idx = pcl::getFieldIndex<PointT> (field_name_, fields);
250 const std::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 (
const auto& point: cloud)
268 pcl::getFieldValue<PointT, float> (point, offset, val);
274 scaling_factor = min == max ? 0 : std::numeric_limits<unsigned short>::max() / (max - min);
278 for (std::size_t i = 0; i < cloud.
size (); ++i)
281 pcl::getFieldValue<PointT, float> (cloud[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;