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 <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 template <
typename Po
intT>
void
70 cloud.
width = polydata->GetNumberOfPoints ();
76 std::vector<pcl::PCLPointField> fields;
80 int x_idx = -1, y_idx = -1, z_idx = -1;
81 for (
size_t d = 0; d < fields.size (); ++d)
83 if (fields[d].name ==
"x")
84 x_idx = fields[d].offset;
85 else if (fields[d].name ==
"y")
86 y_idx = fields[d].offset;
87 else if (fields[d].name ==
"z")
88 z_idx = fields[d].offset;
91 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
93 for (
size_t i = 0; i < cloud.
points.size (); ++i)
96 polydata->GetPoint (i, coordinate);
97 pcl::setFieldValue<PointT, float> (cloud.
points[i], x_idx, coordinate[0]);
98 pcl::setFieldValue<PointT, float> (cloud.
points[i], y_idx, coordinate[1]);
99 pcl::setFieldValue<PointT, float> (cloud.
points[i], z_idx, coordinate[2]);
104 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
105 for (
size_t d = 0; d < fields.size (); ++d)
107 if (fields[d].name ==
"normal_x")
108 normal_x_idx = fields[d].offset;
109 else if (fields[d].name ==
"normal_y")
110 normal_y_idx = fields[d].offset;
111 else if (fields[d].name ==
"normal_z")
112 normal_z_idx = fields[d].offset;
115 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
116 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
118 for (
size_t i = 0; i < cloud.
points.size (); ++i)
121 normals->GetTupleValue (i, normal);
122 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_x_idx, normal[0]);
123 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_y_idx, normal[1]);
124 pcl::setFieldValue<PointT, float> (cloud.
points[i], normal_z_idx, normal[2]);
129 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
131 for (
size_t d = 0; d < fields.size (); ++d)
133 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
135 rgb_idx = fields[d].offset;
140 if (rgb_idx != -1 && colors)
142 for (
size_t i = 0; i < cloud.
points.size (); ++i)
144 unsigned char color[3];
145 colors->GetTupleValue (i, color);
147 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
148 pcl::setFieldValue<PointT, uint32_t> (cloud.
points[i], rgb_idx, rgb.rgba);
154 template <
typename Po
intT>
void
158 structured_grid->GetDimensions (dimensions);
159 cloud.
width = dimensions[0];
160 cloud.
height = dimensions[1];
165 std::vector<pcl::PCLPointField> fields;
169 int x_idx = -1, y_idx = -1, z_idx = -1;
170 for (
size_t d = 0; d < fields.size (); ++d)
172 if (fields[d].name ==
"x")
173 x_idx = fields[d].offset;
174 else if (fields[d].name ==
"y")
175 y_idx = fields[d].offset;
176 else if (fields[d].name ==
"z")
177 z_idx = fields[d].offset;
180 if (x_idx != -1 && y_idx != -1 && z_idx != -1)
182 for (
size_t i = 0; i < cloud.
width; ++i)
184 for (
size_t j = 0; j < cloud.
height; ++j)
186 int queryPoint[3] = {i, j, 0};
187 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
188 double coordinate[3];
189 if (structured_grid->IsPointVisible (pointId))
191 structured_grid->GetPoint (pointId, coordinate);
192 pcl::setFieldValue<PointT, float> (cloud (i, j), x_idx, coordinate[0]);
193 pcl::setFieldValue<PointT, float> (cloud (i, j), y_idx, coordinate[1]);
194 pcl::setFieldValue<PointT, float> (cloud (i, j), z_idx, coordinate[2]);
205 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
206 for (
size_t d = 0; d < fields.size (); ++d)
208 if (fields[d].name ==
"normal_x")
209 normal_x_idx = fields[d].offset;
210 else if (fields[d].name ==
"normal_y")
211 normal_y_idx = fields[d].offset;
212 else if (fields[d].name ==
"normal_z")
213 normal_z_idx = fields[d].offset;
216 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
218 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
220 for (
size_t i = 0; i < cloud.
width; ++i)
222 for (
size_t j = 0; j < cloud.
height; ++j)
224 int queryPoint[3] = {i, j, 0};
225 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
227 if (structured_grid->IsPointVisible (pointId))
229 normals->GetTupleValue (i, normal);
230 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_x_idx, normal[0]);
231 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_y_idx, normal[1]);
232 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_z_idx, normal[2]);
243 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray (
"Colors"));
245 for (
size_t d = 0; d < fields.size (); ++d)
247 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
249 rgb_idx = fields[d].offset;
254 if (rgb_idx != -1 && colors)
256 for (
size_t i = 0; i < cloud.
width; ++i)
258 for (
size_t j = 0; j < cloud.
height; ++j)
260 int queryPoint[3] = {i, j, 0};
261 vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint);
262 unsigned char color[3];
263 if (structured_grid->IsPointVisible (pointId))
265 colors->GetTupleValue (i, color);
267 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
268 pcl::setFieldValue<PointT, uint32_t> (cloud (i, j), rgb_idx, rgb.rgba);
280 template <
typename Po
intT>
void
284 std::vector<pcl::PCLPointField> fields;
288 vtkIdType nr_points = cloud.
points.size ();
290 points->SetNumberOfPoints (nr_points);
292 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
297 for (vtkIdType i = 0; i < nr_points; ++i)
298 memcpy (&data[i * 3], &cloud[i].x, 12);
303 for (vtkIdType i = 0; i < nr_points; ++i)
306 if (!pcl_isfinite (cloud[i].x) ||
307 !pcl_isfinite (cloud[i].y) ||
308 !pcl_isfinite (cloud[i].z))
311 memcpy (&data[j * 3], &cloud[i].x, 12);
315 points->SetNumberOfPoints (nr_points);
320 temp_polydata->SetPoints (points);
323 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
324 for (
size_t d = 0; d < fields.size (); ++d)
326 if (fields[d].name ==
"normal_x")
327 normal_x_idx = fields[d].offset;
328 else if (fields[d].name ==
"normal_y")
329 normal_y_idx = fields[d].offset;
330 else if (fields[d].name ==
"normal_z")
331 normal_z_idx = fields[d].offset;
333 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
336 normals->SetNumberOfComponents (3);
337 normals->SetNumberOfTuples (cloud.
size ());
338 normals->SetName (
"Normals");
340 for (
size_t i = 0; i < cloud.
size (); ++i)
343 pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
344 pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
345 pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
346 normals->SetTupleValue (i, normal);
348 temp_polydata->GetPointData ()->SetNormals (normals);
353 for (
size_t d = 0; d < fields.size (); ++d)
355 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
357 rgb_idx = fields[d].offset;
364 colors->SetNumberOfComponents (3);
365 colors->SetNumberOfTuples (cloud.
size ());
366 colors->SetName (
"RGB");
368 for (
size_t i = 0; i < cloud.
size (); ++i)
370 unsigned char color[3];
372 pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
373 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
374 colors->SetTupleValue (i, color);
376 temp_polydata->GetPointData ()->SetScalars (colors);
381 #if VTK_MAJOR_VERSION < 6
382 vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ());
384 vertex_glyph_filter->SetInputData (temp_polydata);
386 vertex_glyph_filter->Update ();
388 pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
392 template <
typename Po
intT>
void
396 std::vector<pcl::PCLPointField> fields;
399 int dimensions[3] = {cloud.
width, cloud.
height, 1};
400 structured_grid->SetDimensions (dimensions);
403 points->SetNumberOfPoints (cloud.
width * cloud.
height);
405 for (
size_t i = 0; i < cloud.
width; ++i)
407 for (
size_t j = 0; j < cloud.
height; ++j)
409 int queryPoint[3] = {i, j, 0};
410 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
411 const PointT &point = cloud (i, j);
415 float p[3] = {point.x, point.y, point.z};
416 points->SetPoint (pointId, p);
424 structured_grid->SetPoints (points);
427 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
428 for (
size_t d = 0; d < fields.size (); ++d)
430 if (fields[d].name ==
"normal_x")
431 normal_x_idx = fields[d].offset;
432 else if (fields[d].name ==
"normal_y")
433 normal_y_idx = fields[d].offset;
434 else if (fields[d].name ==
"normal_z")
435 normal_z_idx = fields[d].offset;
438 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
441 normals->SetNumberOfComponents (3);
442 normals->SetNumberOfTuples (cloud.
width * cloud.
height);
443 normals->SetName (
"Normals");
444 for (
size_t i = 0; i < cloud.
width; ++i)
446 for (
size_t j = 0; j < cloud.
height; ++j)
448 int queryPoint[3] = {i, j, 0};
449 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
450 const PointT &point = cloud (i, j);
453 pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]);
454 pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]);
455 pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]);
456 normals->SetTupleValue (pointId, normal);
460 structured_grid->GetPointData ()->SetNormals (normals);
465 for (
size_t d = 0; d < fields.size (); ++d)
467 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
469 rgb_idx = fields[d].offset;
477 colors->SetNumberOfComponents (3);
478 colors->SetNumberOfTuples (cloud.
width * cloud.
height);
479 colors->SetName (
"Colors");
480 for (
size_t i = 0; i < cloud.
width; ++i)
482 for (
size_t j = 0; j < cloud.
height; ++j)
484 int queryPoint[3] = {i, j, 0};
485 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
486 const PointT &point = cloud (i, j);
491 unsigned char color[3];
493 pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
494 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
495 colors->SetTupleValue (pointId, color);
502 structured_grid->GetPointData ()->AddArray (colors);
506 #endif //#ifndef PCL_IO_VTK_IO_H_
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void pointCloudTovtkPolyData(const pcl::PointCloud< PointT > &cloud, vtkPolyData *const polydata)
Convert a pcl::PointCloud object to a VTK PolyData one.
uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
void vtkStructuredGridToPointCloud(vtkStructuredGrid *const structured_grid, pcl::PointCloud< PointT > &cloud)
Convert a VTK StructuredGrid object to a pcl::PointCloud one.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void pointCloudTovtkStructuredGrid(const pcl::PointCloud< PointT > &cloud, vtkStructuredGrid *const structured_grid)
Convert a pcl::PointCloud object to a VTK StructuredGrid one.
uint32_t height
The point cloud height (if organized as an image-structure).