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 (
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 (
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 (
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 (
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 (
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 (
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, 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 (
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 (
size_t i = 0; i < cloud.
width; ++i)
191 for (
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 (
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 (
size_t i = 0; i < cloud.
width; ++i)
229 for (
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 (
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 (
size_t i = 0; i < cloud.
width; ++i)
265 for (
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, 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 (!pcl_isfinite (cloud[i].x) ||
314 !pcl_isfinite (cloud[i].y) ||
315 !pcl_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 (
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 (
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 (
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 (
size_t i = 0; i < cloud.
size (); ++i)
377 unsigned char color[3];
379 pcl::getFieldValue<PointT, 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 #if VTK_MAJOR_VERSION < 6 389 vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ());
391 vertex_glyph_filter->SetInputData (temp_polydata);
393 vertex_glyph_filter->Update ();
395 pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
399 template <
typename Po
intT>
void 403 std::vector<pcl::PCLPointField> fields;
406 int dimensions[3] = {cloud.
width, cloud.
height, 1};
407 structured_grid->SetDimensions (dimensions);
410 points->SetNumberOfPoints (cloud.
width * cloud.
height);
412 for (
size_t i = 0; i < cloud.
width; ++i)
414 for (
size_t j = 0; j < cloud.
height; ++j)
416 int queryPoint[3] = {i, j, 0};
417 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
418 const PointT &point = cloud (i, j);
422 float p[3] = {point.x, point.y, point.z};
423 points->SetPoint (pointId, p);
431 structured_grid->SetPoints (points);
434 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
435 for (
size_t d = 0; d < fields.size (); ++d)
437 if (fields[d].name ==
"normal_x")
438 normal_x_idx = fields[d].offset;
439 else if (fields[d].name ==
"normal_y")
440 normal_y_idx = fields[d].offset;
441 else if (fields[d].name ==
"normal_z")
442 normal_z_idx = fields[d].offset;
445 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
448 normals->SetNumberOfComponents (3);
449 normals->SetNumberOfTuples (cloud.
width * cloud.
height);
450 normals->SetName (
"Normals");
451 for (
size_t i = 0; i < cloud.
width; ++i)
453 for (
size_t j = 0; j < cloud.
height; ++j)
455 int queryPoint[3] = {i, j, 0};
456 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
457 const PointT &point = cloud (i, j);
460 pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]);
461 pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]);
462 pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]);
463 normals->SetTupleValue (pointId, normal);
467 structured_grid->GetPointData ()->SetNormals (normals);
472 for (
size_t d = 0; d < fields.size (); ++d)
474 if (fields[d].name ==
"rgb" || fields[d].name ==
"rgba")
476 rgb_idx = fields[d].offset;
484 colors->SetNumberOfComponents (3);
485 colors->SetNumberOfTuples (cloud.
width * cloud.
height);
486 colors->SetName (
"Colors");
487 for (
size_t i = 0; i < cloud.
width; ++i)
489 for (
size_t j = 0; j < cloud.
height; ++j)
491 int queryPoint[3] = {i, j, 0};
492 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
493 const PointT &point = cloud (i, j);
498 unsigned char color[3];
500 pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
501 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
502 colors->SetTupleValue (pointId, color);
509 structured_grid->GetPointData ()->AddArray (colors);
513 #ifdef vtkGenericDataArray_h 515 #undef InsertNextTupleValue 519 #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 return true if f...
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.
uint32_t height
The point cloud height (if organized as an image-structure).
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Defines all the PCL implemented PointT point type structures.
void vtkPolyDataToPointCloud(vtkPolyData *const polydata, pcl::PointCloud< PointT > &cloud)
Convert a VTK PolyData object to a pcl::PointCloud one.
PointCloud represents the base class in PCL for storing collections of 3D points.
void vtkStructuredGridToPointCloud(vtkStructuredGrid *const structured_grid, pcl::PointCloud< PointT > &cloud)
Convert a VTK StructuredGrid object to a pcl::PointCloud one.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
A point structure representing Euclidean xyz coordinates, and the RGB color.
void pointCloudTovtkStructuredGrid(const pcl::PointCloud< PointT > &cloud, vtkStructuredGrid *const structured_grid)
Convert a pcl::PointCloud object to a VTK StructuredGrid one.