Point Cloud Library (PCL)  1.3.1
pcd_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: pcd_io.hpp 2693 2011-10-11 03:45:58Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_IO_PCD_IO_IMPL_H_
00041 #define PCL_IO_PCD_IO_IMPL_H_
00042 
00043 #include <fstream>
00044 #include <fcntl.h>
00045 #include <string>
00046 #include <stdlib.h>
00047 #include <boost/algorithm/string.hpp>
00048 #include <pcl/console/print.h>
00049 #ifdef _WIN32
00050 # include <io.h>
00051 # define WIN32_LEAN_AND_MEAN
00052 # define NOMINMAX
00053 # include <windows.h>
00054 # define pcl_open                    _open
00055 # define pcl_close(fd)               _close(fd)
00056 # define pcl_lseek(fd,offset,origin) _lseek(fd,offset,origin)
00057 #else
00058 # include <sys/mman.h>
00059 # define pcl_open                    open
00060 # define pcl_close(fd)               close(fd)
00061 # define pcl_lseek(fd,offset,origin) lseek(fd,offset,origin)
00062 #endif
00063 
00064 #include <pcl/io/lzf.h>
00065 
00067 template <typename PointT> std::string
00068 pcl::PCDWriter::generateHeader (const pcl::PointCloud<PointT> &cloud, const int nr_points)
00069 {
00070   std::ostringstream oss;
00071   oss.imbue (std::locale::classic ());
00072 
00073   oss << "# .PCD v0.7 - Point Cloud Data file format"
00074          "\nVERSION 0.7"
00075          "\nFIELDS";
00076 
00077   std::vector<sensor_msgs::PointField> fields;
00078   pcl::getFields (cloud, fields);
00079  
00080   std::stringstream field_names, field_types, field_sizes, field_counts;
00081   for (size_t i = 0; i < fields.size (); ++i)
00082   {
00083     if (fields[i].name == "_")
00084       continue;
00085     // Add the regular dimension
00086     field_names << " " << fields[i].name;
00087     field_sizes << " " << pcl::getFieldSize (fields[i].datatype);
00088     field_types << " " << pcl::getFieldType (fields[i].datatype);
00089     int count = abs ((int)fields[i].count);
00090     if (count == 0) count = 1;  // check for 0 counts (coming from older converter code)
00091     field_counts << " " << count;
00092   }
00093   oss << field_names.str ();
00094   oss << "\nSIZE" << field_sizes.str () 
00095       << "\nTYPE" << field_types.str () 
00096       << "\nCOUNT" << field_counts.str ();
00097   // If the user passes in a number of points value, use that instead
00098   if (nr_points != std::numeric_limits<int>::max ())
00099     oss << "\nWIDTH " << nr_points << "\nHEIGHT " << 1 << "\n";
00100   else
00101     oss << "\nWIDTH " << cloud.width << "\nHEIGHT " << cloud.height << "\n";
00102 
00103   oss << "VIEWPOINT " << cloud.sensor_origin_[0] << " " << cloud.sensor_origin_[1] << " " << cloud.sensor_origin_[2] << " " << 
00104                          cloud.sensor_orientation_.w () << " " << 
00105                          cloud.sensor_orientation_.x () << " " << 
00106                          cloud.sensor_orientation_.y () << " " << 
00107                          cloud.sensor_orientation_.z () << "\n";
00108   
00109   // If the user passes in a number of points value, use that instead
00110   if (nr_points != std::numeric_limits<int>::max ())
00111     oss << "POINTS " << nr_points << "\n";
00112   else
00113     oss << "POINTS " << cloud.points.size () << "\n";
00114 
00115   return (oss.str ());
00116 }
00117 
00119 template <typename PointT> int
00120 pcl::PCDWriter::writeBinary (const std::string &file_name, 
00121                              const pcl::PointCloud<PointT> &cloud)
00122 {
00123   if (cloud.points.empty ())
00124   {
00125     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
00126     return (-1);
00127   }
00128   int data_idx = 0;
00129   std::ostringstream oss;
00130   oss << generateHeader<PointT> (cloud) << "DATA binary\n";
00131   oss.flush ();
00132   data_idx = (int) oss.tellp ();
00133 
00134 #if _WIN32
00135   HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
00136   if(h_native_file == INVALID_HANDLE_VALUE)
00137   {
00138     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
00139     return (-1);
00140   }
00141 #else
00142   int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, (mode_t)0600);
00143   if (fd < 0)
00144   {
00145     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
00146     return (-1);
00147   }
00148 #endif
00149 
00150   std::vector<sensor_msgs::PointField> fields;
00151   std::vector<int> fields_sizes;
00152   size_t fsize = 0;
00153   size_t data_size = 0;
00154   size_t nri = 0;
00155   pcl::getFields (cloud, fields);
00156   // Compute the total size of the fields
00157   for (size_t i = 0; i < fields.size (); ++i)
00158   {
00159     if (fields[i].name == "_")
00160       continue;
00161     
00162     int fs = fields[i].count * getFieldSize (fields[i].datatype);
00163     fsize += fs;
00164     fields_sizes.push_back (fs);
00165     fields[nri++] = fields[i];
00166   }
00167   fields.resize (nri);
00168   
00169   data_size = cloud.points.size () * fsize;
00170 
00171   // Prepare the map
00172 #if _WIN32
00173   HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
00174   char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
00175   CloseHandle (fm);
00176 
00177 #else
00178   // Stretch the file size to the size of the data
00179   int result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
00180   if (result < 0)
00181   {
00182     pcl_close (fd);
00183     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
00184     return (-1);
00185   }
00186   // Write a bogus entry so that the new file size comes in effect
00187   result = ::write (fd, "", 1);
00188   if (result != 1)
00189   {
00190     pcl_close (fd);
00191     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!");
00192     return (-1);
00193   }
00194 
00195   char *map = (char*)mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0);
00196   if (map == MAP_FAILED)
00197   {
00198     pcl_close (fd);
00199     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
00200     return (-1);
00201   }
00202 #endif
00203 
00204   // Copy the header
00205   memcpy (&map[0], oss.str ().c_str (), data_idx);
00206 
00207   // Copy the data
00208   char *out = &map[0] + data_idx;
00209   for (size_t i = 0; i < cloud.points.size (); ++i)
00210   {
00211     int nrj = 0;
00212     for (size_t j = 0; j < fields.size (); ++j)
00213     {
00214       memcpy (out, (const char*)&cloud.points[i] + fields[j].offset, fields_sizes[nrj]);
00215       out += fields_sizes[nrj++];
00216     }
00217   }
00218 
00219   // If the user set the synchronization flag on, call msync
00220 #if !_WIN32
00221   if (map_synchronization_)
00222     msync (map, data_idx + data_size, MS_SYNC);
00223 #endif
00224 
00225   // Unmap the pages of memory
00226 #if _WIN32
00227     UnmapViewOfFile (map);
00228 #else
00229   if (munmap (map, (data_idx + data_size)) == -1)
00230   {
00231     pcl_close (fd);
00232     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
00233     return (-1);
00234   }
00235 #endif
00236   // Close file
00237 #if _WIN32
00238   CloseHandle (h_native_file);
00239 #else
00240   pcl_close (fd);
00241 #endif
00242   return (0);
00243 }
00244 
00246 template <typename PointT> int
00247 pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, 
00248                                        const pcl::PointCloud<PointT> &cloud)
00249 {
00250   if (cloud.points.empty ())
00251   {
00252     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!");
00253     return (-1);
00254   }
00255   int data_idx = 0;
00256   std::ostringstream oss;
00257   oss << generateHeader<PointT> (cloud) << "DATA binary_compressed\n";
00258   oss.flush ();
00259   data_idx = oss.tellp ();
00260 
00261 #if _WIN32
00262   HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
00263   if(h_native_file == INVALID_HANDLE_VALUE)
00264   {
00265     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!");
00266     return (-1);
00267   }
00268 #else
00269   int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, (mode_t)0600);
00270   if (fd < 0)
00271   {
00272     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during open!");
00273     return (-1);
00274   }
00275 #endif
00276 
00277   std::vector<sensor_msgs::PointField> fields;
00278   size_t fsize = 0;
00279   size_t data_size = 0;
00280   size_t nri = 0;
00281   pcl::getFields (cloud, fields);
00282   std::vector<int> fields_sizes (fields.size ());
00283   // Compute the total size of the fields
00284   for (size_t i = 0; i < fields.size (); ++i)
00285   {
00286     if (fields[i].name == "_")
00287       continue;
00288     
00289     fields_sizes[nri] = fields[i].count * pcl::getFieldSize (fields[i].datatype);
00290     fsize += fields_sizes[nri];
00291     fields[nri] = fields[i];
00292     ++nri;
00293   }
00294   fields_sizes.resize (nri);
00295   fields.resize (nri);
00296  
00297   // Compute the size of data
00298   data_size = cloud.points.size () * fsize;
00299 
00301   // Empty array holding only the valid data
00302   // data_size = nr_points * point_size 
00303   //           = nr_points * (sizeof_field_1 + sizeof_field_2 + ... sizeof_field_n)
00304   //           = sizeof_field_1 * nr_points + sizeof_field_2 * nr_points + ... sizeof_field_n * nr_points
00305   char *only_valid_data = (char*)malloc (data_size);
00306 
00307   // Convert the XYZRGBXYZRGB structure to XXYYZZRGBRGB to aid compression. For
00308   // this, we need a vector of fields.size () (4 in this case), which points to
00309   // each individual plane:
00310   //   pters[0] = &only_valid_data[offset_of_plane_x];
00311   //   pters[1] = &only_valid_data[offset_of_plane_y];
00312   //   pters[2] = &only_valid_data[offset_of_plane_z];
00313   //   pters[3] = &only_valid_data[offset_of_plane_RGB];
00314   //
00315   std::vector<char*> pters (fields.size ());
00316   int toff = 0;
00317   for (size_t i = 0; i < pters.size (); ++i)
00318   {
00319     pters[i] = &only_valid_data[toff];
00320     toff += fields_sizes[i] * cloud.points.size ();
00321   }
00322   
00323   // Go over all the points, and copy the data in the appropriate places
00324   for (size_t i = 0; i < cloud.points.size (); ++i)
00325   {
00326     for (size_t j = 0; j < fields.size (); ++j)
00327     {
00328       memcpy (pters[j], (const char*)&cloud.points[i] + fields[j].offset, fields_sizes[j]);
00329       // Increment the pointer
00330       pters[j] += fields_sizes[j];
00331     }
00332   }
00333 
00334   char* temp_buf = (char*)malloc (data_size * 1.5 + 8);
00335   // Compress the valid data
00336   unsigned int compressed_size = pcl::lzfCompress (only_valid_data, data_size, &temp_buf[8], data_size * 1.5);
00337   unsigned int compressed_final_size = 0;
00338   // Was the compression successful?
00339   if (compressed_size)
00340   {
00341     char *header = &temp_buf[0];
00342     memcpy (&header[0], &compressed_size, sizeof (unsigned int));
00343     memcpy (&header[4], &data_size, sizeof (unsigned int));
00344     data_size = compressed_size + 8;
00345     compressed_final_size = data_size + data_idx;
00346   }
00347   else
00348   {
00349 #if !_WIN32
00350     pcl_close (fd);
00351 #endif
00352     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!");
00353     return (-1);
00354   }
00355 
00356 #if !_WIN32
00357   // Stretch the file size to the size of the data
00358   int result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
00359   if (result < 0)
00360   {
00361     pcl_close (fd);
00362     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during lseek ()!");
00363     return (-1);
00364   }
00365   // Write a bogus entry so that the new file size comes in effect
00366   result = ::write (fd, "", 1);
00367   if (result != 1)
00368   {
00369     pcl_close (fd);
00370     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during write ()!");
00371     return (-1);
00372   }
00373 #endif
00374 
00375   // Prepare the map
00376 #if _WIN32
00377   HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
00378   char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
00379   CloseHandle (fm);
00380 
00381 #else
00382   char *map = (char*)mmap (0, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0);
00383   if (map == MAP_FAILED)
00384   {
00385     pcl_close (fd);
00386     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!");
00387     return (-1);
00388   }
00389 #endif
00390 
00391   // Copy the header
00392   memcpy (&map[0], oss.str ().c_str (), data_idx);
00393   // Copy the compressed data
00394   memcpy (&map[data_idx], temp_buf, data_size);
00395 
00396 #if !_WIN32
00397   // If the user set the synchronization flag on, call msync
00398   if (map_synchronization_)
00399     msync (map, compressed_final_size, MS_SYNC);
00400 #endif
00401 
00402   // Unmap the pages of memory
00403 #if _WIN32
00404     UnmapViewOfFile (map);
00405 #else
00406   if (munmap (map, (compressed_final_size)) == -1)
00407   {
00408     pcl_close (fd);
00409     throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!");
00410     return (-1);
00411   }
00412 #endif
00413   // Close file
00414 #if _WIN32
00415   CloseHandle (h_native_file);
00416 #else
00417   pcl_close (fd);
00418 #endif
00419 
00420   free (only_valid_data);
00421   free (temp_buf);
00422   return (0);
00423 }
00424 
00425 
00427 template <typename PointT> int
00428 pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, 
00429                             const int precision)
00430 {
00431   if (cloud.points.empty ())
00432   {
00433     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
00434     return (-1);
00435   }
00436 
00437   if (cloud.width * cloud.height != cloud.points.size ())
00438   {
00439     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
00440     return (-1);
00441   }
00442 
00443   std::ofstream fs;
00444   fs.open (file_name.c_str ());      // Open file
00445   
00446   if (!fs.is_open () || fs.fail ())
00447   {
00448     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
00449     return (-1);
00450   }
00451   
00452   fs.precision (precision);
00453   fs.imbue (std::locale::classic ());
00454 
00455   std::vector<sensor_msgs::PointField> fields;
00456   pcl::getFields (cloud, fields);
00457 
00458   // Write the header information
00459   fs << generateHeader<PointT> (cloud) << "DATA ascii\n";
00460 
00461   std::ostringstream stream;
00462   stream.precision (precision);
00463   stream.imbue (std::locale::classic ());
00464   // Iterate through the points
00465   for (size_t i = 0; i < cloud.points.size (); ++i)
00466   {
00467     for (size_t d = 0; d < fields.size (); ++d)
00468     {
00469       // Ignore invalid padded dimensions that are inherited from binary data
00470       if (fields[d].name == "_")
00471         continue;
00472 
00473       int count = fields[d].count;
00474       if (count == 0) 
00475         count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
00476 
00477       for (int c = 0; c < count; ++c)
00478       {
00479         switch (fields[d].datatype)
00480         {
00481           case sensor_msgs::PointField::INT8:
00482           {
00483             int8_t value;
00484             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (int8_t), sizeof (int8_t));
00485             if (pcl_isnan (value))
00486               stream << "nan";
00487             else
00488               stream << boost::numeric_cast<uint32_t>(value);
00489             break;
00490           }
00491           case sensor_msgs::PointField::UINT8:
00492           {
00493             uint8_t value;
00494             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (uint8_t), sizeof (uint8_t));
00495             if (pcl_isnan (value))
00496               stream << "nan";
00497             else
00498               stream << boost::numeric_cast<uint32_t>(value);
00499             break;
00500           }
00501           case sensor_msgs::PointField::INT16:
00502           {
00503             int16_t value;
00504             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (int16_t), sizeof (int16_t));
00505             if (pcl_isnan (value))
00506               stream << "nan";
00507             else
00508               stream << boost::numeric_cast<int16_t>(value);
00509             break;
00510           }
00511           case sensor_msgs::PointField::UINT16:
00512           {
00513             uint16_t value;
00514             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (uint16_t), sizeof (uint16_t));
00515             if (pcl_isnan (value))
00516               stream << "nan";
00517             else
00518               stream << boost::numeric_cast<uint16_t>(value);
00519             break;
00520           }
00521           case sensor_msgs::PointField::INT32:
00522           {
00523             int32_t value;
00524             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (int32_t), sizeof (int32_t));
00525             if (pcl_isnan (value))
00526               stream << "nan";
00527             else
00528               stream << boost::numeric_cast<int32_t>(value);
00529             break;
00530           }
00531           case sensor_msgs::PointField::UINT32:
00532           {
00533             uint32_t value;
00534             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (uint32_t), sizeof (uint32_t));
00535             if (pcl_isnan (value))
00536               stream << "nan";
00537             else
00538               stream << boost::numeric_cast<uint32_t>(value);
00539             break;
00540           }
00541           case sensor_msgs::PointField::FLOAT32:
00542           {
00543             float value;
00544             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (float), sizeof (float));
00545             if (pcl_isnan (value))
00546               stream << "nan";
00547             else
00548               stream << boost::numeric_cast<float>(value);
00549             break;
00550           }
00551           case sensor_msgs::PointField::FLOAT64:
00552           {
00553             double value;
00554             memcpy (&value, (const char*)&cloud.points[i] + fields[d].offset + c * sizeof (double), sizeof (double));
00555             if (pcl_isnan (value))
00556               stream << "nan";
00557             else
00558               stream << boost::numeric_cast<double>(value);
00559             break;
00560           }
00561           default:
00562             PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
00563             break;
00564         }
00565 
00566         if (d < fields.size () - 1 || c < (int)fields[d].count - 1)
00567           stream << " ";
00568       }
00569     }
00570     // Copy the stream, trim it, and write it to disk
00571     std::string result = stream.str ();
00572     boost::trim (result);
00573     stream.str ("");
00574     fs << result << "\n";
00575   }
00576   fs.close ();              // Close file
00577   return (0);
00578 }
00579 
00580 
00582 template <typename PointT> int
00583 pcl::PCDWriter::writeBinary (const std::string &file_name, 
00584                              const pcl::PointCloud<PointT> &cloud, 
00585                              const std::vector<int> &indices)
00586 {
00587   if (cloud.points.empty () || indices.empty ())
00588   {
00589     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
00590     return (-1);
00591   }
00592   int data_idx = 0;
00593   std::ostringstream oss;
00594   oss << generateHeader<PointT> (cloud, indices.size ()) << "DATA binary\n";
00595   oss.flush ();
00596   data_idx = oss.tellp ();
00597 
00598 #if _WIN32
00599   HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
00600   if(h_native_file == INVALID_HANDLE_VALUE)
00601   {
00602     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
00603     return (-1);
00604   }
00605 #else
00606   int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, (mode_t)0600);
00607   if (fd < 0)
00608   {
00609     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
00610     return (-1);
00611   }
00612 #endif
00613 
00614   std::vector<sensor_msgs::PointField> fields;
00615   std::vector<int> fields_sizes;
00616   size_t fsize = 0;
00617   size_t data_size = 0;
00618   size_t nri = 0;
00619   pcl::getFields (cloud, fields);
00620   // Compute the total size of the fields
00621   for (size_t i = 0; i < fields.size (); ++i)
00622   {
00623     if (fields[i].name == "_")
00624       continue;
00625     
00626     int fs = fields[i].count * getFieldSize (fields[i].datatype);
00627     fsize += fs;
00628     fields_sizes.push_back (fs);
00629     fields[nri++] = fields[i];
00630   }
00631   fields.resize (nri);
00632   
00633   data_size = indices.size () * fsize;
00634 
00635   // Prepare the map
00636 #if _WIN32
00637   HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
00638   char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
00639   CloseHandle (fm);
00640 
00641 #else
00642   // Stretch the file size to the size of the data
00643   int result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
00644   if (result < 0)
00645   {
00646     pcl_close (fd);
00647     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
00648     return (-1);
00649   }
00650   // Write a bogus entry so that the new file size comes in effect
00651   result = ::write (fd, "", 1);
00652   if (result != 1)
00653   {
00654     pcl_close (fd);
00655     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!");
00656     return (-1);
00657   }
00658 
00659   char *map = (char*)mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0);
00660   if (map == MAP_FAILED)
00661   {
00662     pcl_close (fd);
00663     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
00664     return (-1);
00665   }
00666 #endif
00667 
00668   // Copy the header
00669   memcpy (&map[0], oss.str ().c_str (), data_idx);
00670 
00671   char *out = &map[0] + data_idx;
00672   // Copy the data
00673   for (size_t i = 0; i < indices.size (); ++i)
00674   {
00675     int nrj = 0;
00676     for (size_t j = 0; j < fields.size (); ++j)
00677     {
00678       memcpy (out, (const char*)&cloud.points[indices[i]] + fields[j].offset, fields_sizes[nrj]);
00679       out += fields_sizes[nrj++];
00680     }
00681   }
00682 
00683 #if !_WIN32
00684   // If the user set the synchronization flag on, call msync
00685   if (map_synchronization_)
00686     msync (map, data_idx + data_size, MS_SYNC);
00687 #endif
00688 
00689   // Unmap the pages of memory
00690 #if _WIN32
00691     UnmapViewOfFile (map);
00692 #else
00693   if (munmap (map, (data_idx + data_size)) == -1)
00694   {
00695     pcl_close (fd);
00696     throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
00697     return (-1);
00698   }
00699 #endif
00700   // Close file
00701 #if _WIN32
00702   CloseHandle(h_native_file);
00703 #else
00704   pcl_close (fd);
00705 #endif
00706   return (0);
00707 }
00708 
00710 template <typename PointT> int
00711 pcl::PCDWriter::writeASCII (const std::string &file_name, 
00712                             const pcl::PointCloud<PointT> &cloud, 
00713                             const std::vector<int> &indices,
00714                             const int precision)
00715 {
00716   if (cloud.points.empty () || indices.empty ())
00717   {
00718     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
00719     return (-1);
00720   }
00721 
00722   if (cloud.width * cloud.height != cloud.points.size ())
00723   {
00724     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
00725     return (-1);
00726   }
00727 
00728   std::ofstream fs;
00729   fs.open (file_name.c_str ());      // Open file
00730   if (!fs.is_open () || fs.fail ())
00731   {
00732     throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
00733     return (-1);
00734   }
00735   fs.precision (precision);
00736   fs.imbue (std::locale::classic ());
00737 
00738   std::vector<sensor_msgs::PointField> fields;
00739   pcl::getFields (cloud, fields);
00740 
00741   // Write the header information
00742   fs << generateHeader<PointT> (cloud, indices.size ()) << "DATA ascii\n";
00743 
00744   std::ostringstream stream;
00745   stream.precision (precision);
00746   stream.imbue (std::locale::classic ());
00747 
00748   // Iterate through the points
00749   for (size_t i = 0; i < indices.size (); ++i)
00750   {
00751     for (size_t d = 0; d < fields.size (); ++d)
00752     {
00753       // Ignore invalid padded dimensions that are inherited from binary data
00754       if (fields[d].name == "_")
00755         continue;
00756 
00757       int count = fields[d].count;
00758       if (count == 0) 
00759         count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
00760 
00761       for (int c = 0; c < count; ++c)
00762       {
00763         switch (fields[d].datatype)
00764         {
00765           case sensor_msgs::PointField::INT8:
00766           {
00767             int8_t value;
00768             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (int8_t), sizeof (int8_t));
00769             if (pcl_isnan (value))
00770               stream << "nan";
00771             else
00772               stream << boost::numeric_cast<uint32_t>(value);
00773             break;
00774           }
00775           case sensor_msgs::PointField::UINT8:
00776           {
00777             uint8_t value;
00778             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (uint8_t), sizeof (uint8_t));
00779             if (pcl_isnan (value))
00780               stream << "nan";
00781             else
00782               stream << boost::numeric_cast<uint32_t>(value);
00783             break;
00784           }
00785           case sensor_msgs::PointField::INT16:
00786           {
00787             int16_t value;
00788             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (int16_t), sizeof (int16_t));
00789             if (pcl_isnan (value))
00790               stream << "nan";
00791             else
00792               stream << boost::numeric_cast<int16_t>(value);
00793             break;
00794           }
00795           case sensor_msgs::PointField::UINT16:
00796           {
00797             uint16_t value;
00798             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (uint16_t), sizeof (uint16_t));
00799             if (pcl_isnan (value))
00800               stream << "nan";
00801             else
00802               stream << boost::numeric_cast<uint16_t>(value);
00803             break;
00804           }
00805           case sensor_msgs::PointField::INT32:
00806           {
00807             int32_t value;
00808             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (int32_t), sizeof (int32_t));
00809             if (pcl_isnan (value))
00810               stream << "nan";
00811             else
00812               stream << boost::numeric_cast<int32_t>(value);
00813             break;
00814           }
00815           case sensor_msgs::PointField::UINT32:
00816           {
00817             uint32_t value;
00818             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (uint32_t), sizeof (uint32_t));
00819             if (pcl_isnan (value))
00820               stream << "nan";
00821             else
00822               stream << boost::numeric_cast<uint32_t>(value);
00823             break;
00824           }
00825           case sensor_msgs::PointField::FLOAT32:
00826           {
00827             float value;
00828             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (float), sizeof (float));
00829             if (pcl_isnan (value))
00830               stream << "nan";
00831             else
00832               stream << boost::numeric_cast<float>(value);
00833             break;
00834           }
00835           case sensor_msgs::PointField::FLOAT64:
00836           {
00837             double value;
00838             memcpy (&value, (const char*)&cloud.points[indices[i]] + fields[d].offset + c * sizeof (double), sizeof (double));
00839             if (pcl_isnan (value))
00840               stream << "nan";
00841             else
00842               stream << boost::numeric_cast<double>(value);
00843             break;
00844           }
00845           default:
00846             PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
00847             break;
00848         }
00849 
00850         if (d < fields.size () - 1 || c < (int)fields[d].count - 1)
00851           stream << " ";
00852       }
00853     }
00854     // Copy the stream, trim it, and write it to disk
00855     std::string result = stream.str ();
00856     boost::trim (result);
00857     stream.str ("");
00858     fs << result << "\n";
00859   }
00860   fs.close ();              // Close file
00861   return (0);
00862 }
00863 
00864 #endif  //#ifndef PCL_IO_PCD_IO_H_
00865 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines