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