vtk_utils.cpp
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) 2011, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * $Id: vtk_utils.cpp 5066 2012-03-14 06:42:21Z rusu $
00036  *
00037  */
00038 
00039 
00040 #include <pcl/surface/vtk_smoothing/vtk_utils.h>
00041 
00042 #include <pcl/ros/conversions.h>
00043 #include <pcl/common/common.h>
00044 #include <vtkCellArray.h>
00045 #include <vtkTriangleFilter.h>
00046 #include <vtkPoints.h>
00047 #include <vtkPolyData.h>
00048 #include <vtkPointData.h>
00049 #include <vtkFloatArray.h>
00050 
00051 
00053 int
00054 pcl::VTKUtils::convertToVTK (const pcl::PolygonMesh &triangles, vtkSmartPointer<vtkPolyData> &triangles_out_vtk)
00055 {
00056   if (triangles.cloud.data.empty ())
00057   {
00058     PCL_ERROR ("[pcl::surface::convertToVTK] Input point cloud has no data!\n");
00059     return (-1);
00060   }
00061 
00062   vtkSmartPointer<vtkPolyData> vtk_polygons;
00063   mesh2vtk (triangles, vtk_polygons);
00064 
00065   vtkSmartPointer<vtkTriangleFilter> vtk_triangles = vtkTriangleFilter::New ();
00066   vtk_triangles->SetInput (vtk_polygons);
00067   vtk_triangles->Update();
00068 
00069   triangles_out_vtk = vtk_triangles->GetOutput ();
00070   return 1;
00071 }
00072 
00073 
00075 void
00076 pcl::VTKUtils::convertToPCL (vtkSmartPointer<vtkPolyData> &vtk_polygons, pcl::PolygonMesh &triangles)
00077 {
00078   vtk2mesh (vtk_polygons, triangles);
00079 }
00080 
00082 int
00083 pcl::VTKUtils::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMesh& mesh)
00084 {
00085   mesh.polygons.clear ();
00086   mesh.cloud.data.clear ();
00087   mesh.cloud.width = mesh.cloud.height = 0;
00088   mesh.cloud.is_dense = true;
00089 
00090   vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
00091   vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
00092   vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
00093 
00094   if (nr_points == 0)
00095     return 0;
00096 
00097   vtkUnsignedCharArray* poly_colors = NULL;
00098   if (poly_data->GetPointData() != NULL)
00099     poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
00100 
00101   // Some applications do not save the name of scalars (including PCL's native vtk_io)
00102   if (!poly_colors)
00103     poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
00104 
00105   // TODO: currently only handles rgb values with 3 components
00106   if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
00107   {
00108     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZRGB>());
00109     cloud_temp->points.resize (nr_points);
00110     double point_xyz[3];
00111     unsigned char point_color[3];
00112     for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i)
00113     {
00114       mesh_points->GetPoint (i, &point_xyz[0]);
00115       cloud_temp->points[i].x = static_cast<float> (point_xyz[0]);
00116       cloud_temp->points[i].y = static_cast<float> (point_xyz[1]);
00117       cloud_temp->points[i].z = static_cast<float> (point_xyz[2]);
00118 
00119       poly_colors->GetTupleValue (i, &point_color[0]);
00120       cloud_temp->points[i].r = point_color[0];
00121       cloud_temp->points[i].g = point_color[1];
00122       cloud_temp->points[i].b = point_color[2];
00123     }
00124     cloud_temp->width = static_cast<uint32_t> (cloud_temp->points.size ());
00125     cloud_temp->height = 1;
00126     cloud_temp->is_dense = true;
00127 
00128     pcl::toROSMsg (*cloud_temp, mesh.cloud);
00129   }
00130   else // in case points do not have color information:
00131   {
00132     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ> ());
00133     cloud_temp->points.resize (nr_points);
00134     double point_xyz[3];
00135     for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i)
00136     {
00137       mesh_points->GetPoint (i, &point_xyz[0]);
00138       cloud_temp->points[i].x = static_cast<float> (point_xyz[0]);
00139       cloud_temp->points[i].y = static_cast<float> (point_xyz[1]);
00140       cloud_temp->points[i].z = static_cast<float> (point_xyz[2]);
00141     }
00142     cloud_temp->width = static_cast<uint32_t> (cloud_temp->points.size ());
00143     cloud_temp->height = 1;
00144     cloud_temp->is_dense = true;
00145 
00146     pcl::toROSMsg (*cloud_temp, mesh.cloud);
00147   }
00148 
00149   mesh.polygons.resize (nr_polygons);
00150   vtkIdType* cell_points;
00151   vtkIdType nr_cell_points;
00152   vtkCellArray * mesh_polygons = poly_data->GetPolys ();
00153   mesh_polygons->InitTraversal ();
00154   int id_poly = 0;
00155   while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
00156   {
00157     mesh.polygons[id_poly].vertices.resize (nr_cell_points);
00158     for (int i = 0; i < nr_cell_points; ++i)
00159       mesh.polygons[id_poly].vertices[i] = static_cast<unsigned int> (cell_points[i]);
00160     ++id_poly;
00161   }
00162 
00163   return (static_cast<int> (nr_points));
00164 }
00165 
00167 int
00168 pcl::VTKUtils::mesh2vtk (const pcl::PolygonMesh& mesh, vtkSmartPointer<vtkPolyData> &poly_data)
00169 {
00170   int nr_points = mesh.cloud.width * mesh.cloud.height;
00171   int nr_polygons = static_cast<int> (mesh.polygons.size ());
00172 
00173   // reset vtkPolyData object
00174   poly_data = vtkSmartPointer<vtkPolyData>::New (); // OR poly_data->Reset();
00175   vtkSmartPointer<vtkPoints> vtk_mesh_points = vtkSmartPointer<vtkPoints>::New ();
00176   vtkSmartPointer<vtkCellArray> vtk_mesh_polygons = vtkSmartPointer<vtkCellArray>::New ();
00177   poly_data->SetPoints (vtk_mesh_points);
00178 
00179   // get field indices for x, y, z (as well as rgb and/or rgba)
00180   int idx_x = -1, idx_y = -1, idx_z = -1, idx_rgb = -1, idx_rgba = -1, idx_normal_x = -1, idx_normal_y = -1, idx_normal_z = -1;
00181   for (int d = 0; d < static_cast<int> (mesh.cloud.fields.size ()); ++d)
00182   {
00183     if (mesh.cloud.fields[d].name == "x") idx_x = d;
00184     else if (mesh.cloud.fields[d].name == "y") idx_y = d;
00185     else if (mesh.cloud.fields[d].name == "z") idx_z = d;
00186     else if (mesh.cloud.fields[d].name == "rgb") idx_rgb = d;
00187     else if (mesh.cloud.fields[d].name == "rgba") idx_rgba = d;
00188     else if (mesh.cloud.fields[d].name == "normal_x") idx_normal_x = d;
00189     else if (mesh.cloud.fields[d].name == "normal_y") idx_normal_y = d;
00190     else if (mesh.cloud.fields[d].name == "normal_z") idx_normal_z = d;
00191   }
00192   if ( ( idx_x == -1 ) || ( idx_y == -1 ) || ( idx_z == -1 ) )
00193     nr_points = 0;
00194 
00195   // copy point data
00196   vtk_mesh_points->SetNumberOfPoints (nr_points);
00197   if (nr_points > 0)
00198   {
00199     Eigen::Vector4f pt = Eigen::Vector4f::Zero ();
00200     Eigen::Array4i xyz_offset (mesh.cloud.fields[idx_x].offset, mesh.cloud.fields[idx_y].offset, mesh.cloud.fields[idx_z].offset, 0);
00201     for (vtkIdType cp = 0; cp < nr_points; ++cp, xyz_offset += mesh.cloud.point_step)
00202     {
00203       memcpy(&pt[0], &mesh.cloud.data[xyz_offset[0]], sizeof(float));
00204       memcpy(&pt[1], &mesh.cloud.data[xyz_offset[1]], sizeof(float));
00205       memcpy(&pt[2], &mesh.cloud.data[xyz_offset[2]], sizeof(float));
00206       vtk_mesh_points->InsertPoint(cp, pt[0], pt[1], pt[2]);
00207     }
00208   }
00209 
00210   // copy polygon data
00211   if (nr_polygons > 0)
00212   {
00213     for (int i = 0; i < nr_polygons; i++)
00214     {
00215       unsigned int nr_points_in_polygon = static_cast<unsigned int> (mesh.polygons[i].vertices.size ());
00216       vtk_mesh_polygons->InsertNextCell (nr_points_in_polygon);
00217       for (unsigned int j = 0; j < nr_points_in_polygon; j++)
00218         vtk_mesh_polygons->InsertCellPoint(mesh.polygons[i].vertices[j]);
00219     }
00220     poly_data->SetPolys (vtk_mesh_polygons);
00221   }
00222 
00223   // copy color information
00224   if (idx_rgb != -1 || idx_rgba != -1)
00225   {
00226     vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00227     colors->SetNumberOfComponents (3);
00228     colors->SetName ("Colors");
00229     pcl::RGB rgb;
00230     int offset = (idx_rgb != -1) ? mesh.cloud.fields[idx_rgb].offset : mesh.cloud.fields[idx_rgba].offset;
00231     for (vtkIdType cp = 0; cp < nr_points; ++cp)
00232     {
00233       memcpy (&rgb, &mesh.cloud.data[cp * mesh.cloud.point_step + offset], sizeof (pcl::RGB));
00234       const unsigned char color[3] = {rgb.r, rgb.g, rgb.b};
00235       colors->InsertNextTupleValue (color);
00236     }
00237     poly_data->GetPointData ()->SetScalars (colors);
00238   }
00239 
00240   // copy normal information
00241   if ( ( idx_normal_x != -1 ) && ( idx_normal_y != -1 ) && ( idx_normal_z != -1 ) )
00242   {
00243     vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
00244     normals->SetNumberOfComponents (3);
00245     float nx = 0.0f, ny = 0.0f, nz = 0.0f;
00246     for (vtkIdType cp = 0; cp < nr_points; ++cp)
00247     {
00248       memcpy (&nx, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_x].offset], sizeof(float));
00249       memcpy (&ny, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_y].offset], sizeof(float));
00250       memcpy (&nz, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_z].offset], sizeof(float));
00251       const float normal[3] = {nx, ny, nz};
00252       normals->InsertNextTupleValue (normal);
00253     }
00254     poly_data->GetPointData()->SetNormals (normals);
00255   }
00256 
00257   if (poly_data->GetPoints() == NULL)
00258     return (0);
00259   return (static_cast<int> (poly_data->GetPoints()->GetNumberOfPoints ()));
00260 }
00261 
00262 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:19:12