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