40 #ifndef PCL_IO_VTK_IO_IMPL_H_
41 #define PCL_IO_VTK_IO_IMPL_H_
44 #include <pcl/common/io.h>
45 #include <pcl/io/pcd_io.h>
47 #include <pcl/point_traits.h>
52 #pragma GCC system_header
54 #include <vtkVersion.h>
55 #include <vtkFloatArray.h>
56 #include <vtkPointData.h>
57 #include <vtkPoints.h>
58 #include <vtkPolyData.h>
59 #include <vtkUnsignedCharArray.h>
60 #include <vtkSmartPointer.h>
61 #include <vtkStructuredGrid.h>
62 #include <vtkVertexGlyphFilter.h>
65 #ifdef vtkGenericDataArray_h
66 #define SetTupleValue SetTypedTuple
67 #define InsertNextTupleValue InsertNextTypedTuple
68 #define GetTupleValue GetTypedTuple
72 template <
typename Po
intT>
void
77 cloud.
width = polydata->GetNumberOfPoints ();
83 std::vector<pcl::PCLPointField> fields;
87 int x_idx = -1, y_idx = -1, z_idx = -1;
88 for (std::size_t d = 0; d < fields.size (); ++d)
90 if (fields[d].name ==
"x")
91 x_idx = fields[d].offset;
92 else if (fields[d].name ==
"y")
93 y_idx = fields[d].offset;
94 else if (fields[d].name ==
"z")
95 z_idx = fields[d].offset;
98 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
100 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
102 double coordinate[3];
103 polydata->GetPoint (i, coordinate);
104 pcl::setFieldValue<PointT, float> (cloud.
points[i], x_idx, coordinate[0]);
105 pcl::setFieldValue<PointT, float> (cloud.
points[i], y_idx, coordinate[1]);
106 pcl::setFieldValue<PointT, float> (cloud.
points[i], z_idx, coordinate[2]);
111 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
112 for (std::size_t d = 0; d < fields.size (); ++d)
114 if (fields[d].name ==
"normal_x")
115 normal_x_idx = fields[d].offset;
116 else if (fields[d].name ==
"normal_y")
117 normal_y_idx = fields[d].offset;
118 else if (fields[d].name ==
"normal_z")
119 normal_z_idx = fields[d].offset;
122 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
123 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
125 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
128 normals->GetTupleValue (i, normal);
129 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_x_idx, normal[0]);
130 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_y_idx, normal[1]);
131 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_z_idx, normal[2]);
136 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
138 for (std::size_t d = 0; d < fields.size (); ++d)
140 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
142 rgb_idx = fields[d].offset;
147 if (rgb_idx != -1 && colors)
149 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
151 unsigned char color[3];
152 colors->GetTupleValue (i, color);
154 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
155 pcl::setFieldValue<PointT, std::uint32_t> (cloud.
points[i], rgb_idx, rgb.rgba);
161 template <
typename Po
intT>
void
165 structured_grid->GetDimensions (dimensions);
166 cloud.
width = dimensions[0];
167 cloud.
height = dimensions[1];
172 std::vector<pcl::PCLPointField> fields;
176 int x_idx = -1, y_idx = -1, z_idx = -1;
177 for (std::size_t d = 0; d < fields.size (); ++d)
179 if (fields[d].name ==
"x")
180 x_idx = fields[d].offset;
181 else if (fields[d].name ==
"y")
182 y_idx = fields[d].offset;
183 else if (fields[d].name ==
"z")
184 z_idx = fields[d].offset;
187 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
189 for (std::size_t i = 0; i < cloud.
width; ++i)
191 for (std::size_t j = 0; j < cloud.
height; ++j)
193 int queryPoint[3] = {i, j, 0};
194 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
195 double coordinate[3];
196 if (structured_grid->IsPointVisible (pointId))
198 structured_grid->GetPoint (pointId, coordinate);
199 pcl::setFieldValue<PointT, float> (cloud (i, j), x_idx, coordinate[0]);
200 pcl::setFieldValue<PointT, float> (cloud (i, j), y_idx, coordinate[1]);
201 pcl::setFieldValue<PointT, float> (cloud (i, j), z_idx, coordinate[2]);
212 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
213 for (std::size_t d = 0; d < fields.size (); ++d)
215 if (fields[d].name ==
"normal_x")
216 normal_x_idx = fields[d].offset;
217 else if (fields[d].name ==
"normal_y")
218 normal_y_idx = fields[d].offset;
219 else if (fields[d].name ==
"normal_z")
220 normal_z_idx = fields[d].offset;
223 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
225 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
227 for (std::size_t i = 0; i < cloud.
width; ++i)
229 for (std::size_t j = 0; j < cloud.
height; ++j)
231 int queryPoint[3] = {i, j, 0};
232 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
234 if (structured_grid->IsPointVisible (pointId))
236 normals->GetTupleValue (i, normal);
237 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_x_idx, normal[0]);
238 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_y_idx, normal[1]);
239 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_z_idx, normal[2]);
250 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray (
"Colors"));
252 for (std::size_t d = 0; d < fields.size (); ++d)
254 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
256 rgb_idx = fields[d].offset;
261 if (rgb_idx != -1 && colors)
263 for (std::size_t i = 0; i < cloud.
width; ++i)
265 for (std::size_t j = 0; j < cloud.
height; ++j)
267 int queryPoint[3] = {i, j, 0};
268 vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint);
269 unsigned char color[3];
270 if (structured_grid->IsPointVisible (pointId))
272 colors->GetTupleValue (i, color);
274 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
275 pcl::setFieldValue<PointT, std::uint32_t> (cloud (i, j), rgb_idx, rgb.rgba);
287 template <
typename Po
intT>
void
291 std::vector<pcl::PCLPointField> fields;
295 vtkIdType nr_points = cloud.
points.size ();
297 points->SetNumberOfPoints (nr_points);
299 float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
304 for (vtkIdType i = 0; i < nr_points; ++i)
305 memcpy (&data[i * 3], &cloud[i].x, 12);
310 for (vtkIdType i = 0; i < nr_points; ++i)
313 if (!std::isfinite (cloud[i].x) ||
314 !std::isfinite (cloud[i].y) ||
315 !std::isfinite (cloud[i].z))
318 memcpy (&data[j * 3], &cloud[i].x, 12);
322 points->SetNumberOfPoints (nr_points);
327 temp_polydata->SetPoints (points);
330 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
331 for (std::size_t d = 0; d < fields.size (); ++d)
333 if (fields[d].name ==
"normal_x")
334 normal_x_idx = fields[d].offset;
335 else if (fields[d].name ==
"normal_y")
336 normal_y_idx = fields[d].offset;
337 else if (fields[d].name ==
"normal_z")
338 normal_z_idx = fields[d].offset;
340 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
343 normals->SetNumberOfComponents (3);
344 normals->SetNumberOfTuples (cloud.
size ());
345 normals->SetName (
"Normals");
347 for (std::size_t i = 0; i < cloud.
size (); ++i)
350 pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
351 pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
352 pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
353 normals->SetTupleValue (i, normal);
355 temp_polydata->GetPointData ()->SetNormals (normals);
360 for (std::size_t d = 0; d < fields.size (); ++d)
362 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
364 rgb_idx = fields[d].offset;
371 colors->SetNumberOfComponents (3);
372 colors->SetNumberOfTuples (cloud.
size ());
373 colors->SetName (
"RGB");
375 for (std::size_t i = 0; i < cloud.
size (); ++i)
377 unsigned char color[3];
379 pcl::getFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
380 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
381 colors->SetTupleValue (i, color);
383 temp_polydata->GetPointData ()->SetScalars (colors);
388 vertex_glyph_filter->SetInputData (temp_polydata);
389 vertex_glyph_filter->Update ();
391 pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
395 template <
typename Po
intT>
void
399 std::vector<pcl::PCLPointField> fields;
402 int dimensions[3] = {cloud.
width, cloud.
height, 1};
403 structured_grid->SetDimensions (dimensions);
406 points->SetNumberOfPoints (cloud.
width * cloud.
height);
408 for (std::size_t i = 0; i < cloud.
width; ++i)
410 for (std::size_t j = 0; j < cloud.
height; ++j)
412 int queryPoint[3] = {i, j, 0};
413 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
414 const PointT &point = cloud (i, j);
418 float p[3] = {point.x, point.y, point.z};
419 points->SetPoint (pointId, p);
427 structured_grid->SetPoints (points);
430 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
431 for (std::size_t d = 0; d < fields.size (); ++d)
433 if (fields[d].name ==
"normal_x")
434 normal_x_idx = fields[d].offset;
435 else if (fields[d].name ==
"normal_y")
436 normal_y_idx = fields[d].offset;
437 else if (fields[d].name ==
"normal_z")
438 normal_z_idx = fields[d].offset;
441 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
444 normals->SetNumberOfComponents (3);
445 normals->SetNumberOfTuples (cloud.
width * cloud.
height);
446 normals->SetName (
"Normals");
447 for (std::size_t i = 0; i < cloud.
width; ++i)
449 for (std::size_t j = 0; j < cloud.
height; ++j)
451 int queryPoint[3] = {i, j, 0};
452 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
453 const PointT &point = cloud (i, j);
456 pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]);
457 pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]);
458 pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]);
459 normals->SetTupleValue (pointId, normal);
463 structured_grid->GetPointData ()->SetNormals (normals);
468 for (std::size_t d = 0; d < fields.size (); ++d)
470 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
472 rgb_idx = fields[d].offset;
480 colors->SetNumberOfComponents (3);
481 colors->SetNumberOfTuples (cloud.
width * cloud.
height);
482 colors->SetName (
"Colors");
483 for (std::size_t i = 0; i < cloud.
width; ++i)
485 for (std::size_t j = 0; j < cloud.
height; ++j)
487 int queryPoint[3] = {i, j, 0};
488 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
489 const PointT &point = cloud (i, j);
494 unsigned char color[3];
496 pcl::getFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
497 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
498 colors->SetTupleValue (pointId, color);
505 structured_grid->GetPointData ()->AddArray (colors);
509 #ifdef vtkGenericDataArray_h
511 #undef InsertNextTupleValue
515 #endif //#ifndef PCL_IO_VTK_IO_H_