44 #include <pcl/common/colors.h>
45 #include <pcl/common/io.h>
46 #include <pcl/common/point_tests.h>
52 namespace visualization
58 if (!capable_ || !cloud_)
62 scalars->SetNumberOfComponents (3);
64 vtkIdType nr_points = cloud_->size ();
65 scalars->SetNumberOfTuples (nr_points);
68 unsigned char* colors =
new unsigned char[nr_points * 3];
71 for (vtkIdType cp = 0; cp < nr_points; ++cp)
73 colors[cp * 3 + 0] =
static_cast<unsigned char> (r_);
74 colors[cp * 3 + 1] =
static_cast<unsigned char> (g_);
75 colors[cp * 3 + 2] =
static_cast<unsigned char> (b_);
77 scalars->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
85 if (!capable_ || !cloud_)
89 scalars->SetNumberOfComponents (3);
91 vtkIdType nr_points = cloud_->size ();
92 scalars->SetNumberOfTuples (nr_points);
95 unsigned char* colors =
new unsigned char[nr_points * 3];
99 int r_ =
static_cast<int> (
pcl_lrint (r * 255.0)),
100 g_ =
static_cast<int> (
pcl_lrint (g * 255.0)),
101 b_ =
static_cast<int> (
pcl_lrint (b * 255.0));
104 for (vtkIdType cp = 0; cp < nr_points; ++cp)
106 colors[cp * 3 + 0] =
static_cast<unsigned char> (r_);
107 colors[cp * 3 + 1] =
static_cast<unsigned char> (g_);
108 colors[cp * 3 + 2] =
static_cast<unsigned char> (b_);
110 scalars->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
115 template <
typename Po
intT>
void
121 field_idx_ = pcl::getFieldIndex<PointT> (
"rgb", fields_);
122 if (field_idx_ != -1)
129 field_idx_ = pcl::getFieldIndex<PointT> (
"rgba", fields_);
130 if (field_idx_ != -1)
141 if (!capable_ || !cloud_)
145 std::vector<pcl::PCLPointField> fields;
147 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
148 if (rgba_index == -1)
149 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
151 int rgba_offset = fields[rgba_index].offset;
154 scalars->SetNumberOfComponents (3);
156 vtkIdType nr_points = cloud_->size ();
157 scalars->SetNumberOfTuples (nr_points);
158 unsigned char* colors = scalars->GetPointer (0);
163 for (std::size_t d = 0; d < fields_.size (); ++d)
164 if (fields_[d].name ==
"x")
165 x_idx =
static_cast<int> (d);
171 for (vtkIdType cp = 0; cp < nr_points; ++cp)
174 if (!std::isfinite ((*cloud_)[cp].x) ||
175 !std::isfinite ((*cloud_)[cp].y) ||
176 !std::isfinite ((*cloud_)[cp].z))
179 memcpy (&rgb, (
reinterpret_cast<const char *
> (&(*cloud_)[cp])) + rgba_offset,
sizeof (
pcl::RGB));
181 colors[j + 1] = rgb.g;
182 colors[j + 2] = rgb.b;
189 for (vtkIdType cp = 0; cp < nr_points; ++cp)
191 int idx =
static_cast<int> (cp) * 3;
192 memcpy (&rgb, (
reinterpret_cast<const char *
> (&(*cloud_)[cp])) + rgba_offset,
sizeof (
pcl::RGB));
193 colors[idx ] = rgb.r;
194 colors[idx + 1] = rgb.g;
195 colors[idx + 2] = rgb.b;
202 template <
typename Po
intT>
236 if (!capable_ || !cloud_)
240 scalars->SetNumberOfComponents (3);
242 vtkIdType nr_points = cloud_->size ();
243 scalars->SetNumberOfTuples (nr_points);
244 unsigned char* colors = scalars->GetPointer (0);
250 for (std::size_t d = 0; d < fields_.size (); ++d)
251 if (fields_[d].name ==
"x")
252 x_idx =
static_cast<int> (d);
257 for (vtkIdType cp = 0; cp < nr_points; ++cp)
260 if (!std::isfinite ((*cloud_)[cp].x) ||
261 !std::isfinite ((*cloud_)[cp].y) ||
262 !std::isfinite ((*cloud_)[cp].z))
267 float h = (*cloud_)[cp].h;
268 float v = (*cloud_)[cp].v;
269 float s = (*cloud_)[cp].s;
273 h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((
int)h)/360)*360;
276 if (s > 1.0f) s = 1.0f;
277 if (s < 0.0f) s = 0.0f;
278 if (v > 1.0f) v = 1.0f;
279 if (v < 0.0f) v = 0.0f;
283 colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
289 int i = std::floor (a);
291 float p = v * (1 - s);
292 float q = v * (1 - s * f);
293 float t = v * (1 - s * (1 - f));
298 colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255;
break;
300 colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255;
break;
302 colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255;
break;
304 colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255;
break;
306 colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255;
break;
308 colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255;
break;
317 for (vtkIdType cp = 0; cp < nr_points; ++cp)
319 float h = (*cloud_)[cp].h;
320 float v = (*cloud_)[cp].v;
321 float s = (*cloud_)[cp].s;
325 h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((
int)h)/360)*360;
328 if (s > 1.0f) s = 1.0f;
329 if (s < 0.0f) s = 0.0f;
330 if (v > 1.0f) v = 1.0f;
331 if (v < 0.0f) v = 0.0f;
335 colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
341 int i = std::floor (a);
343 float p = v * (1 - s);
344 float q = v * (1 - s * f);
345 float t = v * (1 - s * (1 - f));
350 colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255;
break;
352 colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255;
break;
354 colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255;
break;
356 colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255;
break;
358 colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255;
break;
360 colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255;
break;
370 template <
typename Po
intT>
void
375 field_idx_ = pcl::getFieldIndex<PointT> (field_name_, fields_);
376 if (field_idx_ != -1)
386 if (!capable_ || !cloud_)
390 scalars->SetNumberOfComponents (1);
392 vtkIdType nr_points = cloud_->size ();
393 scalars->SetNumberOfTuples (nr_points);
395 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
397 float* colors =
new float[nr_points];
403 for (std::size_t d = 0; d < fields_.size (); ++d)
404 if (fields_[d].name ==
"x")
405 x_idx =
static_cast<int> (d);
410 for (vtkIdType cp = 0; cp < nr_points; ++cp)
413 if (!std::isfinite ((*cloud_)[cp].x) || !std::isfinite ((*cloud_)[cp].y) || !std::isfinite ((*cloud_)[cp].z))
416 const std::uint8_t* pt_data =
reinterpret_cast<const std::uint8_t*
> (&(*cloud_)[cp]);
417 memcpy (&field_data, pt_data + fields_[field_idx_].offset,
pcl::getFieldSize (fields_[field_idx_].datatype));
419 colors[j] = field_data;
426 for (vtkIdType cp = 0; cp < nr_points; ++cp)
428 const std::uint8_t* pt_data =
reinterpret_cast<const std::uint8_t*
> (&(*cloud_)[cp]);
429 memcpy (&field_data, pt_data + fields_[field_idx_].offset,
pcl::getFieldSize (fields_[field_idx_].datatype));
431 if (!std::isfinite (field_data))
434 colors[j] = field_data;
438 scalars->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
443 template <
typename Po
intT>
void
449 field_idx_ = pcl::getFieldIndex<PointT> (
"rgba", fields_);
450 if (field_idx_ != -1)
460 if (!capable_ || !cloud_)
464 scalars->SetNumberOfComponents (4);
466 vtkIdType nr_points = cloud_->size ();
467 scalars->SetNumberOfTuples (nr_points);
468 unsigned char* colors = scalars->GetPointer (0);
472 for (std::size_t d = 0; d < fields_.size (); ++d)
473 if (fields_[d].name ==
"x")
474 x_idx =
static_cast<int> (d);
480 for (vtkIdType cp = 0; cp < nr_points; ++cp)
483 if (!std::isfinite ((*cloud_)[cp].x) ||
484 !std::isfinite ((*cloud_)[cp].y) ||
485 !std::isfinite ((*cloud_)[cp].z))
488 colors[j ] = (*cloud_)[cp].r;
489 colors[j + 1] = (*cloud_)[cp].g;
490 colors[j + 2] = (*cloud_)[cp].b;
491 colors[j + 3] = (*cloud_)[cp].a;
498 for (vtkIdType cp = 0; cp < nr_points; ++cp)
500 int idx =
static_cast<int> (cp) * 4;
501 colors[idx ] = (*cloud_)[cp].r;
502 colors[idx + 1] = (*cloud_)[cp].g;
503 colors[idx + 2] = (*cloud_)[cp].b;
504 colors[idx + 3] = (*cloud_)[cp].a;
511 template <
typename Po
intT>
void
515 field_idx_ = pcl::getFieldIndex<PointT> (
"label", fields_);
516 if (field_idx_ != -1)
527 if (!capable_ || !cloud_)
531 scalars->SetNumberOfComponents (3);
533 vtkIdType nr_points = cloud_->size ();
534 scalars->SetNumberOfTuples (nr_points);
535 unsigned char* colors = scalars->GetPointer (0);
538 std::map<std::uint32_t, pcl::RGB> colormap;
539 if (!static_mapping_)
541 std::set<std::uint32_t> labels;
543 for (vtkIdType i = 0; i < nr_points; ++i)
544 labels.insert ((*cloud_)[i].label);
547 std::size_t color = 0;
548 for (std::set<std::uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
553 for (vtkIdType cp = 0; cp < nr_points; ++cp)
558 colors[j ] = color.r;
559 colors[j + 1] = color.g;
560 colors[j + 2] = color.b;