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>
46 #include <pcl/point_types.h>
47 #include <pcl/point_traits.h>
52 #pragma GCC system_header
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 template <
typename Po
intT>
void
69 cloud.
width = polydata->GetNumberOfPoints ();
75 std::vector<pcl::PCLPointField> fields;
79 int x_idx = -1, y_idx = -1, z_idx = -1;
80 for (
size_t d = 0; d < fields.size (); ++d)
82 if (fields[d].name ==
"x")
83 x_idx = fields[d].offset;
84 else if (fields[d].name ==
"y")
85 y_idx = fields[d].offset;
86 else if (fields[d].name ==
"z")
87 z_idx = fields[d].offset;
90 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
92 for (
size_t i = 0; i < cloud.
points.size (); ++i)
95 polydata->GetPoint (i, coordinate);
96 pcl::setFieldValue<PointT, float> (cloud.
points[i], x_idx, coordinate[0]);
97 pcl::setFieldValue<PointT, float> (cloud.
points[i], y_idx, coordinate[1]);
98 pcl::setFieldValue<PointT, float> (cloud.
points[i], z_idx, coordinate[2]);
103 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
104 for (
size_t d = 0; d < fields.size (); ++d)
106 if (fields[d].name ==
"normal_x")
107 normal_x_idx = fields[d].offset;
108 else if (fields[d].name ==
"normal_y")
109 normal_y_idx = fields[d].offset;
110 else if (fields[d].name ==
"normal_z")
111 normal_z_idx = fields[d].offset;
114 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
115 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
117 for (
size_t i = 0; i < cloud.
points.size (); ++i)
120 normals->GetTupleValue (i, normal);
121 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_x_idx, normal[0]);
122 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_y_idx, normal[1]);
123 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_z_idx, normal[2]);
128 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
130 for (
size_t d = 0; d < fields.size (); ++d)
132 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
134 rgb_idx = fields[d].offset;
139 if (rgb_idx != -1 && colors)
141 for (
size_t i = 0; i < cloud.
points.size (); ++i)
143 unsigned char color[3];
144 colors->GetTupleValue (i, color);
146 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
147 pcl::setFieldValue<PointT, uint32_t> (cloud.
points[i], rgb_idx, rgb.rgba);
153 template <
typename Po
intT>
void
157 structured_grid->GetDimensions (dimensions);
158 cloud.
width = dimensions[0];
159 cloud.
height = dimensions[1];
164 std::vector<pcl::PCLPointField> fields;
168 int x_idx = -1, y_idx = -1, z_idx = -1;
169 for (
size_t d = 0; d < fields.size (); ++d)
171 if (fields[d].name ==
"x")
172 x_idx = fields[d].offset;
173 else if (fields[d].name ==
"y")
174 y_idx = fields[d].offset;
175 else if (fields[d].name ==
"z")
176 z_idx = fields[d].offset;
179 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
181 for (
size_t i = 0; i < cloud.
width; ++i)
183 for (
size_t j = 0; j < cloud.
height; ++j)
185 int queryPoint[3] = {i, j, 0};
186 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
187 double coordinate[3];
188 if (structured_grid->IsPointVisible (pointId))
190 structured_grid->GetPoint (pointId, coordinate);
191 pcl::setFieldValue<PointT, float> (cloud (i, j), x_idx, coordinate[0]);
192 pcl::setFieldValue<PointT, float> (cloud (i, j), y_idx, coordinate[1]);
193 pcl::setFieldValue<PointT, float> (cloud (i, j), z_idx, coordinate[2]);
204 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
205 for (
size_t d = 0; d < fields.size (); ++d)
207 if (fields[d].name ==
"normal_x")
208 normal_x_idx = fields[d].offset;
209 else if (fields[d].name ==
"normal_y")
210 normal_y_idx = fields[d].offset;
211 else if (fields[d].name ==
"normal_z")
212 normal_z_idx = fields[d].offset;
215 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
217 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
219 for (
size_t i = 0; i < cloud.
width; ++i)
221 for (
size_t j = 0; j < cloud.
height; ++j)
223 int queryPoint[3] = {i, j, 0};
224 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
226 if (structured_grid->IsPointVisible (pointId))
228 normals->GetTupleValue (i, normal);
229 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_x_idx, normal[0]);
230 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_y_idx, normal[1]);
231 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_z_idx, normal[2]);
242 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray (
"Colors"));
244 for (
size_t d = 0; d < fields.size (); ++d)
246 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
248 rgb_idx = fields[d].offset;
253 if (rgb_idx != -1 && colors)
255 for (
size_t i = 0; i < cloud.
width; ++i)
257 for (
size_t j = 0; j < cloud.
height; ++j)
259 int queryPoint[3] = {i, j, 0};
260 vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint);
261 unsigned char color[3];
262 if (structured_grid->IsPointVisible (pointId))
264 colors->GetTupleValue (i, color);
266 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
267 pcl::setFieldValue<PointT, uint32_t> (cloud (i, j), rgb_idx, rgb.rgba);
279 template <
typename Po
intT>
void
283 std::vector<pcl::PCLPointField> fields;
287 vtkIdType nr_points = cloud.
points.size ();
289 points->SetNumberOfPoints (nr_points);
291 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
296 for (vtkIdType i = 0; i < nr_points; ++i)
297 memcpy (&data[i * 3], &cloud[i].x, 12);
302 for (vtkIdType i = 0; i < nr_points; ++i)
305 if (!pcl_isfinite (cloud[i].x) ||
306 !pcl_isfinite (cloud[i].y) ||
307 !pcl_isfinite (cloud[i].z))
310 memcpy (&data[j * 3], &cloud[i].x, 12);
314 points->SetNumberOfPoints (nr_points);
319 temp_polydata->SetPoints (points);
322 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
323 for (
size_t d = 0; d < fields.size (); ++d)
325 if (fields[d].name ==
"normal_x")
326 normal_x_idx = fields[d].offset;
327 else if (fields[d].name ==
"normal_y")
328 normal_y_idx = fields[d].offset;
329 else if (fields[d].name ==
"normal_z")
330 normal_z_idx = fields[d].offset;
332 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
335 normals->SetNumberOfComponents (3);
336 normals->SetNumberOfTuples (cloud.
size ());
337 normals->SetName (
"Normals");
339 for (
size_t i = 0; i < cloud.
size (); ++i)
342 pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
343 pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
344 pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
345 normals->SetTupleValue (i, normal);
347 temp_polydata->GetPointData ()->SetNormals (normals);
352 for (
size_t d = 0; d < fields.size (); ++d)
354 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
356 rgb_idx = fields[d].offset;
363 colors->SetNumberOfComponents (3);
364 colors->SetNumberOfTuples (cloud.
size ());
365 colors->SetName (
"RGB");
367 for (
size_t i = 0; i < cloud.
size (); ++i)
369 unsigned char color[3];
371 pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
372 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
373 colors->SetTupleValue (i, color);
375 temp_polydata->GetPointData ()->SetScalars (colors);
380 #if VTK_MAJOR_VERSION <= 5
381 vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ());
383 vertex_glyph_filter->SetInputData (temp_polydata);
385 vertex_glyph_filter->Update ();
387 pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
391 template <
typename Po
intT>
void
395 std::vector<pcl::PCLPointField> fields;
398 int dimensions[3] = {cloud.
width, cloud.
height, 1};
399 structured_grid->SetDimensions (dimensions);
402 points->SetNumberOfPoints (cloud.
width * cloud.
height);
404 for (
size_t i = 0; i < cloud.
width; ++i)
406 for (
size_t j = 0; j < cloud.
height; ++j)
408 int queryPoint[3] = {i, j, 0};
409 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
410 const PointT &point = cloud (i, j);
414 float p[3] = {point.x, point.y, point.z};
415 points->SetPoint (pointId, p);
423 structured_grid->SetPoints (points);
426 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
427 for (
size_t d = 0; d < fields.size (); ++d)
429 if (fields[d].name ==
"normal_x")
430 normal_x_idx = fields[d].offset;
431 else if (fields[d].name ==
"normal_y")
432 normal_y_idx = fields[d].offset;
433 else if (fields[d].name ==
"normal_z")
434 normal_z_idx = fields[d].offset;
437 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
440 normals->SetNumberOfComponents (3);
441 normals->SetNumberOfTuples (cloud.
width * cloud.
height);
442 normals->SetName (
"Normals");
443 for (
size_t i = 0; i < cloud.
width; ++i)
445 for (
size_t j = 0; j < cloud.
height; ++j)
447 int queryPoint[3] = {i, j, 0};
448 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
449 const PointT &point = cloud (i, j);
452 pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]);
453 pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]);
454 pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]);
455 normals->SetTupleValue (pointId, normal);
459 structured_grid->GetPointData ()->SetNormals (normals);
464 for (
size_t d = 0; d < fields.size (); ++d)
466 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
468 rgb_idx = fields[d].offset;
476 colors->SetNumberOfComponents (3);
477 colors->SetNumberOfTuples (cloud.
width * cloud.
height);
478 colors->SetName (
"Colors");
479 for (
size_t i = 0; i < cloud.
width; ++i)
481 for (
size_t j = 0; j < cloud.
height; ++j)
483 int queryPoint[3] = {i, j, 0};
484 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
485 const PointT &point = cloud (i, j);
490 unsigned char color[3];
492 pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
493 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
494 colors->SetTupleValue (pointId, color);
501 structured_grid->GetPointData ()->AddArray (colors);
505 #endif //#ifndef PCL_IO_VTK_IO_H_
uint32_t width
The point cloud width (if organized as an image-structure).
void vtkStructuredGridToPointCloud(vtkStructuredGrid *const structured_grid, pcl::PointCloud< PointT > &cloud)
Convert a VTK StructuredGrid object to a pcl::PointCloud one.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
A structure representing RGB color information.
void vtkPolyDataToPointCloud(vtkPolyData *const polydata, pcl::PointCloud< PointT > &cloud)
Convert a VTK PolyData object to a pcl::PointCloud one.
uint32_t height
The point cloud height (if organized as an image-structure).
void pointCloudTovtkStructuredGrid(const pcl::PointCloud< PointT > &cloud, vtkStructuredGrid *const structured_grid)
Convert a pcl::PointCloud object to a VTK StructuredGrid one.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void pointCloudTovtkPolyData(const pcl::PointCloud< PointT > &cloud, vtkPolyData *const polydata)
Convert a pcl::PointCloud object to a VTK PolyData one.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
A point structure representing Euclidean xyz coordinates, and the RGB color.