Point Cloud Library (PCL)  1.8.0
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 ///////////////////////////////////////////////////////////////////////////////////////////
65 template <typename PointT> void
66 pcl::io::vtkPolyDataToPointCloud (vtkPolyData* const polydata, pcl::PointCloud<PointT>& cloud)
67 {
68  // This generic template will convert any VTK PolyData
69  // to a coordinate-only PointXYZ PCL format.
70  cloud.width = polydata->GetNumberOfPoints ();
71  cloud.height = 1; // This indicates that the point cloud is unorganized
72  cloud.is_dense = false;
73  cloud.points.resize (cloud.width * cloud.height);
74 
75  // Get a list of all the fields available
76  std::vector<pcl::PCLPointField> fields;
77  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
78 
79  // Check if XYZ is present
80  int x_idx = -1, y_idx = -1, z_idx = -1;
81  for (size_t d = 0; d < fields.size (); ++d)
82  {
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;
89  }
90  // Set the coordinates of the pcl::PointCloud (if the pcl::PointCloud supports coordinates)
91  if (x_idx != -1 && y_idx != -1 && z_idx != -1)
92  {
93  for (size_t i = 0; i < cloud.points.size (); ++i)
94  {
95  double coordinate[3];
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]);
100  }
101  }
102 
103  // Check if Normals are present
104  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
105  for (size_t d = 0; d < fields.size (); ++d)
106  {
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;
113  }
114  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkPolyData has normals)
115  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
116  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
117  {
118  for (size_t i = 0; i < cloud.points.size (); ++i)
119  {
120  float normal[3];
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]);
125  }
126  }
127 
128  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkPolyData has colors)
129  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
130  int rgb_idx = -1;
131  for (size_t d = 0; d < fields.size (); ++d)
132  {
133  if (fields[d].name == "rgb" || fields[d].name == "rgba")
134  {
135  rgb_idx = fields[d].offset;
136  break;
137  }
138  }
139 
140  if (rgb_idx != -1 && colors)
141  {
142  for (size_t i = 0; i < cloud.points.size (); ++i)
143  {
144  unsigned char color[3];
145  colors->GetTupleValue (i, color);
146  pcl::RGB rgb;
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);
149  }
150  }
151 }
152 
153 ///////////////////////////////////////////////////////////////////////////////////////////
154 template <typename PointT> void
155 pcl::io::vtkStructuredGridToPointCloud (vtkStructuredGrid* const structured_grid, pcl::PointCloud<PointT>& cloud)
156 {
157  int dimensions[3];
158  structured_grid->GetDimensions (dimensions);
159  cloud.width = dimensions[0];
160  cloud.height = dimensions[1]; // This indicates that the point cloud is organized
161  cloud.is_dense = true;
162  cloud.points.resize (cloud.width * cloud.height);
163 
164  // Get a list of all the fields available
165  std::vector<pcl::PCLPointField> fields;
166  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
167 
168  // Check if XYZ is present
169  int x_idx = -1, y_idx = -1, z_idx = -1;
170  for (size_t d = 0; d < fields.size (); ++d)
171  {
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;
178  }
179 
180  if (x_idx != -1 && y_idx != -1 && z_idx != -1)
181  {
182  for (size_t i = 0; i < cloud.width; ++i)
183  {
184  for (size_t j = 0; j < cloud.height; ++j)
185  {
186  int queryPoint[3] = {i, j, 0};
187  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
188  double coordinate[3];
189  if (structured_grid->IsPointVisible (pointId))
190  {
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]);
195  }
196  else
197  {
198  // Fill the point with an "empty" point?
199  }
200  }
201  }
202  }
203 
204  // Check if Normals are present
205  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
206  for (size_t d = 0; d < fields.size (); ++d)
207  {
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;
214  }
215  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkStructuredGrid has normals)
216  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
217 
218  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
219  {
220  for (size_t i = 0; i < cloud.width; ++i)
221  {
222  for (size_t j = 0; j < cloud.height; ++j)
223  {
224  int queryPoint[3] = {i, j, 0};
225  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
226  float normal[3];
227  if (structured_grid->IsPointVisible (pointId))
228  {
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]);
233  }
234  else
235  {
236  // Fill the point with an "empty" point?
237  }
238  }
239  }
240  }
241 
242  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkStructuredGrid has colors)
243  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray ("Colors"));
244  int rgb_idx = -1;
245  for (size_t d = 0; d < fields.size (); ++d)
246  {
247  if (fields[d].name == "rgb" || fields[d].name == "rgba")
248  {
249  rgb_idx = fields[d].offset;
250  break;
251  }
252  }
253 
254  if (rgb_idx != -1 && colors)
255  {
256  for (size_t i = 0; i < cloud.width; ++i)
257  {
258  for (size_t j = 0; j < cloud.height; ++j)
259  {
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))
264  {
265  colors->GetTupleValue (i, color);
266  pcl::RGB rgb;
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);
269  }
270  else
271  {
272  // Fill the point with an "empty" point?
273  }
274  }
275  }
276  }
277 }
278 
279 ///////////////////////////////////////////////////////////////////////////////////////////
280 template <typename PointT> void
281 pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud<PointT>& cloud, vtkPolyData* const pdata)
282 {
283  // Get a list of all the fields available
284  std::vector<pcl::PCLPointField> fields;
285  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
286 
287  // Coordinates (always must have coordinates)
288  vtkIdType nr_points = cloud.points.size ();
290  points->SetNumberOfPoints (nr_points);
291  // Get a pointer to the beginning of the data array
292  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
293 
294  // Set the points
295  if (cloud.is_dense)
296  {
297  for (vtkIdType i = 0; i < nr_points; ++i)
298  memcpy (&data[i * 3], &cloud[i].x, 12); // sizeof (float) * 3
299  }
300  else
301  {
302  vtkIdType j = 0; // true point index
303  for (vtkIdType i = 0; i < nr_points; ++i)
304  {
305  // Check if the point is invalid
306  if (!pcl_isfinite (cloud[i].x) ||
307  !pcl_isfinite (cloud[i].y) ||
308  !pcl_isfinite (cloud[i].z))
309  continue;
310 
311  memcpy (&data[j * 3], &cloud[i].x, 12); // sizeof (float) * 3
312  j++;
313  }
314  nr_points = j;
315  points->SetNumberOfPoints (nr_points);
316  }
317 
318  // Create a temporary PolyData and add the points to it
320  temp_polydata->SetPoints (points);
321 
322  // Check if Normals are present
323  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
324  for (size_t d = 0; d < fields.size (); ++d)
325  {
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;
332  }
333  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
334  {
336  normals->SetNumberOfComponents (3); //3d normals (ie x,y,z)
337  normals->SetNumberOfTuples (cloud.size ());
338  normals->SetName ("Normals");
339 
340  for (size_t i = 0; i < cloud.size (); ++i)
341  {
342  float normal[3];
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);
347  }
348  temp_polydata->GetPointData ()->SetNormals (normals);
349  }
350 
351  // Colors (optional)
352  int rgb_idx = -1;
353  for (size_t d = 0; d < fields.size (); ++d)
354  {
355  if (fields[d].name == "rgb" || fields[d].name == "rgba")
356  {
357  rgb_idx = fields[d].offset;
358  break;
359  }
360  }
361  if (rgb_idx != -1)
362  {
364  colors->SetNumberOfComponents (3);
365  colors->SetNumberOfTuples (cloud.size ());
366  colors->SetName ("RGB");
367 
368  for (size_t i = 0; i < cloud.size (); ++i)
369  {
370  unsigned char color[3];
371  pcl::RGB rgb;
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);
375  }
376  temp_polydata->GetPointData ()->SetScalars (colors);
377  }
378 
379  // Add 0D topology to every point
381  #if VTK_MAJOR_VERSION < 6
382  vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ());
383  #else
384  vertex_glyph_filter->SetInputData (temp_polydata);
385  #endif
386  vertex_glyph_filter->Update ();
387 
388  pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
389 }
390 
391 ///////////////////////////////////////////////////////////////////////////////////////////
392 template <typename PointT> void
393 pcl::io::pointCloudTovtkStructuredGrid (const pcl::PointCloud<PointT>& cloud, vtkStructuredGrid* const structured_grid)
394 {
395  // Get a list of all the fields available
396  std::vector<pcl::PCLPointField> fields;
397  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
398 
399  int dimensions[3] = {cloud.width, cloud.height, 1};
400  structured_grid->SetDimensions (dimensions);
401 
403  points->SetNumberOfPoints (cloud.width * cloud.height);
404 
405  for (size_t i = 0; i < cloud.width; ++i)
406  {
407  for (size_t j = 0; j < cloud.height; ++j)
408  {
409  int queryPoint[3] = {i, j, 0};
410  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
411  const PointT &point = cloud (i, j);
412 
413  if (pcl::isFinite (point))
414  {
415  float p[3] = {point.x, point.y, point.z};
416  points->SetPoint (pointId, p);
417  }
418  else
419  {
420  }
421  }
422  }
423 
424  structured_grid->SetPoints (points);
425 
426  // Check if Normals are present
427  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
428  for (size_t d = 0; d < fields.size (); ++d)
429  {
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;
436  }
437 
438  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
439  {
441  normals->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
442  normals->SetNumberOfTuples (cloud.width * cloud.height);
443  normals->SetName ("Normals");
444  for (size_t i = 0; i < cloud.width; ++i)
445  {
446  for (size_t j = 0; j < cloud.height; ++j)
447  {
448  int queryPoint[3] = {i, j, 0};
449  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
450  const PointT &point = cloud (i, j);
451 
452  float normal[3];
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);
457  }
458  }
459 
460  structured_grid->GetPointData ()->SetNormals (normals);
461  }
462 
463  // Colors (optional)
464  int rgb_idx = -1;
465  for (size_t d = 0; d < fields.size (); ++d)
466  {
467  if (fields[d].name == "rgb" || fields[d].name == "rgba")
468  {
469  rgb_idx = fields[d].offset;
470  break;
471  }
472  }
473 
474  if (rgb_idx != -1)
475  {
477  colors->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
478  colors->SetNumberOfTuples (cloud.width * cloud.height);
479  colors->SetName ("Colors");
480  for (size_t i = 0; i < cloud.width; ++i)
481  {
482  for (size_t j = 0; j < cloud.height; ++j)
483  {
484  int queryPoint[3] = {i, j, 0};
485  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
486  const PointT &point = cloud (i, j);
487 
488  if (pcl::isFinite (point))
489  {
490 
491  unsigned char color[3];
492  pcl::RGB rgb;
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);
496  }
497  else
498  {
499  }
500  }
501  }
502  structured_grid->GetPointData ()->AddArray (colors);
503  }
504 }
505 
506 #endif //#ifndef PCL_IO_VTK_IO_H_
507 
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:54
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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:281
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
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:66
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:155
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
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:393
size_t size() const
Definition: point_cloud.h:440