43 #include <pcl/common/point_tests.h>
44 #include <pcl/io/pcd_io.h>
46 #include <pcl/type_traits.h>
51 #pragma GCC system_header
53 #include <vtkVersion.h>
54 #include <vtkFloatArray.h>
55 #include <vtkPointData.h>
56 #include <vtkPoints.h>
57 #include <vtkPolyData.h>
58 #include <vtkUnsignedCharArray.h>
59 #include <vtkSmartPointer.h>
60 #include <vtkStructuredGrid.h>
61 #include <vtkVertexGlyphFilter.h>
64 #ifdef vtkGenericDataArray_h
65 #define SetTupleValue SetTypedTuple
66 #define InsertNextTupleValue InsertNextTypedTuple
67 #define GetTupleValue GetTypedTuple
71 template <
typename Po
intT>
void
76 cloud.
width = polydata->GetNumberOfPoints ();
82 std::vector<pcl::PCLPointField> fields;
86 int x_idx = -1, y_idx = -1, z_idx = -1;
87 for (std::size_t d = 0; d < fields.size (); ++d)
89 if (fields[d].name ==
"x")
90 x_idx = fields[d].offset;
91 else if (fields[d].name ==
"y")
92 y_idx = fields[d].offset;
93 else if (fields[d].name ==
"z")
94 z_idx = fields[d].offset;
97 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
99 for (std::size_t i = 0; i < cloud.
size (); ++i)
101 double coordinate[3];
102 polydata->GetPoint (i, coordinate);
103 pcl::setFieldValue<PointT, float> (cloud[i], x_idx, coordinate[0]);
104 pcl::setFieldValue<PointT, float> (cloud[i], y_idx, coordinate[1]);
105 pcl::setFieldValue<PointT, float> (cloud[i], z_idx, coordinate[2]);
110 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
111 for (std::size_t d = 0; d < fields.size (); ++d)
113 if (fields[d].name ==
"normal_x")
114 normal_x_idx = fields[d].offset;
115 else if (fields[d].name ==
"normal_y")
116 normal_y_idx = fields[d].offset;
117 else if (fields[d].name ==
"normal_z")
118 normal_z_idx = fields[d].offset;
121 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
122 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
124 for (std::size_t i = 0; i < cloud.
size (); ++i)
127 normals->GetTupleValue (i, normal);
128 pcl::setFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
129 pcl::setFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
130 pcl::setFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
135 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
137 for (std::size_t d = 0; d < fields.size (); ++d)
139 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
141 rgb_idx = fields[d].offset;
146 if (rgb_idx != -1 && colors)
148 for (std::size_t i = 0; i < cloud.
size (); ++i)
150 unsigned char color[3];
151 colors->GetTupleValue (i, color);
153 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
154 pcl::setFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
160 template <
typename Po
intT>
void
164 structured_grid->GetDimensions (dimensions);
165 cloud.
width = dimensions[0];
166 cloud.
height = dimensions[1];
171 std::vector<pcl::PCLPointField> fields;
175 int x_idx = -1, y_idx = -1, z_idx = -1;
176 for (std::size_t d = 0; d < fields.size (); ++d)
178 if (fields[d].name ==
"x")
179 x_idx = fields[d].offset;
180 else if (fields[d].name ==
"y")
181 y_idx = fields[d].offset;
182 else if (fields[d].name ==
"z")
183 z_idx = fields[d].offset;
186 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
188 for (std::size_t i = 0; i < cloud.
width; ++i)
190 for (std::size_t j = 0; j < cloud.
height; ++j)
192 int queryPoint[3] = {i, j, 0};
193 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
194 double coordinate[3];
195 if (structured_grid->IsPointVisible (pointId))
197 structured_grid->GetPoint (pointId, coordinate);
198 pcl::setFieldValue<PointT, float> (cloud (i, j), x_idx, coordinate[0]);
199 pcl::setFieldValue<PointT, float> (cloud (i, j), y_idx, coordinate[1]);
200 pcl::setFieldValue<PointT, float> (cloud (i, j), z_idx, coordinate[2]);
211 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
212 for (std::size_t d = 0; d < fields.size (); ++d)
214 if (fields[d].name ==
"normal_x")
215 normal_x_idx = fields[d].offset;
216 else if (fields[d].name ==
"normal_y")
217 normal_y_idx = fields[d].offset;
218 else if (fields[d].name ==
"normal_z")
219 normal_z_idx = fields[d].offset;
222 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
224 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
226 for (std::size_t i = 0; i < cloud.
width; ++i)
228 for (std::size_t j = 0; j < cloud.
height; ++j)
230 int queryPoint[3] = {i, j, 0};
231 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
233 if (structured_grid->IsPointVisible (pointId))
235 normals->GetTupleValue (i, normal);
236 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_x_idx, normal[0]);
237 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_y_idx, normal[1]);
238 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_z_idx, normal[2]);
249 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray (
"Colors"));
251 for (std::size_t d = 0; d < fields.size (); ++d)
253 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
255 rgb_idx = fields[d].offset;
260 if (rgb_idx != -1 && colors)
262 for (std::size_t i = 0; i < cloud.
width; ++i)
264 for (std::size_t j = 0; j < cloud.
height; ++j)
266 int queryPoint[3] = {i, j, 0};
267 vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint);
268 unsigned char color[3];
269 if (structured_grid->IsPointVisible (pointId))
271 colors->GetTupleValue (i, color);
273 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
274 pcl::setFieldValue<PointT, std::uint32_t> (cloud (i, j), rgb_idx, rgb.rgba);
286 template <
typename Po
intT>
void
290 std::vector<pcl::PCLPointField> fields;
294 vtkIdType nr_points = cloud.
size ();
296 points->SetNumberOfPoints (nr_points);
298 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
303 for (vtkIdType i = 0; i < nr_points; ++i)
304 memcpy (&data[i * 3], &cloud[i].x, 12);
309 for (vtkIdType i = 0; i < nr_points; ++i)
312 if (!std::isfinite (cloud[i].x) ||
313 !std::isfinite (cloud[i].y) ||
314 !std::isfinite (cloud[i].z))
317 memcpy (&data[j * 3], &cloud[i].x, 12);
321 points->SetNumberOfPoints (nr_points);
326 temp_polydata->SetPoints (points);
329 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
330 for (std::size_t d = 0; d < fields.size (); ++d)
332 if (fields[d].name ==
"normal_x")
333 normal_x_idx = fields[d].offset;
334 else if (fields[d].name ==
"normal_y")
335 normal_y_idx = fields[d].offset;
336 else if (fields[d].name ==
"normal_z")
337 normal_z_idx = fields[d].offset;
339 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
342 normals->SetNumberOfComponents (3);
343 normals->SetNumberOfTuples (cloud.
size ());
344 normals->SetName (
"Normals");
346 for (std::size_t i = 0; i < cloud.
size (); ++i)
349 pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
350 pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
351 pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
352 normals->SetTupleValue (i, normal);
354 temp_polydata->GetPointData ()->SetNormals (normals);
359 for (std::size_t d = 0; d < fields.size (); ++d)
361 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
363 rgb_idx = fields[d].offset;
370 colors->SetNumberOfComponents (3);
371 colors->SetNumberOfTuples (cloud.
size ());
372 colors->SetName (
"RGB");
374 for (std::size_t i = 0; i < cloud.
size (); ++i)
376 unsigned char color[3];
378 pcl::getFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
379 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
380 colors->SetTupleValue (i, color);
382 temp_polydata->GetPointData ()->SetScalars (colors);
387 vertex_glyph_filter->SetInputData (temp_polydata);
388 vertex_glyph_filter->Update ();
390 pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
394 template <
typename Po
intT>
void
398 std::vector<pcl::PCLPointField> fields;
401 int dimensions[3] = {cloud.
width, cloud.
height, 1};
402 structured_grid->SetDimensions (dimensions);
405 points->SetNumberOfPoints (cloud.
width * cloud.
height);
407 for (std::size_t i = 0; i < cloud.
width; ++i)
409 for (std::size_t j = 0; j < cloud.
height; ++j)
411 int queryPoint[3] = {i, j, 0};
412 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
413 const PointT &point = cloud (i, j);
417 float p[3] = {point.x, point.y, point.z};
418 points->SetPoint (pointId, p);
426 structured_grid->SetPoints (points);
429 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
430 for (std::size_t d = 0; d < fields.size (); ++d)
432 if (fields[d].name ==
"normal_x")
433 normal_x_idx = fields[d].offset;
434 else if (fields[d].name ==
"normal_y")
435 normal_y_idx = fields[d].offset;
436 else if (fields[d].name ==
"normal_z")
437 normal_z_idx = fields[d].offset;
440 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
443 normals->SetNumberOfComponents (3);
444 normals->SetNumberOfTuples (cloud.
width * cloud.
height);
445 normals->SetName (
"Normals");
446 for (std::size_t i = 0; i < cloud.
width; ++i)
448 for (std::size_t j = 0; j < cloud.
height; ++j)
450 int queryPoint[3] = {i, j, 0};
451 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
452 const PointT &point = cloud (i, j);
455 pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]);
456 pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]);
457 pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]);
458 normals->SetTupleValue (pointId, normal);
462 structured_grid->GetPointData ()->SetNormals (normals);
467 for (std::size_t d = 0; d < fields.size (); ++d)
469 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
471 rgb_idx = fields[d].offset;
479 colors->SetNumberOfComponents (3);
480 colors->SetNumberOfTuples (cloud.
width * cloud.
height);
481 colors->SetName (
"Colors");
482 for (std::size_t i = 0; i < cloud.
width; ++i)
484 for (std::size_t j = 0; j < cloud.
height; ++j)
486 int queryPoint[3] = {i, j, 0};
487 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
488 const PointT &point = cloud (i, j);
493 unsigned char color[3];
495 pcl::getFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
496 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
497 colors->SetTupleValue (pointId, color);
504 structured_grid->GetPointData ()->AddArray (colors);
508 #ifdef vtkGenericDataArray_h
510 #undef InsertNextTupleValue