Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
vtk_lib_io.hpp
Go to the documentation of this file.
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/io/pcd_io.h>
45 #include <pcl/point_types.h>
46 
47 // VTK
48 // Ignore warnings in the above headers
49 #ifdef __GNUC__
50 #pragma GCC system_header
51 #endif
52 #include <vtkFloatArray.h>
53 #include <vtkPointData.h>
54 #include <vtkPoints.h>
55 #include <vtkPolyData.h>
56 #include <vtkUnsignedCharArray.h>
57 #include <vtkSmartPointer.h>
58 #include <vtkStructuredGrid.h>
59 #include <vtkVertexGlyphFilter.h>
60 
62 template <typename PointT> void
63 pcl::io::vtkPolyDataToPointCloud (vtkPolyData* const polydata, pcl::PointCloud<PointT>& cloud)
64 {
65  // This generic template will convert any VTK PolyData
66  // to a coordinate-only PointXYZ PCL format.
67  typedef pcl::PointCloud<PointT> CloudT;
68 
69  typedef typename pcl::traits::fieldList<typename CloudT::PointType>::type FieldList;
70 
71  cloud.width = polydata->GetNumberOfPoints ();
72  cloud.height = 1; // This indicates that the point cloud is unorganized
73  cloud.is_dense = false;
74  cloud.points.resize (cloud.width);
75 
76  typename CloudT::PointType test_point = cloud.points[0];
77 
78  bool has_x = false; bool has_y = false; bool has_z = false;
79  float x_val = 0.0f; float y_val = 0.0f; float z_val = 0.0f;
80  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "x", has_x, x_val));
81  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "y", has_y, y_val));
82  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "z", has_z, z_val));
83 
84  // Set the coordinates of the pcl::PointCloud (if the pcl::PointCloud supports coordinates)
85  if (has_x && has_y && has_z)
86  {
87  for (size_t i = 0; i < cloud.points.size (); ++i)
88  {
89  double coordinate[3];
90  polydata->GetPoint (i, coordinate);
91  typename CloudT::PointType p = cloud.points[i];
92  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "x", coordinate[0]));
93  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "y", coordinate[1]));
94  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "z", coordinate[2]));
95  cloud.points[i] = p;
96  }
97  }
98 
99  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkPolyData has normals)
100  bool has_normal_x = false; bool has_normal_y = false; bool has_normal_z = false;
101  float normal_x_val = 0.0f; float normal_y_val = 0.0f; float normal_z_val = 0.0f;
102  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point,
103  "normal_x", has_normal_x, normal_x_val));
104  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point,
105  "y_normal", has_normal_y, normal_y_val));
106  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point,
107  "z_normal", has_normal_z, normal_z_val));
108 
109  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
110  if (has_normal_x && has_normal_y && has_normal_z && normals)
111  {
112  for (size_t i = 0; i < cloud.points.size (); ++i)
113  {
114  float normal[3];
115  normals->GetTupleValue (i, normal);
116  typename CloudT::PointType p = cloud.points[i];
117  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "normal_x", normal[0]));
118  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "normal_y", normal[1]));
119  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "normal_z", normal[2]));
120  cloud.points[i] = p;
121  }
122  }
123 
124  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkPolyData has colors)
125  bool has_r = false; bool has_g = false; bool has_b = false;
126  unsigned char r_val = 0.0f; unsigned char g_val = 0.0f; unsigned char b_val = 0.0f;
127  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point,
128  "r", has_r, r_val));
129  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point,
130  "g", has_g, g_val));
131  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point,
132  "b", has_b, b_val));
133 
134  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
135  if (has_r && has_g && has_b && colors)
136  {
137  for (size_t i = 0; i < cloud.points.size (); ++i)
138  {
139  unsigned char color[3];
140  colors->GetTupleValue (i, color);
141  typename CloudT::PointType p = cloud.points[i];
142  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, unsigned char> (p, "r", color[0]));
143  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, unsigned char> (p, "g", color[1]));
144  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, unsigned char> (p, "b", color[2]));
145  cloud.points[i] = p;
146  }
147  }
148 }
149 
151 template <typename PointT> void
152 pcl::io::vtkStructuredGridToPointCloud (vtkStructuredGrid* const structured_grid, pcl::PointCloud<PointT>& cloud)
153 {
154  typedef pcl::PointCloud<PointT> CloudT;
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  typename CloudT::PointType test_point = cloud.points[0];
164 
165  typedef typename pcl::traits::fieldList<typename CloudT::PointType>::type FieldList;
166 
167  bool has_x = false; bool has_y = false; bool has_z = false;
168  float x_val = 0.0f; float y_val = 0.0f; float z_val = 0.0f;
169  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "x", has_x, x_val));
170  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "y", has_y, y_val));
171  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "z", has_z, z_val));
172 
173  if (has_x && has_y && has_z)
174  {
175  for (size_t i = 0; i < cloud.width; ++i)
176  {
177  for (size_t j = 0; j < cloud.height; ++j)
178  {
179  int queryPoint[3] = {i, j, 0};
180  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
181  double coordinate[3];
182  if (structured_grid->IsPointVisible (pointId))
183  {
184  structured_grid->GetPoint (pointId, coordinate);
185  typename CloudT::PointType p = cloud (i, j);
186  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "x", coordinate[0]));
187  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "y", coordinate[1]));
188  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "z", coordinate[2]));
189  cloud (i, j) = p;
190  }
191  else
192  {
193  // Fill the point with an "empty" point?
194  }
195  }
196  }
197  }
198 
199  // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkStructuredGrid has normals)
200  bool has_normal_x = false; bool has_normal_y = false; bool has_normal_z = false;
201  float normal_x_val = 0.0f; float normal_y_val = 0.0f; float normal_z_val = 0.0f;
202  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point,
203  "x_normal", has_normal_x, normal_x_val));
204  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point,
205  "y_normal", has_normal_y, normal_y_val));
206  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point,
207  "z_normal", has_normal_z, normal_z_val));
208 
209  vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
210 
211  if (has_x && has_y && has_z)
212  {
213  for (size_t i = 0; i < cloud.width; ++i)
214  {
215  for (size_t j = 0; j < cloud.height; ++j)
216  {
217  int queryPoint[3] = {i, j, 0};
218  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
219  float normal[3];
220  if(structured_grid->IsPointVisible (pointId))
221  {
222  normals->GetTupleValue (i, normal);
223  typename CloudT::PointType p = cloud (i, j);
224  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "normal_x", normal[0]));
225  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "normal_y", normal[1]));
226  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "normal_z", normal[2]));
227  cloud (i, j) = p;
228  }
229  else
230  {
231  // Fill the point with an "empty" point?
232  }
233  }
234  }
235  }
236 
237  // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkStructuredGrid has colors)
238  bool has_r = false; bool has_g = false; bool has_b = false;
239  unsigned char r_val = 0.0f; unsigned char g_val = 0.0f; unsigned char b_val = 0.0f;
240  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point,
241  "r", has_r, r_val));
242  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point,
243  "g", has_g, g_val));
244  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point,
245  "b", has_b, b_val));
246  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast(structured_grid->GetPointData()->GetArray("Colors"));
247 
248  if (has_r && has_g && has_b && colors)
249  {
250  for (size_t i = 0; i < cloud.width; ++i)
251  {
252  for (size_t j = 0; j < cloud.height; ++j)
253  {
254  int queryPoint[3] = {i, j, 0};
255  vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint);
256  unsigned char color[3];
257  if (structured_grid->IsPointVisible (pointId))
258  {
259  colors->GetTupleValue (i, color);
260  typename CloudT::PointType p = cloud (i, j);
261  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "r", color[0]));
262  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "g", color[1]));
263  pcl::for_each_type<FieldList> (pcl::SetIfFieldExists<typename CloudT::PointType, float> (p, "b", color[2]));
264  cloud (i, j) = p;
265  }
266  else
267  {
268  // Fill the point with an "empty" point?
269  }
270  }
271  }
272  }
273 }
274 
276 template <typename PointT> void
277 pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud<PointT>& cloud, vtkPolyData* const pdata)
278 {
279  typedef pcl::PointCloud<PointT> CloudT;
280 
281  typename CloudT::PointType test_point = cloud.points[0];
282 
283  typedef typename pcl::traits::fieldList<typename CloudT::PointType>::type FieldList;
284 
285  // Coordiantes (always must have coordinates)
286  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
287  for (size_t i = 0; i < cloud.points.size (); ++i)
288  {
289  double p[3];
290  p[0] = cloud.points[i].x;
291  p[1] = cloud.points[i].y;
292  p[2] = cloud.points[i].z;
293  points->InsertNextPoint (p);
294  }
295 
296  // Create a temporary PolyData and add the points to it
297  vtkSmartPointer<vtkPolyData> temp_polydata = vtkSmartPointer<vtkPolyData>::New ();
298  temp_polydata->SetPoints (points);
299 
300  // Normals (optional)
301  bool has_normal_x = false; bool has_normal_y = false; bool has_normal_z = false;
302  float normal_x_val = 0.0f; float normal_y_val = 0.0f; float normal_z_val = 0.0f;
303  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "normal_x", has_normal_x, normal_x_val));
304  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "normal_y", has_normal_y, normal_y_val));
305  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "normal_z", has_normal_z, normal_z_val));
306  if (has_normal_x && has_normal_y && has_normal_z)
307  {
308  vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
309  normals->SetNumberOfComponents (3); //3d normals (ie x,y,z)
310  normals->SetNumberOfTuples (cloud.points.size ());
311  normals->SetName ("Normals");
312 
313  for (size_t i = 0; i < cloud.points.size (); ++i)
314  {
315  typename CloudT::PointType p = cloud.points[i];
316  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (p, "x_normal", has_normal_x, normal_x_val));
317  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (p, "y_normal", has_normal_y, normal_y_val));
318  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (p, "z_normal", has_normal_z, normal_z_val));
319  float normal[3] = {normal_x_val, normal_y_val, normal_z_val};
320  normals->SetTupleValue (i, normal);
321  }
322  temp_polydata->GetPointData ()->SetNormals (normals);
323  }
324 
325  // Colors (optional)
326  bool has_r = false; bool has_g = false; bool has_b = false;
327  unsigned char r_val = 0; unsigned char g_val = 0; unsigned char b_val = 0;
328  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point, "r", has_r, r_val));
329  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point, "g", has_g, g_val));
330  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point, "b", has_b, b_val));
331  if (has_r && has_g && has_b)
332  {
333  vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
334  colors->SetNumberOfComponents (3);
335  colors->SetNumberOfTuples (cloud.points.size ());
336  colors->SetName ("RGB");
337 
338  for (size_t i = 0; i < cloud.points.size (); ++i)
339  {
340  typename CloudT::PointType p = cloud[i];
341  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (p, "r", has_r, r_val));
342  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (p, "g", has_g, g_val));
343  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (p, "b", has_b, b_val));
344  unsigned char color[3] = {r_val, g_val, b_val};
345  colors->SetTupleValue (i, color);
346  }
347  temp_polydata->GetPointData ()->SetScalars (colors);
348  }
349 
350  // Add 0D topology to every point
351  vtkSmartPointer<vtkVertexGlyphFilter> vertex_glyph_filter = vtkSmartPointer<vtkVertexGlyphFilter>::New ();
352  #if VTK_MAJOR_VERSION <= 5
353  vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ());
354  #else
355  vertex_glyph_filter->SetInputData (temp_polydata);
356  #endif
357  vertex_glyph_filter->Update ();
358 
359  pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
360 }
361 
363 template <typename PointT> void
364 pcl::io::pointCloudTovtkStructuredGrid (const pcl::PointCloud<PointT>& cloud, vtkStructuredGrid* const structured_grid)
365 {
366  typedef pcl::PointCloud<PointT> CloudT;
367 
368  typename CloudT::PointType test_point = cloud.points[0];
369 
370  typedef typename pcl::traits::fieldList<typename CloudT::PointType>::type FieldList;
371 
372  int dimensions[3] = {cloud.width, cloud.height, 1};
373  structured_grid->SetDimensions (dimensions);
374 
375  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
376  points->SetNumberOfPoints (cloud.width * cloud.height);
377 
378  unsigned int numberOfInvalidPoints = 0;
379 
380  for (size_t i = 0; i < cloud.width; ++i)
381  {
382  for (size_t j = 0; j < cloud.height; ++j)
383  {
384  int queryPoint[3] = {i, j, 0};
385  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
386  typename CloudT::PointType point = cloud (i, j);
387 
388  if (pcl::isFinite (point))
389  {
390  float p[3] = {point.x, point.y, point.z};
391  points->SetPoint (pointId, p);
392  }
393  else
394  {
395  }
396  }
397  }
398 
399  structured_grid->SetPoints (points);
400 
401  // Normals (optional)
402  bool has_normal_x = false; bool has_normal_y = false; bool has_normal_z = false;
403  float normal_x_val = 0.0f; float normal_y_val = 0.0f; float normal_z_val = 0.0f;
404  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "normal_x", has_normal_x, normal_x_val));
405  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "normal_y", has_normal_y, normal_y_val));
406  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (test_point, "normal_z", has_normal_z, normal_z_val));
407  if (has_normal_x && has_normal_y && has_normal_z)
408  {
409  vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
410  normals->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
411  normals->SetNumberOfTuples (cloud.width * cloud.height);
412  normals->SetName ("Normals");
413  for (size_t i = 0; i < cloud.width; ++i)
414  {
415  for (size_t j = 0; j < cloud.height; ++j)
416  {
417  int queryPoint[3] = {i, j, 0};
418  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
419  typename CloudT::PointType point = cloud (i, j);
420 
421  if (pcl::isFinite (point))
422  {
423  typename CloudT::PointType p = cloud.points[i];
424  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (p, "x_normal", has_normal_x, normal_x_val));
425  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (p, "y_normal", has_normal_y, normal_y_val));
426  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, float> (p, "z_normal", has_normal_z, normal_z_val));
427  float normal[3] = {normal_x_val, normal_y_val, normal_z_val};
428  normals->SetTupleValue (pointId, normal);
429  }
430  else
431  {
432  }
433  }
434  }
435 
436  structured_grid->GetPointData ()->SetNormals (normals);
437  }
438 
439  // Colors (optional)
440  bool has_r = false; bool has_g = false; bool has_b = false;
441  unsigned char r_val = 0; unsigned char g_val = 0; unsigned char b_val = 0;
442  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point, "r", has_r, r_val));
443  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point, "g", has_g, g_val));
444  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (test_point, "b", has_b, b_val));
445  if (has_r && has_g && has_b)
446  {
447  vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
448  colors->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
449  colors->SetNumberOfTuples (cloud.width * cloud.height);
450  colors->SetName ("Colors");
451  for (size_t i = 0; i < cloud.width; ++i)
452  {
453  for (size_t j = 0; j < cloud.height; ++j)
454  {
455  int queryPoint[3] = {i, j, 0};
456  vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
457  typename CloudT::PointType point = cloud (i, j);
458 
459  if (pcl::isFinite (point))
460  {
461  typename CloudT::PointType p = cloud[i];
462  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (p, "r", has_r, r_val));
463  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (p, "g", has_g, g_val));
464  pcl::for_each_type<FieldList> (pcl::CopyIfFieldExists<typename CloudT::PointType, unsigned char> (p, "b", has_b, b_val));
465  unsigned char color[3] = {r_val, g_val, b_val};
466  colors->SetTupleValue (i, color);
467  }
468  else
469  {
470  }
471  }
472  }
473  structured_grid->GetPointData ()->AddArray (colors);
474  }
475 }
476 
477 #endif //#ifndef PCL_IO_VTK_IO_H_
478 
Generated on Sat Aug 24 2013 17:28:33 for Point Cloud Library (PCL) by   doxygen 1.8.4