Point Cloud Library (PCL)  1.9.1-dev
vtk_lib_io.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: vtk_io.hpp 4968 2012-05-03 06:39:52Z doria $
37  *
38  */
39 
40 #ifndef PCL_IO_VTK_IO_IMPL_H_
41 #define PCL_IO_VTK_IO_IMPL_H_
42 
43 // PCL
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>
48 
49 // VTK
50 // Ignore warnings in the above headers
51 #ifdef __GNUC__
52 #pragma GCC system_header
53 #endif
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>
63 
64 // Support for VTK 7.1 upwards
65 #ifdef vtkGenericDataArray_h
66 #define SetTupleValue SetTypedTuple
67 #define InsertNextTupleValue InsertNextTypedTuple
68 #define GetTupleValue GetTypedTuple
69 #endif
70 
71 ///////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointT> void
73 pcl::io::vtkPolyDataToPointCloud (vtkPolyData* const polydata, pcl::PointCloud<PointT>& cloud)
74 {
75  // This generic template will convert any VTK PolyData
76  // to a coordinate-only PointXYZ PCL format.
77  cloud.width = polydata->GetNumberOfPoints ();
78  cloud.height = 1; // This indicates that the point cloud is unorganized
79  cloud.is_dense = false;
80  cloud.points.resize (cloud.width * cloud.height);
81 
82  // Get a list of all the fields available
83  std::vector<pcl::PCLPointField> fields;
84  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
85 
86  // Check if XYZ is present
87  int x_idx = -1, y_idx = -1, z_idx = -1;
88  for (size_t d = 0; d < fields.size (); ++d)
89  {
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;
96  }
97  // Set the coordinates of the pcl::PointCloud (if the pcl::PointCloud supports coordinates)
98  if (x_idx != -1 && y_idx != -1 && z_idx != -1)
99  {
100  for (size_t i = 0; i < cloud.points.size (); ++i)
101  {
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]);
107  }
108  }
109 
110  // Check if Normals are present
111  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
112  for (size_t d = 0; d < fields.size (); ++d)
113  {
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;
120  }
121  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkPolyData has normals)
122  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
123  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
124  {
125  for (size_t i = 0; i < cloud.points.size (); ++i)
126  {
127  float normal[3];
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]);
132  }
133  }
134 
135  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkPolyData has colors)
136  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
137  int rgb_idx = -1;
138  for (size_t d = 0; d < fields.size (); ++d)
139  {
140  if (fields[d].name == "rgb" || fields[d].name == "rgba")
141  {
142  rgb_idx = fields[d].offset;
143  break;
144  }
145  }
146 
147  if (rgb_idx != -1 && colors)
148  {
149  for (size_t i = 0; i < cloud.points.size (); ++i)
150  {
151  unsigned char color[3];
152  colors->GetTupleValue (i, color);
153  pcl::RGB rgb;
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);
156  }
157  }
158 }
159 
160 ///////////////////////////////////////////////////////////////////////////////////////////
161 template <typename PointT> void
162 pcl::io::vtkStructuredGridToPointCloud (vtkStructuredGrid* const structured_grid, pcl::PointCloud<PointT>& cloud)
163 {
164  int dimensions[3];
165  structured_grid->GetDimensions (dimensions);
166  cloud.width = dimensions[0];
167  cloud.height = dimensions[1]; // This indicates that the point cloud is organized
168  cloud.is_dense = true;
169  cloud.points.resize (cloud.width * cloud.height);
170 
171  // Get a list of all the fields available
172  std::vector<pcl::PCLPointField> fields;
173  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
174 
175  // Check if XYZ is present
176  int x_idx = -1, y_idx = -1, z_idx = -1;
177  for (size_t d = 0; d < fields.size (); ++d)
178  {
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;
185  }
186 
187  if (x_idx != -1 && y_idx != -1 && z_idx != -1)
188  {
189  for (size_t i = 0; i < cloud.width; ++i)
190  {
191  for (size_t j = 0; j < cloud.height; ++j)
192  {
193  int queryPoint[3] = {i, j, 0};
194  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
195  double coordinate[3];
196  if (structured_grid->IsPointVisible (pointId))
197  {
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]);
202  }
203  else
204  {
205  // Fill the point with an "empty" point?
206  }
207  }
208  }
209  }
210 
211  // Check if Normals are present
212  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
213  for (size_t d = 0; d < fields.size (); ++d)
214  {
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;
221  }
222  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkStructuredGrid has normals)
223  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
224 
225  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
226  {
227  for (size_t i = 0; i < cloud.width; ++i)
228  {
229  for (size_t j = 0; j < cloud.height; ++j)
230  {
231  int queryPoint[3] = {i, j, 0};
232  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
233  float normal[3];
234  if (structured_grid->IsPointVisible (pointId))
235  {
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]);
240  }
241  else
242  {
243  // Fill the point with an "empty" point?
244  }
245  }
246  }
247  }
248 
249  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkStructuredGrid has colors)
250  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray ("Colors"));
251  int rgb_idx = -1;
252  for (size_t d = 0; d < fields.size (); ++d)
253  {
254  if (fields[d].name == "rgb" || fields[d].name == "rgba")
255  {
256  rgb_idx = fields[d].offset;
257  break;
258  }
259  }
260 
261  if (rgb_idx != -1 && colors)
262  {
263  for (size_t i = 0; i < cloud.width; ++i)
264  {
265  for (size_t j = 0; j < cloud.height; ++j)
266  {
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))
271  {
272  colors->GetTupleValue (i, color);
273  pcl::RGB rgb;
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);
276  }
277  else
278  {
279  // Fill the point with an "empty" point?
280  }
281  }
282  }
283  }
284 }
285 
286 ///////////////////////////////////////////////////////////////////////////////////////////
287 template <typename PointT> void
288 pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud<PointT>& cloud, vtkPolyData* const pdata)
289 {
290  // Get a list of all the fields available
291  std::vector<pcl::PCLPointField> fields;
292  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
293 
294  // Coordinates (always must have coordinates)
295  vtkIdType nr_points = cloud.points.size ();
297  points->SetNumberOfPoints (nr_points);
298  // Get a pointer to the beginning of the data array
299  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
300 
301  // Set the points
302  if (cloud.is_dense)
303  {
304  for (vtkIdType i = 0; i < nr_points; ++i)
305  memcpy (&data[i * 3], &cloud[i].x, 12); // sizeof (float) * 3
306  }
307  else
308  {
309  vtkIdType j = 0; // true point index
310  for (vtkIdType i = 0; i < nr_points; ++i)
311  {
312  // Check if the point is invalid
313  if (!std::isfinite (cloud[i].x) ||
314  !std::isfinite (cloud[i].y) ||
315  !std::isfinite (cloud[i].z))
316  continue;
317 
318  memcpy (&data[j * 3], &cloud[i].x, 12); // sizeof (float) * 3
319  j++;
320  }
321  nr_points = j;
322  points->SetNumberOfPoints (nr_points);
323  }
324 
325  // Create a temporary PolyData and add the points to it
327  temp_polydata->SetPoints (points);
328 
329  // Check if Normals are present
330  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
331  for (size_t d = 0; d < fields.size (); ++d)
332  {
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;
339  }
340  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
341  {
343  normals->SetNumberOfComponents (3); //3d normals (ie x,y,z)
344  normals->SetNumberOfTuples (cloud.size ());
345  normals->SetName ("Normals");
346 
347  for (size_t i = 0; i < cloud.size (); ++i)
348  {
349  float normal[3];
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);
354  }
355  temp_polydata->GetPointData ()->SetNormals (normals);
356  }
357 
358  // Colors (optional)
359  int rgb_idx = -1;
360  for (size_t d = 0; d < fields.size (); ++d)
361  {
362  if (fields[d].name == "rgb" || fields[d].name == "rgba")
363  {
364  rgb_idx = fields[d].offset;
365  break;
366  }
367  }
368  if (rgb_idx != -1)
369  {
371  colors->SetNumberOfComponents (3);
372  colors->SetNumberOfTuples (cloud.size ());
373  colors->SetName ("RGB");
374 
375  for (size_t i = 0; i < cloud.size (); ++i)
376  {
377  unsigned char color[3];
378  pcl::RGB rgb;
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);
382  }
383  temp_polydata->GetPointData ()->SetScalars (colors);
384  }
385 
386  // Add 0D topology to every point
388  vertex_glyph_filter->SetInputData (temp_polydata);
389  vertex_glyph_filter->Update ();
390 
391  pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
392 }
393 
394 ///////////////////////////////////////////////////////////////////////////////////////////
395 template <typename PointT> void
396 pcl::io::pointCloudTovtkStructuredGrid (const pcl::PointCloud<PointT>& cloud, vtkStructuredGrid* const structured_grid)
397 {
398  // Get a list of all the fields available
399  std::vector<pcl::PCLPointField> fields;
400  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
401 
402  int dimensions[3] = {cloud.width, cloud.height, 1};
403  structured_grid->SetDimensions (dimensions);
404 
406  points->SetNumberOfPoints (cloud.width * cloud.height);
407 
408  for (size_t i = 0; i < cloud.width; ++i)
409  {
410  for (size_t j = 0; j < cloud.height; ++j)
411  {
412  int queryPoint[3] = {i, j, 0};
413  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
414  const PointT &point = cloud (i, j);
415 
416  if (pcl::isFinite (point))
417  {
418  float p[3] = {point.x, point.y, point.z};
419  points->SetPoint (pointId, p);
420  }
421  else
422  {
423  }
424  }
425  }
426 
427  structured_grid->SetPoints (points);
428 
429  // Check if Normals are present
430  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
431  for (size_t d = 0; d < fields.size (); ++d)
432  {
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;
439  }
440 
441  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
442  {
444  normals->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
445  normals->SetNumberOfTuples (cloud.width * cloud.height);
446  normals->SetName ("Normals");
447  for (size_t i = 0; i < cloud.width; ++i)
448  {
449  for (size_t j = 0; j < cloud.height; ++j)
450  {
451  int queryPoint[3] = {i, j, 0};
452  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
453  const PointT &point = cloud (i, j);
454 
455  float normal[3];
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);
460  }
461  }
462 
463  structured_grid->GetPointData ()->SetNormals (normals);
464  }
465 
466  // Colors (optional)
467  int rgb_idx = -1;
468  for (size_t d = 0; d < fields.size (); ++d)
469  {
470  if (fields[d].name == "rgb" || fields[d].name == "rgba")
471  {
472  rgb_idx = fields[d].offset;
473  break;
474  }
475  }
476 
477  if (rgb_idx != -1)
478  {
480  colors->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
481  colors->SetNumberOfTuples (cloud.width * cloud.height);
482  colors->SetName ("Colors");
483  for (size_t i = 0; i < cloud.width; ++i)
484  {
485  for (size_t j = 0; j < cloud.height; ++j)
486  {
487  int queryPoint[3] = {i, j, 0};
488  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
489  const PointT &point = cloud (i, j);
490 
491  if (pcl::isFinite (point))
492  {
493 
494  unsigned char color[3];
495  pcl::RGB rgb;
496  pcl::getFieldValue<PointT, 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);
499  }
500  else
501  {
502  }
503  }
504  }
505  structured_grid->GetPointData ()->AddArray (colors);
506  }
507 }
508 
509 #ifdef vtkGenericDataArray_h
510 #undef SetTupleValue
511 #undef InsertNextTupleValue
512 #undef GetTupleValue
513 #endif
514 
515 #endif //#ifndef PCL_IO_VTK_IO_H_
516 
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...
Definition: point_tests.h:53
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
void pointCloudTovtkPolyData(const pcl::PointCloud< PointT > &cloud, vtkPolyData *const polydata)
Convert a pcl::PointCloud object to a VTK PolyData one.
Definition: vtk_lib_io.hpp:288
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:428
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:426
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.
Definition: vtk_lib_io.hpp:73
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.
Definition: vtk_lib_io.hpp:162
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:431
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.
Definition: vtk_lib_io.hpp:396
size_t size() const
Definition: point_cloud.h:461