38 #ifndef PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
39 #define PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
45 #include <pcl/common/colors.h>
51 if (!capable_ || !cloud_)
55 scalars->SetNumberOfComponents (3);
57 vtkIdType nr_points = cloud_->points.size ();
58 scalars->SetNumberOfTuples (nr_points);
61 unsigned char* colors =
new unsigned char[nr_points * 3];
64 for (vtkIdType cp = 0; cp < nr_points; ++cp)
66 colors[cp * 3 + 0] =
static_cast<unsigned char> (r_);
67 colors[cp * 3 + 1] =
static_cast<unsigned char> (g_);
68 colors[cp * 3 + 2] =
static_cast<unsigned char> (b_);
70 scalars->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
78 if (!capable_ || !cloud_)
82 scalars->SetNumberOfComponents (3);
84 vtkIdType nr_points = cloud_->points.size ();
85 scalars->SetNumberOfTuples (nr_points);
88 unsigned char* colors =
new unsigned char[nr_points * 3];
92 int r_ =
static_cast<int> (
pcl_lrint (r * 255.0)),
93 g_ =
static_cast<int> (
pcl_lrint (g * 255.0)),
94 b_ =
static_cast<int> (
pcl_lrint (b * 255.0));
97 for (vtkIdType cp = 0; cp < nr_points; ++cp)
99 colors[cp * 3 + 0] =
static_cast<unsigned char> (r_);
100 colors[cp * 3 + 1] =
static_cast<unsigned char> (g_);
101 colors[cp * 3 + 2] =
static_cast<unsigned char> (b_);
103 scalars->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
108 template <
typename Po
intT>
void
114 field_idx_ = pcl::getFieldIndex<PointT> (
"rgb", fields_);
115 if (field_idx_ != -1)
122 field_idx_ = pcl::getFieldIndex<PointT> (
"rgba", fields_);
123 if (field_idx_ != -1)
134 if (!capable_ || !cloud_)
138 std::vector<pcl::PCLPointField> fields;
140 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
141 if (rgba_index == -1)
142 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
144 int rgba_offset = fields[rgba_index].offset;
147 scalars->SetNumberOfComponents (3);
149 vtkIdType nr_points = cloud_->points.size ();
150 scalars->SetNumberOfTuples (nr_points);
151 unsigned char* colors = scalars->GetPointer (0);
156 for (std::size_t d = 0; d < fields_.size (); ++d)
157 if (fields_[d].name ==
"x")
158 x_idx =
static_cast<int> (d);
164 for (vtkIdType cp = 0; cp < nr_points; ++cp)
167 if (!std::isfinite (cloud_->points[cp].x) ||
168 !std::isfinite (cloud_->points[cp].y) ||
169 !std::isfinite (cloud_->points[cp].z))
172 memcpy (&rgb, (
reinterpret_cast<const char *
> (&cloud_->points[cp])) + rgba_offset, sizeof (
pcl::RGB));
174 colors[j + 1] = rgb.g;
175 colors[j + 2] = rgb.b;
182 for (vtkIdType cp = 0; cp < nr_points; ++cp)
184 int idx =
static_cast<int> (cp) * 3;
185 memcpy (&rgb, (
reinterpret_cast<const char *
> (&cloud_->points[cp])) + rgba_offset, sizeof (
pcl::RGB));
186 colors[idx ] = rgb.r;
187 colors[idx + 1] = rgb.g;
188 colors[idx + 2] = rgb.b;
195 template <
typename Po
intT>
229 if (!capable_ || !cloud_)
233 scalars->SetNumberOfComponents (3);
235 vtkIdType nr_points = cloud_->points.size ();
236 scalars->SetNumberOfTuples (nr_points);
237 unsigned char* colors = scalars->GetPointer (0);
243 for (std::size_t d = 0; d < fields_.size (); ++d)
244 if (fields_[d].name ==
"x")
245 x_idx =
static_cast<int> (d);
250 for (vtkIdType cp = 0; cp < nr_points; ++cp)
253 if (!std::isfinite (cloud_->points[cp].x) ||
254 !std::isfinite (cloud_->points[cp].y) ||
255 !std::isfinite (cloud_->points[cp].z))
260 float h = cloud_->points[cp].h;
261 float v = cloud_->points[cp].v;
262 float s = cloud_->points[cp].s;
266 h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((
int)h)/360)*360;
269 if (s > 1.0f) s = 1.0f;
270 if (s < 0.0f) s = 0.0f;
271 if (v > 1.0f) v = 1.0f;
272 if (v < 0.0f) v = 0.0f;
276 colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
282 int i = std::floor (a);
284 float p = v * (1 - s);
285 float q = v * (1 - s * f);
286 float t = v * (1 - s * (1 - f));
291 colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255;
break;
293 colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255;
break;
295 colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255;
break;
297 colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255;
break;
299 colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255;
break;
301 colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255;
break;
310 for (vtkIdType cp = 0; cp < nr_points; ++cp)
312 float h = cloud_->points[cp].h;
313 float v = cloud_->points[cp].v;
314 float s = cloud_->points[cp].s;
318 h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((
int)h)/360)*360;
321 if (s > 1.0f) s = 1.0f;
322 if (s < 0.0f) s = 0.0f;
323 if (v > 1.0f) v = 1.0f;
324 if (v < 0.0f) v = 0.0f;
328 colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
334 int i = std::floor (a);
336 float p = v * (1 - s);
337 float q = v * (1 - s * f);
338 float t = v * (1 - s * (1 - f));
343 colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255;
break;
345 colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255;
break;
347 colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255;
break;
349 colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255;
break;
351 colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255;
break;
353 colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255;
break;
363 template <
typename Po
intT>
void
368 field_idx_ = pcl::getFieldIndex<PointT> (field_name_, fields_);
369 if (field_idx_ != -1)
379 if (!capable_ || !cloud_)
383 scalars->SetNumberOfComponents (1);
385 vtkIdType nr_points = cloud_->points.size ();
386 scalars->SetNumberOfTuples (nr_points);
390 float* colors =
new float[nr_points];
396 for (std::size_t d = 0; d < fields_.size (); ++d)
397 if (fields_[d].name ==
"x")
398 x_idx =
static_cast<int> (d);
403 for (vtkIdType cp = 0; cp < nr_points; ++cp)
406 if (!std::isfinite (cloud_->points[cp].x) || !std::isfinite (cloud_->points[cp].y) || !std::isfinite (cloud_->points[cp].z))
410 memcpy (&field_data, pt_data + fields_[field_idx_].offset,
pcl::getFieldSize (fields_[field_idx_].datatype));
412 colors[j] = field_data;
419 for (vtkIdType cp = 0; cp < nr_points; ++cp)
422 memcpy (&field_data, pt_data + fields_[field_idx_].offset,
pcl::getFieldSize (fields_[field_idx_].datatype));
424 if (!std::isfinite (field_data))
427 colors[j] = field_data;
431 scalars->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
436 template <
typename Po
intT>
void
442 field_idx_ = pcl::getFieldIndex<PointT> (
"rgba", fields_);
443 if (field_idx_ != -1)
453 if (!capable_ || !cloud_)
457 scalars->SetNumberOfComponents (4);
459 vtkIdType nr_points = cloud_->points.size ();
460 scalars->SetNumberOfTuples (nr_points);
461 unsigned char* colors = scalars->GetPointer (0);
466 for (std::size_t d = 0; d < fields_.size (); ++d)
467 if (fields_[d].name ==
"x")
468 x_idx =
static_cast<int> (d);
473 for (vtkIdType cp = 0; cp < nr_points; ++cp)
476 if (!std::isfinite (cloud_->points[cp].x) ||
477 !std::isfinite (cloud_->points[cp].y) ||
478 !std::isfinite (cloud_->points[cp].z))
481 colors[j ] = cloud_->points[cp].r;
482 colors[j + 1] = cloud_->points[cp].g;
483 colors[j + 2] = cloud_->points[cp].b;
484 colors[j + 3] = cloud_->points[cp].a;
491 for (vtkIdType cp = 0; cp < nr_points; ++cp)
493 int idx =
static_cast<int> (cp) * 4;
494 colors[idx ] = cloud_->points[cp].r;
495 colors[idx + 1] = cloud_->points[cp].g;
496 colors[idx + 2] = cloud_->points[cp].b;
497 colors[idx + 3] = cloud_->points[cp].a;
504 template <
typename Po
intT>
void
508 field_idx_ = pcl::getFieldIndex<PointT> (
"label", fields_);
509 if (field_idx_ != -1)
520 if (!capable_ || !cloud_)
524 scalars->SetNumberOfComponents (3);
526 vtkIdType nr_points = cloud_->points.size ();
527 scalars->SetNumberOfTuples (nr_points);
528 unsigned char* colors = scalars->GetPointer (0);
531 std::map<std::uint32_t, pcl::RGB> colormap;
532 if (!static_mapping_)
534 std::set<std::uint32_t> labels;
536 for (vtkIdType i = 0; i < nr_points; ++i)
537 labels.insert (cloud_->points[i].label);
540 std::size_t color = 0;
541 for (std::set<std::uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
546 for (vtkIdType cp = 0; cp < nr_points; ++cp)
551 colors[j ] = color.r;
552 colors[j + 1] = color.g;
553 colors[j + 2] = color.b;
561 #endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_