vtk_io.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) 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.cpp 6141 2012-07-04 20:28:01Z rusu $
00037  *
00038  */
00039 
00040 #include <pcl/point_types.h>
00041 #include <pcl/io/vtk_io.h>
00042 #include <fstream>
00043 #include <iostream>
00044 #include <pcl/common/io.h>
00045 
00047 int
00048 pcl::io::saveVTKFile (const std::string &file_name, 
00049                       const pcl::PolygonMesh &triangles, unsigned precision)
00050 {
00051   if (triangles.cloud.data.empty ())
00052   {
00053     PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no data!\n");
00054     return (-1);
00055   }
00056 
00057   // Open file
00058   std::ofstream fs;
00059   fs.precision (precision);
00060   fs.open (file_name.c_str ());
00061 
00062   unsigned int nr_points  = triangles.cloud.width * triangles.cloud.height;
00063   unsigned int point_size = static_cast<unsigned int> (triangles.cloud.data.size () / nr_points);
00064 
00065   // Write the header information
00066   fs << "# vtk DataFile Version 3.0\nvtk output\nASCII\nDATASET POLYDATA\nPOINTS " << nr_points << " float" << std::endl;
00067 
00068   // Iterate through the points
00069   for (unsigned int i = 0; i < nr_points; ++i)
00070   {
00071     int xyz = 0;
00072     for (size_t d = 0; d < triangles.cloud.fields.size (); ++d)
00073     {
00074       int count = triangles.cloud.fields[d].count;
00075       if (count == 0)
00076         count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
00077       int c = 0;
00078       if ((triangles.cloud.fields[d].datatype == sensor_msgs::PointField::FLOAT32) && (
00079            triangles.cloud.fields[d].name == "x" || 
00080            triangles.cloud.fields[d].name == "y" || 
00081            triangles.cloud.fields[d].name == "z"))
00082       {
00083         float value;
00084         memcpy (&value, &triangles.cloud.data[i * point_size + triangles.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
00085         fs << value;
00086         if (++xyz == 3)
00087           break;
00088       }
00089       fs << " ";
00090     }
00091     if (xyz != 3)
00092     {
00093       PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no XYZ data!\n");
00094       return (-2);
00095     }
00096     fs << std::endl;
00097   }
00098 
00099   // Write vertices
00100   fs << "\nVERTICES " << nr_points << " " << 2*nr_points << std::endl;
00101   for (unsigned int i = 0; i < nr_points; ++i)
00102     fs << "1 " << i << std::endl;
00103 
00104   // Write polygons
00105   // compute the correct number of values:
00106   size_t triangle_size = triangles.polygons.size ();
00107   size_t correct_number = triangle_size;
00108   for (size_t i = 0; i < triangle_size; ++i)
00109     correct_number += triangles.polygons[i].vertices.size ();
00110   fs << "\nPOLYGONS " << triangle_size << " " << correct_number << std::endl;
00111   for (size_t i = 0; i < triangle_size; ++i)
00112   {
00113     fs << triangles.polygons[i].vertices.size () << " ";
00114     size_t j = 0;
00115     for (j = 0; j < triangles.polygons[i].vertices.size () - 1; ++j)
00116       fs << triangles.polygons[i].vertices[j] << " ";
00117     fs << triangles.polygons[i].vertices[j] << std::endl;
00118   }
00119 
00120   // Write RGB values
00121   int field_index = getFieldIndex (triangles.cloud, "rgb");
00122   if (field_index != -1)
00123   {
00124     fs << "\nPOINT_DATA " << nr_points << "\nCOLOR_SCALARS scalars 3\n";
00125     for (unsigned int i = 0; i < nr_points; ++i)
00126     {
00127       int count = triangles.cloud.fields[field_index].count;
00128       if (count == 0)
00129         count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
00130       int c = 0;
00131       if (triangles.cloud.fields[field_index].datatype == sensor_msgs::PointField::FLOAT32)
00132       {
00133         pcl::RGB color;
00134         memcpy (&color, &triangles.cloud.data[i * point_size + triangles.cloud.fields[field_index].offset + c * sizeof (float)], sizeof (RGB));
00135         int r = color.r;
00136         int g = color.g;
00137         int b = color.b;
00138         fs << static_cast<float> (r) / 255.0f << " " << static_cast<float> (g) / 255.0f << " " << static_cast<float> (b) / 255.0f;
00139       }
00140       fs << std::endl;
00141     }
00142   }
00143 
00144   // Close file
00145   fs.close ();
00146   return (0);
00147 }
00148 
00150 int
00151 pcl::io::saveVTKFile (const std::string &file_name, 
00152                       const sensor_msgs::PointCloud2 &cloud, unsigned precision)
00153 {
00154   if (cloud.data.empty ())
00155   {
00156     PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no data!\n");
00157     return (-1);
00158   }
00159 
00160   // Open file
00161   std::ofstream fs;
00162   fs.precision (precision);
00163   fs.open (file_name.c_str ());
00164 
00165   unsigned int nr_points  = cloud.width * cloud.height;
00166   unsigned int point_size = static_cast<unsigned int> (cloud.data.size () / nr_points);
00167 
00168   // Write the header information
00169   fs << "# vtk DataFile Version 3.0\nvtk output\nASCII\nDATASET POLYDATA\nPOINTS " << nr_points << " float" << std::endl;
00170 
00171   // Iterate through the points
00172   for (unsigned int i = 0; i < nr_points; ++i)
00173   {
00174     int xyz = 0;
00175     for (size_t d = 0; d < cloud.fields.size (); ++d)
00176     {
00177       int count = cloud.fields[d].count;
00178       if (count == 0)
00179         count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
00180       int c = 0;
00181       if ((cloud.fields[d].datatype == sensor_msgs::PointField::FLOAT32) && (
00182            cloud.fields[d].name == "x" || 
00183            cloud.fields[d].name == "y" || 
00184            cloud.fields[d].name == "z"))
00185       {
00186         float value;
00187         memcpy (&value, &cloud.data[i * point_size + cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
00188         fs << value;
00189         if (++xyz == 3)
00190           break;
00191       }
00192       fs << " ";
00193     }
00194     if (xyz != 3)
00195     {
00196       PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no XYZ data!\n");
00197       return (-2);
00198     }
00199     fs << std::endl;
00200   }
00201 
00202   // Write vertices
00203   fs << "\nVERTICES " << nr_points << " " << 2*nr_points << std::endl;
00204   for (unsigned int i = 0; i < nr_points; ++i)
00205     fs << "1 " << i << std::endl;
00206 
00207   // Write RGB values
00208   int field_index = getFieldIndex (cloud, "rgb");
00209   if (field_index != -1)
00210   {
00211     fs << "\nPOINT_DATA " << nr_points << "\nCOLOR_SCALARS scalars 3\n";
00212     for (unsigned int i = 0; i < nr_points; ++i)
00213     {
00214       int count = cloud.fields[field_index].count;
00215       if (count == 0)
00216         count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
00217       int c = 0;
00218       if (cloud.fields[field_index].datatype == sensor_msgs::PointField::FLOAT32)
00219       {
00220         pcl::RGB color;
00221         memcpy (&color, &cloud.data[i * point_size + cloud.fields[field_index].offset + c * sizeof (float)], sizeof (RGB));
00222         int r = color.r;
00223         int g = color.g;
00224         int b = color.b;
00225         fs << static_cast<float> (r) / 255.0f << " " << static_cast<float> (g) / 255.0f << " " << static_cast<float> (b) / 255.0f;
00226       }
00227       fs << std::endl;
00228     }
00229   }
00230 
00231   // Close file
00232   fs.close ();
00233   return (0);
00234 }
00235 


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