Point Cloud Library (PCL)  1.14.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 #pragma once
41 
42 // PCL
43 #include <pcl/common/point_tests.h> // for pcl::isFinite
44 #include <pcl/point_types.h>
45 #include <pcl/type_traits.h>
46 
47 // VTK
48 // Ignore warnings in the above headers
49 #ifdef __GNUC__
50 #pragma GCC system_header
51 #endif
52 #include <vtkVersion.h>
53 #include <vtkFloatArray.h>
54 #include <vtkPointData.h>
55 #include <vtkPoints.h>
56 #include <vtkPolyData.h>
57 #include <vtkUnsignedCharArray.h>
58 #include <vtkSmartPointer.h>
59 #include <vtkStructuredGrid.h>
60 #include <vtkVertexGlyphFilter.h>
61 
62 // Support for VTK 7.1 upwards
63 #ifdef vtkGenericDataArray_h
64 #define SetTupleValue SetTypedTuple
65 #define InsertNextTupleValue InsertNextTypedTuple
66 #define GetTupleValue GetTypedTuple
67 #endif
68 
69 ///////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> void
71 pcl::io::vtkPolyDataToPointCloud (vtkPolyData* const polydata, pcl::PointCloud<PointT>& cloud)
72 {
73  // This generic template will convert any VTK PolyData
74  // to a coordinate-only PointXYZ PCL format.
75  cloud.width = polydata->GetNumberOfPoints ();
76  cloud.height = 1; // This indicates that the point cloud is unorganized
77  cloud.is_dense = false;
78  cloud.resize (cloud.width * cloud.height);
79 
80  // Get a list of all the fields available
81  std::vector<pcl::PCLPointField> fields;
82  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
83 
84  // Check if XYZ is present
85  int x_idx = -1, y_idx = -1, z_idx = -1;
86  for (std::size_t d = 0; d < fields.size (); ++d)
87  {
88  if (fields[d].name == "x")
89  x_idx = fields[d].offset;
90  else if (fields[d].name == "y")
91  y_idx = fields[d].offset;
92  else if (fields[d].name == "z")
93  z_idx = fields[d].offset;
94  }
95  // Set the coordinates of the pcl::PointCloud (if the pcl::PointCloud supports coordinates)
96  if (x_idx != -1 && y_idx != -1 && z_idx != -1)
97  {
98  for (std::size_t i = 0; i < cloud.size (); ++i)
99  {
100  double coordinate[3];
101  polydata->GetPoint (i, coordinate);
102  pcl::setFieldValue<PointT, float> (cloud[i], x_idx, coordinate[0]);
103  pcl::setFieldValue<PointT, float> (cloud[i], y_idx, coordinate[1]);
104  pcl::setFieldValue<PointT, float> (cloud[i], z_idx, coordinate[2]);
105  }
106  }
107 
108  // Check if Normals are present
109  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
110  for (std::size_t d = 0; d < fields.size (); ++d)
111  {
112  if (fields[d].name == "normal_x")
113  normal_x_idx = fields[d].offset;
114  else if (fields[d].name == "normal_y")
115  normal_y_idx = fields[d].offset;
116  else if (fields[d].name == "normal_z")
117  normal_z_idx = fields[d].offset;
118  }
119  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkPolyData has normals)
120  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
121  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
122  {
123  for (std::size_t i = 0; i < cloud.size (); ++i)
124  {
125  float normal[3];
126  normals->GetTupleValue (i, normal);
127  pcl::setFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
128  pcl::setFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
129  pcl::setFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
130  }
131  }
132 
133  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkPolyData has colors)
134  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
135  int rgb_idx = -1;
136  for (std::size_t d = 0; d < fields.size (); ++d)
137  {
138  if (fields[d].name == "rgb" || fields[d].name == "rgba")
139  {
140  rgb_idx = fields[d].offset;
141  break;
142  }
143  }
144 
145  if (rgb_idx != -1 && colors)
146  {
147  for (std::size_t i = 0; i < cloud.size (); ++i)
148  {
149  unsigned char color[3];
150  colors->GetTupleValue (i, color);
151  pcl::RGB rgb;
152  rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
153  pcl::setFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
154  }
155  }
156 }
157 
158 ///////////////////////////////////////////////////////////////////////////////////////////
159 template <typename PointT> void
160 pcl::io::vtkStructuredGridToPointCloud (vtkStructuredGrid* const structured_grid, pcl::PointCloud<PointT>& cloud)
161 {
162  int dimensions[3];
163  structured_grid->GetDimensions (dimensions);
164  cloud.width = dimensions[0];
165  cloud.height = dimensions[1]; // This indicates that the point cloud is organized
166  cloud.is_dense = true;
167  cloud.resize (cloud.width * cloud.height);
168 
169  // Get a list of all the fields available
170  std::vector<pcl::PCLPointField> fields;
171  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
172 
173  // Check if XYZ is present
174  int x_idx = -1, y_idx = -1, z_idx = -1;
175  for (std::size_t d = 0; d < fields.size (); ++d)
176  {
177  if (fields[d].name == "x")
178  x_idx = fields[d].offset;
179  else if (fields[d].name == "y")
180  y_idx = fields[d].offset;
181  else if (fields[d].name == "z")
182  z_idx = fields[d].offset;
183  }
184 
185  if (x_idx != -1 && y_idx != -1 && z_idx != -1)
186  {
187  for (std::size_t i = 0; i < cloud.width; ++i)
188  {
189  for (std::size_t j = 0; j < cloud.height; ++j)
190  {
191  int queryPoint[3] = {i, j, 0};
192  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
193  if (structured_grid->IsPointVisible (pointId))
194  {
195  double coordinate[3];
196  structured_grid->GetPoint (pointId, coordinate);
197  pcl::setFieldValue<PointT, float> (cloud (i, j), x_idx, coordinate[0]);
198  pcl::setFieldValue<PointT, float> (cloud (i, j), y_idx, coordinate[1]);
199  pcl::setFieldValue<PointT, float> (cloud (i, j), z_idx, coordinate[2]);
200  }
201  else
202  {
203  // Fill the point with an "empty" point?
204  }
205  }
206  }
207  }
208 
209  // Check if Normals are present
210  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
211  for (std::size_t d = 0; d < fields.size (); ++d)
212  {
213  if (fields[d].name == "normal_x")
214  normal_x_idx = fields[d].offset;
215  else if (fields[d].name == "normal_y")
216  normal_y_idx = fields[d].offset;
217  else if (fields[d].name == "normal_z")
218  normal_z_idx = fields[d].offset;
219  }
220  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkStructuredGrid has normals)
221  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
222 
223  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
224  {
225  for (std::size_t i = 0; i < cloud.width; ++i)
226  {
227  for (std::size_t j = 0; j < cloud.height; ++j)
228  {
229  int queryPoint[3] = {i, j, 0};
230  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
231  if (structured_grid->IsPointVisible (pointId))
232  {
233  float normal[3];
234  normals->GetTupleValue (i, normal);
235  pcl::setFieldValue<PointT, float> (cloud (i, j), normal_x_idx, normal[0]);
236  pcl::setFieldValue<PointT, float> (cloud (i, j), normal_y_idx, normal[1]);
237  pcl::setFieldValue<PointT, float> (cloud (i, j), normal_z_idx, normal[2]);
238  }
239  else
240  {
241  // Fill the point with an "empty" point?
242  }
243  }
244  }
245  }
246 
247  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkStructuredGrid has colors)
248  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray ("Colors"));
249  int rgb_idx = -1;
250  for (std::size_t d = 0; d < fields.size (); ++d)
251  {
252  if (fields[d].name == "rgb" || fields[d].name == "rgba")
253  {
254  rgb_idx = fields[d].offset;
255  break;
256  }
257  }
258 
259  if (rgb_idx != -1 && colors)
260  {
261  for (std::size_t i = 0; i < cloud.width; ++i)
262  {
263  for (std::size_t j = 0; j < cloud.height; ++j)
264  {
265  int queryPoint[3] = {i, j, 0};
266  vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint);
267  unsigned char color[3];
268  if (structured_grid->IsPointVisible (pointId))
269  {
270  colors->GetTupleValue (i, color);
271  pcl::RGB rgb;
272  rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
273  pcl::setFieldValue<PointT, std::uint32_t> (cloud (i, j), rgb_idx, rgb.rgba);
274  }
275  else
276  {
277  // Fill the point with an "empty" point?
278  }
279  }
280  }
281  }
282 }
283 
284 ///////////////////////////////////////////////////////////////////////////////////////////
285 template <typename PointT> void
286 pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud<PointT>& cloud, vtkPolyData* const pdata)
287 {
288  // Get a list of all the fields available
289  std::vector<pcl::PCLPointField> fields;
290  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
291 
292  // Coordinates (always must have coordinates)
293  vtkIdType nr_points = cloud.size ();
295  points->SetNumberOfPoints (nr_points);
296  // Get a pointer to the beginning of the data array
297  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
298 
299  // Set the points
300  if (cloud.is_dense)
301  {
302  for (vtkIdType i = 0; i < nr_points; ++i)
303  memcpy (&data[i * 3], &cloud[i].x, 12); // sizeof (float) * 3
304  }
305  else
306  {
307  vtkIdType j = 0; // true point index
308  for (vtkIdType i = 0; i < nr_points; ++i)
309  {
310  // Check if the point is invalid
311  if (!std::isfinite (cloud[i].x) ||
312  !std::isfinite (cloud[i].y) ||
313  !std::isfinite (cloud[i].z))
314  continue;
315 
316  memcpy (&data[j * 3], &cloud[i].x, 12); // sizeof (float) * 3
317  j++;
318  }
319  nr_points = j;
320  points->SetNumberOfPoints (nr_points);
321  }
322 
323  // Create a temporary PolyData and add the points to it
325  temp_polydata->SetPoints (points);
326 
327  // Check if Normals are present
328  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
329  for (std::size_t d = 0; d < fields.size (); ++d)
330  {
331  if (fields[d].name == "normal_x")
332  normal_x_idx = fields[d].offset;
333  else if (fields[d].name == "normal_y")
334  normal_y_idx = fields[d].offset;
335  else if (fields[d].name == "normal_z")
336  normal_z_idx = fields[d].offset;
337  }
338  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
339  {
341  normals->SetNumberOfComponents (3); //3d normals (ie x,y,z)
342  normals->SetNumberOfTuples (cloud.size ());
343  normals->SetName ("Normals");
344 
345  for (std::size_t i = 0; i < cloud.size (); ++i)
346  {
347  float normal[3];
348  pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
349  pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
350  pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
351  normals->SetTupleValue (i, normal);
352  }
353  temp_polydata->GetPointData ()->SetNormals (normals);
354  }
355 
356  // Colors (optional)
357  int rgb_idx = -1;
358  for (std::size_t d = 0; d < fields.size (); ++d)
359  {
360  if (fields[d].name == "rgb" || fields[d].name == "rgba")
361  {
362  rgb_idx = fields[d].offset;
363  break;
364  }
365  }
366  if (rgb_idx != -1)
367  {
369  colors->SetNumberOfComponents (3);
370  colors->SetNumberOfTuples (cloud.size ());
371  colors->SetName ("RGB");
372 
373  for (std::size_t i = 0; i < cloud.size (); ++i)
374  {
375  unsigned char color[3];
376  pcl::RGB rgb;
377  pcl::getFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
378  color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
379  colors->SetTupleValue (i, color);
380  }
381  temp_polydata->GetPointData ()->SetScalars (colors);
382  }
383 
384  // Add 0D topology to every point
386  vertex_glyph_filter->SetInputData (temp_polydata);
387  vertex_glyph_filter->Update ();
388 
389  pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
390 }
391 
392 ///////////////////////////////////////////////////////////////////////////////////////////
393 template <typename PointT> void
394 pcl::io::pointCloudTovtkStructuredGrid (const pcl::PointCloud<PointT>& cloud, vtkStructuredGrid* const structured_grid)
395 {
396  // Get a list of all the fields available
397  std::vector<pcl::PCLPointField> fields;
398  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
399 
400  int dimensions[3] = {cloud.width, cloud.height, 1};
401  structured_grid->SetDimensions (dimensions);
402 
404  points->SetNumberOfPoints (cloud.width * cloud.height);
405 
406  for (std::size_t i = 0; i < cloud.width; ++i)
407  {
408  for (std::size_t j = 0; j < cloud.height; ++j)
409  {
410  int queryPoint[3] = {i, j, 0};
411  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
412  const PointT &point = cloud (i, j);
413 
414  if (pcl::isFinite (point))
415  {
416  float p[3] = {point.x, point.y, point.z};
417  points->SetPoint (pointId, p);
418  }
419  else
420  {
421  }
422  }
423  }
424 
425  structured_grid->SetPoints (points);
426 
427  // Check if Normals are present
428  int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
429  for (std::size_t d = 0; d < fields.size (); ++d)
430  {
431  if (fields[d].name == "normal_x")
432  normal_x_idx = fields[d].offset;
433  else if (fields[d].name == "normal_y")
434  normal_y_idx = fields[d].offset;
435  else if (fields[d].name == "normal_z")
436  normal_z_idx = fields[d].offset;
437  }
438 
439  if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
440  {
442  normals->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
443  normals->SetNumberOfTuples (cloud.width * cloud.height);
444  normals->SetName ("Normals");
445  for (std::size_t i = 0; i < cloud.width; ++i)
446  {
447  for (std::size_t j = 0; j < cloud.height; ++j)
448  {
449  int queryPoint[3] = {i, j, 0};
450  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
451  const PointT &point = cloud (i, j);
452 
453  float normal[3];
454  pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]);
455  pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]);
456  pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]);
457  normals->SetTupleValue (pointId, normal);
458  }
459  }
460 
461  structured_grid->GetPointData ()->SetNormals (normals);
462  }
463 
464  // Colors (optional)
465  int rgb_idx = -1;
466  for (std::size_t d = 0; d < fields.size (); ++d)
467  {
468  if (fields[d].name == "rgb" || fields[d].name == "rgba")
469  {
470  rgb_idx = fields[d].offset;
471  break;
472  }
473  }
474 
475  if (rgb_idx != -1)
476  {
478  colors->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
479  colors->SetNumberOfTuples (cloud.width * cloud.height);
480  colors->SetName ("Colors");
481  for (std::size_t i = 0; i < cloud.width; ++i)
482  {
483  for (std::size_t j = 0; j < cloud.height; ++j)
484  {
485  int queryPoint[3] = {i, j, 0};
486  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
487  const PointT &point = cloud (i, j);
488 
489  if (pcl::isFinite (point))
490  {
491 
492  unsigned char color[3];
493  pcl::RGB rgb;
494  pcl::getFieldValue<PointT, std::uint32_t> (cloud[i], rgb_idx, rgb.rgba);
495  color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
496  colors->SetTupleValue (pointId, color);
497  }
498  else
499  {
500  }
501  }
502  }
503  structured_grid->GetPointData ()->AddArray (colors);
504  }
505 }
506 
507 #ifdef vtkGenericDataArray_h
508 #undef SetTupleValue
509 #undef InsertNextTupleValue
510 #undef GetTupleValue
511 #endif
512 
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
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:71
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:160
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:394
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:286
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:55
A point structure representing Euclidean xyz coordinates, and the RGB color.
A structure representing RGB color information.