vtk_lib_io.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: vtk_io.hpp 4968 2012-05-03 06:39:52Z doria $
00037  *
00038  */
00039 
00040 #ifndef PCL_IO_VTK_IO_IMPL_H_
00041 #define PCL_IO_VTK_IO_IMPL_H_
00042 
00043 // PCL
00044 #include <pcl/common/io.h>
00045 #include <pcl/io/pcd_io.h>
00046 #include <pcl/point_types.h>
00047 #include <pcl/point_traits.h>
00048 
00049 // VTK
00050 // Ignore warnings in the above headers
00051 #ifdef __GNUC__
00052 #pragma GCC system_header
00053 #endif
00054 #include <vtkFloatArray.h>
00055 #include <vtkPointData.h>
00056 #include <vtkPoints.h>
00057 #include <vtkPolyData.h>
00058 #include <vtkUnsignedCharArray.h>
00059 #include <vtkSmartPointer.h>
00060 #include <vtkStructuredGrid.h>
00061 #include <vtkVertexGlyphFilter.h>
00062 
00064 template <typename PointT> void
00065 pcl::io::vtkPolyDataToPointCloud (vtkPolyData* const polydata, pcl::PointCloud<PointT>& cloud)
00066 {
00067   // This generic template will convert any VTK PolyData
00068   // to a coordinate-only PointXYZ PCL format.
00069   cloud.width = polydata->GetNumberOfPoints ();
00070   cloud.height = 1; // This indicates that the point cloud is unorganized
00071   cloud.is_dense = false;
00072   cloud.points.resize (cloud.width * cloud.height);
00073 
00074   // Get a list of all the fields available
00075   std::vector<pcl::PCLPointField> fields;
00076   pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
00077 
00078   // Check if XYZ is present
00079   int x_idx = -1, y_idx = -1, z_idx = -1;
00080   for (size_t d = 0; d < fields.size (); ++d)
00081   {
00082     if (fields[d].name == "x")
00083       x_idx = fields[d].offset;
00084     else if (fields[d].name == "y")
00085       y_idx = fields[d].offset;
00086     else if (fields[d].name == "z")
00087       z_idx = fields[d].offset;
00088   }
00089   // Set the coordinates of the pcl::PointCloud (if the pcl::PointCloud supports coordinates)
00090   if (x_idx != -1 && y_idx != -1 && z_idx != -1)
00091   {
00092     for (size_t i = 0; i < cloud.points.size (); ++i)
00093     {
00094       double coordinate[3];
00095       polydata->GetPoint (i, coordinate);
00096       pcl::setFieldValue<PointT, float> (cloud.points[i], x_idx, coordinate[0]);
00097       pcl::setFieldValue<PointT, float> (cloud.points[i], y_idx, coordinate[1]);
00098       pcl::setFieldValue<PointT, float> (cloud.points[i], z_idx, coordinate[2]);
00099     }
00100   }
00101 
00102   // Check if Normals are present
00103   int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
00104   for (size_t d = 0; d < fields.size (); ++d)
00105   {
00106     if (fields[d].name == "normal_x")
00107       normal_x_idx = fields[d].offset;
00108     else if (fields[d].name == "normal_y")
00109       normal_y_idx = fields[d].offset;
00110     else if (fields[d].name == "normal_z")
00111       normal_z_idx = fields[d].offset;
00112   }
00113   // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkPolyData has normals)
00114   vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ());
00115   if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
00116   {
00117     for (size_t i = 0; i < cloud.points.size (); ++i)
00118     {
00119       float normal[3];
00120       normals->GetTupleValue (i, normal);
00121       pcl::setFieldValue<PointT, float> (cloud.points[i], normal_x_idx, normal[0]);
00122       pcl::setFieldValue<PointT, float> (cloud.points[i], normal_y_idx, normal[1]);
00123       pcl::setFieldValue<PointT, float> (cloud.points[i], normal_z_idx, normal[2]);
00124     }
00125   }
00126 
00127   // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkPolyData has colors)
00128   vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
00129   int rgb_idx = -1;
00130   for (size_t d = 0; d < fields.size (); ++d)
00131   {
00132     if (fields[d].name == "rgb" || fields[d].name == "rgba")
00133     {
00134       rgb_idx = fields[d].offset;
00135       break;
00136     }
00137   }
00138 
00139   if (rgb_idx != -1 && colors)
00140   {
00141     for (size_t i = 0; i < cloud.points.size (); ++i)
00142     {
00143       unsigned char color[3];
00144       colors->GetTupleValue (i, color);
00145       pcl::RGB rgb;
00146       rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
00147       pcl::setFieldValue<PointT, uint32_t> (cloud.points[i], rgb_idx, rgb.rgba);
00148     }
00149   }
00150 }
00151 
00153 template <typename PointT> void
00154 pcl::io::vtkStructuredGridToPointCloud (vtkStructuredGrid* const structured_grid, pcl::PointCloud<PointT>& cloud)
00155 {
00156   int dimensions[3];
00157   structured_grid->GetDimensions (dimensions);
00158   cloud.width = dimensions[0];
00159   cloud.height = dimensions[1]; // This indicates that the point cloud is organized
00160   cloud.is_dense = true;
00161   cloud.points.resize (cloud.width * cloud.height);
00162 
00163   // Get a list of all the fields available
00164   std::vector<pcl::PCLPointField> fields;
00165   pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
00166 
00167   // Check if XYZ is present
00168   int x_idx = -1, y_idx = -1, z_idx = -1;
00169   for (size_t d = 0; d < fields.size (); ++d)
00170   {
00171     if (fields[d].name == "x")
00172       x_idx = fields[d].offset;
00173     else if (fields[d].name == "y")
00174       y_idx = fields[d].offset;
00175     else if (fields[d].name == "z")
00176       z_idx = fields[d].offset;
00177   }
00178 
00179   if (x_idx != -1 && y_idx != -1 && z_idx != -1)
00180   {
00181     for (size_t i = 0; i < cloud.width; ++i)
00182     {
00183       for (size_t j = 0; j < cloud.height; ++j)
00184       {
00185         int queryPoint[3] = {i, j, 0};
00186         vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
00187         double coordinate[3];
00188         if (structured_grid->IsPointVisible (pointId))
00189         {
00190           structured_grid->GetPoint (pointId, coordinate);
00191           pcl::setFieldValue<PointT, float> (cloud (i, j), x_idx, coordinate[0]);
00192           pcl::setFieldValue<PointT, float> (cloud (i, j), y_idx, coordinate[1]);
00193           pcl::setFieldValue<PointT, float> (cloud (i, j), z_idx, coordinate[2]);
00194         }
00195         else
00196         {
00197           // Fill the point with an "empty" point?
00198         }
00199       }
00200     }
00201   }
00202 
00203   // Check if Normals are present
00204   int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
00205   for (size_t d = 0; d < fields.size (); ++d)
00206   {
00207     if (fields[d].name == "normal_x")
00208       normal_x_idx = fields[d].offset;
00209     else if (fields[d].name == "normal_y")
00210       normal_y_idx = fields[d].offset;
00211     else if (fields[d].name == "normal_z")
00212       normal_z_idx = fields[d].offset;
00213   }
00214   // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkStructuredGrid has normals)
00215   vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ());
00216 
00217   if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals)
00218   {
00219     for (size_t i = 0; i < cloud.width; ++i)
00220     {
00221       for (size_t j = 0; j < cloud.height; ++j)
00222       {
00223         int queryPoint[3] = {i, j, 0};
00224         vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
00225         float normal[3];
00226         if (structured_grid->IsPointVisible (pointId))
00227         {
00228           normals->GetTupleValue (i, normal);
00229           pcl::setFieldValue<PointT, float> (cloud (i, j), normal_x_idx, normal[0]);
00230           pcl::setFieldValue<PointT, float> (cloud (i, j), normal_y_idx, normal[1]);
00231           pcl::setFieldValue<PointT, float> (cloud (i, j), normal_z_idx, normal[2]);
00232         }
00233         else
00234         {
00235           // Fill the point with an "empty" point?
00236         }
00237       }
00238     }
00239   }
00240 
00241   // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkStructuredGrid has colors)
00242   vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray ("Colors"));
00243   int rgb_idx = -1;
00244   for (size_t d = 0; d < fields.size (); ++d)
00245   {
00246     if (fields[d].name == "rgb" || fields[d].name == "rgba")
00247     {
00248       rgb_idx = fields[d].offset;
00249       break;
00250     }
00251   }
00252 
00253   if (rgb_idx != -1 && colors)
00254   {
00255     for (size_t i = 0; i < cloud.width; ++i)
00256     {
00257       for (size_t j = 0; j < cloud.height; ++j)
00258       {
00259         int queryPoint[3] = {i, j, 0};
00260         vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint);
00261         unsigned char color[3];
00262         if (structured_grid->IsPointVisible (pointId))
00263         {
00264           colors->GetTupleValue (i, color);
00265           pcl::RGB rgb;
00266           rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2];
00267           pcl::setFieldValue<PointT, uint32_t> (cloud (i, j), rgb_idx, rgb.rgba);
00268         }
00269         else
00270         {
00271           // Fill the point with an "empty" point?
00272         }
00273       }
00274     }
00275   }
00276 }
00277 
00279 template <typename PointT> void
00280 pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud<PointT>& cloud, vtkPolyData* const pdata)
00281 {
00282   // Get a list of all the fields available
00283   std::vector<pcl::PCLPointField> fields;
00284   pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
00285 
00286   // Coordinates (always must have coordinates)
00287   vtkIdType nr_points = cloud.points.size ();
00288   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
00289   points->SetNumberOfPoints (nr_points);
00290   // Get a pointer to the beginning of the data array
00291   float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
00292 
00293   // Set the points
00294   if (cloud.is_dense)
00295   {
00296     for (vtkIdType i = 0; i < nr_points; ++i)
00297       memcpy (&data[i * 3], &cloud[i].x, 12);    // sizeof (float) * 3
00298   }
00299   else
00300   {
00301     vtkIdType j = 0;    // true point index
00302     for (vtkIdType i = 0; i < nr_points; ++i)
00303     {
00304       // Check if the point is invalid
00305       if (!pcl_isfinite (cloud[i].x) ||
00306           !pcl_isfinite (cloud[i].y) ||
00307           !pcl_isfinite (cloud[i].z))
00308         continue;
00309 
00310       memcpy (&data[j * 3], &cloud[i].x, 12);    // sizeof (float) * 3
00311       j++;
00312     }
00313     nr_points = j;
00314     points->SetNumberOfPoints (nr_points);
00315   }
00316 
00317   // Create a temporary PolyData and add the points to it
00318   vtkSmartPointer<vtkPolyData> temp_polydata = vtkSmartPointer<vtkPolyData>::New ();
00319   temp_polydata->SetPoints (points);
00320 
00321   // Check if Normals are present
00322   int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
00323   for (size_t d = 0; d < fields.size (); ++d)
00324   {
00325     if (fields[d].name == "normal_x")
00326       normal_x_idx = fields[d].offset;
00327     else if (fields[d].name == "normal_y")
00328       normal_y_idx = fields[d].offset;
00329     else if (fields[d].name == "normal_z")
00330       normal_z_idx = fields[d].offset;
00331   }
00332   if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
00333   {
00334     vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
00335     normals->SetNumberOfComponents (3); //3d normals (ie x,y,z)
00336     normals->SetNumberOfTuples (cloud.size ());
00337     normals->SetName ("Normals");
00338 
00339     for (size_t i = 0; i < cloud.size (); ++i)
00340     {
00341       float normal[3];
00342       pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]);
00343       pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]);
00344       pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]);
00345       normals->SetTupleValue (i, normal);
00346     }
00347     temp_polydata->GetPointData ()->SetNormals (normals);
00348   }
00349 
00350   // Colors (optional)
00351   int rgb_idx = -1;
00352   for (size_t d = 0; d < fields.size (); ++d)
00353   {
00354     if (fields[d].name == "rgb" || fields[d].name == "rgba")
00355     {
00356       rgb_idx = fields[d].offset;
00357       break;
00358     }
00359   }
00360   if (rgb_idx != -1)
00361   {
00362     vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00363     colors->SetNumberOfComponents (3);
00364     colors->SetNumberOfTuples (cloud.size ());
00365     colors->SetName ("RGB");
00366 
00367     for (size_t i = 0; i < cloud.size (); ++i)
00368     {
00369       unsigned char color[3];
00370       pcl::RGB rgb;
00371       pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
00372       color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
00373       colors->SetTupleValue (i, color);
00374     }
00375     temp_polydata->GetPointData ()->SetScalars (colors);
00376   }
00377 
00378   // Add 0D topology to every point
00379   vtkSmartPointer<vtkVertexGlyphFilter> vertex_glyph_filter = vtkSmartPointer<vtkVertexGlyphFilter>::New ();
00380   #if VTK_MAJOR_VERSION <= 5
00381     vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ());
00382   #else
00383     vertex_glyph_filter->SetInputData (temp_polydata);
00384   #endif
00385   vertex_glyph_filter->Update ();
00386 
00387   pdata->DeepCopy (vertex_glyph_filter->GetOutput ());
00388 }
00389 
00391 template <typename PointT> void
00392 pcl::io::pointCloudTovtkStructuredGrid (const pcl::PointCloud<PointT>& cloud, vtkStructuredGrid* const structured_grid)
00393 {
00394   // Get a list of all the fields available
00395   std::vector<pcl::PCLPointField> fields;
00396   pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));
00397 
00398   int dimensions[3] = {cloud.width, cloud.height, 1};
00399   structured_grid->SetDimensions (dimensions);
00400 
00401   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
00402   points->SetNumberOfPoints (cloud.width * cloud.height);
00403 
00404   for (size_t i = 0; i < cloud.width; ++i)
00405   {
00406     for (size_t j = 0; j < cloud.height; ++j)
00407     {
00408       int queryPoint[3] = {i, j, 0};
00409       vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
00410       const PointT &point = cloud (i, j);
00411 
00412       if (pcl::isFinite (point))
00413       {
00414         float p[3] = {point.x, point.y, point.z};
00415         points->SetPoint (pointId, p);
00416       }
00417       else
00418       {
00419       }
00420     }
00421   }
00422 
00423   structured_grid->SetPoints (points);
00424 
00425   // Check if Normals are present
00426   int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1;
00427   for (size_t d = 0; d < fields.size (); ++d)
00428   {
00429     if (fields[d].name == "normal_x")
00430       normal_x_idx = fields[d].offset;
00431     else if (fields[d].name == "normal_y")
00432       normal_y_idx = fields[d].offset;
00433     else if (fields[d].name == "normal_z")
00434       normal_z_idx = fields[d].offset;
00435   }
00436 
00437   if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1)
00438   {
00439     vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
00440     normals->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
00441     normals->SetNumberOfTuples (cloud.width * cloud.height);
00442     normals->SetName ("Normals");
00443     for (size_t i = 0; i < cloud.width; ++i)
00444     {
00445       for (size_t j = 0; j < cloud.height; ++j)
00446       {
00447         int queryPoint[3] = {i, j, 0};
00448         vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
00449         const PointT &point = cloud (i, j);
00450 
00451         float normal[3];
00452         pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]);
00453         pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]);
00454         pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]);
00455         normals->SetTupleValue (pointId, normal);
00456       }
00457     }
00458 
00459     structured_grid->GetPointData ()->SetNormals (normals);
00460   }
00461 
00462   // Colors (optional)
00463   int rgb_idx = -1;
00464   for (size_t d = 0; d < fields.size (); ++d)
00465   {
00466     if (fields[d].name == "rgb" || fields[d].name == "rgba")
00467     {
00468       rgb_idx = fields[d].offset;
00469       break;
00470     }
00471   }
00472 
00473   if (rgb_idx != -1)
00474   {
00475     vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
00476     colors->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls
00477     colors->SetNumberOfTuples (cloud.width * cloud.height);
00478     colors->SetName ("Colors");
00479     for (size_t i = 0; i < cloud.width; ++i)
00480     {
00481       for (size_t j = 0; j < cloud.height; ++j)
00482       {
00483         int queryPoint[3] = {i, j, 0};
00484         vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint);
00485         const PointT &point = cloud (i, j);
00486 
00487         if (pcl::isFinite (point))
00488         {
00489 
00490           unsigned char color[3];
00491           pcl::RGB rgb;
00492           pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba);
00493           color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b;
00494           colors->SetTupleValue (pointId, color);
00495         }
00496         else
00497         {
00498         }
00499       }
00500     }
00501     structured_grid->GetPointData ()->AddArray (colors);
00502   }
00503 }
00504 
00505 #endif  //#ifndef PCL_IO_VTK_IO_H_
00506 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:38:41