ply_parser.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) 2007-2012, Ares Lagae
00006  *  Copyright (c) 2012, Willow Garage, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of Willow Garage, Inc. nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id: ply_parser.cpp 5855 2012-06-07 02:32:40Z holzers $
00038  *
00039  */
00040 
00041 #include <pcl/io/ply/ply_parser.h>
00042 
00043 bool pcl::io::ply::ply_parser::parse (const std::string& filename)
00044 {
00045   std::ifstream istream (filename.c_str (), std::ios::in | std::ios::binary);
00046 
00047   std::string line;
00048   line_number_ = 0;
00049 
00050   std::size_t number_of_format_statements = 0; 
00051   std::size_t number_of_element_statements = 0; 
00052   std::size_t number_of_property_statements = 0; 
00053   std::size_t number_of_obj_info_statements = 0; 
00054   std::size_t number_of_comment_statements = 0;
00055 
00056   format_type format = pcl::io::ply::unknown;
00057   std::vector< boost::shared_ptr<element> > elements;
00058 
00059   // magic
00060   char magic[3];
00061   istream.read (magic, 3);
00062   istream.ignore (1);
00063   ++line_number_;
00064   if (!istream)
00065   {
00066     if (error_callback_)
00067       error_callback_ (line_number_, "parse error");
00068     return false;
00069   }
00070 
00071   if ((magic[0] != 'p') || (magic[1] != 'l') || (magic[2] != 'y'))
00072   {
00073     if (error_callback_)
00074       error_callback_ (line_number_, "parse error");
00075     return false;
00076   }
00077 
00078   if (magic_callback_)
00079     magic_callback_ ();
00080 
00081   while (std::getline (istream, line))
00082   {
00083     ++line_number_;
00084     std::istringstream stringstream (line);
00085     stringstream.unsetf (std::ios_base::skipws);
00086 
00087     stringstream >> std::ws;
00088     if (stringstream.eof ())
00089     {
00090       if (warning_callback_)
00091         warning_callback_ (line_number_, "ignoring line '" + line + "'");
00092     }
00093     else
00094     {
00095       std::string keyword;
00096       stringstream >> keyword;
00097 
00098       // format
00099       if (keyword == "format")
00100       {
00101         std::string format_string, version;
00102         char space_format_format_string, space_format_string_version;
00103         stringstream >> space_format_format_string >> std::ws >> format_string >> space_format_string_version >> std::ws >> version >> std::ws;
00104         if (!stringstream || 
00105             !stringstream.eof () || 
00106             !isspace (space_format_format_string) || 
00107             !isspace (space_format_string_version))
00108         {
00109           if (error_callback_)
00110             error_callback_ (line_number_, "parse error");
00111           return false;
00112         }
00113         if (format_string == "ascii")
00114         {
00115           format = ascii_format;
00116         }
00117         else if (format_string == "binary_big_endian")
00118         {
00119           format = binary_big_endian_format;
00120         }
00121         else if (format_string == "binary_little_endian")
00122         {
00123           format = binary_little_endian_format;
00124         }
00125         else
00126         {
00127           if (error_callback_)
00128           {
00129             error_callback_ (line_number_, "parse error");
00130           }
00131           return false;
00132         }
00133         if (version != "1.0")
00134         {
00135           if (error_callback_)
00136           {
00137             error_callback_ (line_number_, "version '" + version + "' is not supported");
00138           }
00139           return false;
00140         }
00141         if (number_of_format_statements > 0)
00142         {
00143           if (error_callback_)
00144           {
00145             error_callback_ (line_number_, "parse error");
00146           }
00147           return false;
00148         }
00149         ++number_of_format_statements;
00150         if (format_callback_)
00151         {
00152           format_callback_ (format, version);
00153         }
00154       }
00155       // element
00156       else if (keyword == "element")
00157       {
00158         std::string name;
00159         std::size_t count;
00160         char space_element_name, space_name_count;
00161         stringstream >> space_element_name >> std::ws >> name >> space_name_count >> std::ws >> count >> std::ws;
00162         if (!stringstream || 
00163             !stringstream.eof () || 
00164             !isspace (space_element_name) || 
00165             !isspace (space_name_count))
00166         {
00167           if (error_callback_)
00168           {
00169             error_callback_ (line_number_, "parse error");
00170           }
00171           return false;
00172         }
00173         std::vector< boost::shared_ptr<element> >::const_iterator iterator;
00174         for (iterator = elements.begin (); iterator != elements.end (); ++iterator)
00175         {
00176           const struct element& element = *(iterator->get ());
00177           if (element.name == name)
00178           {
00179             break;
00180           }
00181         }
00182         if (iterator != elements.end ())
00183         {
00184           if (error_callback_)
00185           {
00186             error_callback_ (line_number_, "parse error");
00187           }
00188           return false;
00189         }
00190         ++number_of_element_statements;
00191         element_callbacks_type element_callbacks;
00192         if (element_definition_callbacks_)
00193         {
00194           element_callbacks = element_definition_callbacks_ (name, count);
00195         }
00196         boost::shared_ptr<element> element_ptr (new element (name, 
00197                                                                 count, 
00198                                                                 boost::get<0>(element_callbacks), 
00199                                                                 boost::get<1>(element_callbacks)));
00200         elements.push_back (boost::shared_ptr<element>(element_ptr));
00201         current_element_ = element_ptr.get ();
00202       }
00203 
00204       // property
00205       else if (keyword == "property")
00206       {
00207         std::string type_or_list;
00208         char space_property_type_or_list;
00209         stringstream >> space_property_type_or_list >> std::ws >> type_or_list;
00210         if (!stringstream || !isspace (space_property_type_or_list))
00211         {
00212           if (error_callback_)
00213           {
00214             error_callback_ (line_number_, "parse error");
00215           }
00216           return false;
00217         }
00218         if (type_or_list != "list") {
00219           std::string name;
00220           std::string& type = type_or_list;
00221           char space_type_name;
00222           stringstream >> space_type_name >> std::ws >> name >> std::ws;
00223           if (!stringstream || !isspace (space_type_name))
00224           {
00225             if (error_callback_)
00226             {
00227               error_callback_ (line_number_, "parse error");
00228             }
00229             return false;
00230           }
00231           if (number_of_element_statements == 0)
00232           {
00233             if (error_callback_)
00234             {
00235               error_callback_ (line_number_, "parse error");
00236             }
00237             return false;
00238           }
00239           std::vector< boost::shared_ptr<property> >::const_iterator iterator;
00240           for (iterator = current_element_->properties.begin (); 
00241                iterator != current_element_->properties.end (); 
00242                ++iterator)
00243           {
00244             const struct property& property = *(iterator->get ());
00245             if (property.name == name)
00246               break;
00247           }
00248           if (iterator != current_element_->properties.end ())
00249           {
00250             if (error_callback_)
00251               error_callback_ (line_number_, "parse error");
00252             return false;
00253           }
00254           if ((type == type_traits<int8>::name ()) || (type == type_traits<int8>::old_name ()))
00255           {
00256             parse_scalar_property_definition<int8>(name);
00257           }
00258           else if ((type == type_traits<int16>::name ()) || (type == type_traits<int16>::old_name ()))
00259           {
00260             parse_scalar_property_definition<int16>(name);
00261           }
00262           else if ((type == type_traits<int32>::name ()) || (type == type_traits<int32>::old_name ()))
00263           {
00264             parse_scalar_property_definition<int32>(name);
00265           }
00266           else if ((type == type_traits<uint8>::name ()) || (type == type_traits<uint8>::old_name ()))
00267           {
00268             parse_scalar_property_definition<uint8>(name);
00269           }
00270           else if ((type == type_traits<uint16>::name ()) || (type == type_traits<uint16>::old_name ()))
00271           {
00272             parse_scalar_property_definition<uint16>(name);
00273           }
00274           else if ((type == type_traits<uint32>::name ()) || (type == type_traits<uint32>::old_name ()))
00275           {
00276             parse_scalar_property_definition<uint32>(name);
00277           }
00278           else if ((type == type_traits<float32>::name ()) || (type == type_traits<float32>::old_name ()))
00279           {
00280             parse_scalar_property_definition<float32>(name);
00281           }
00282           else if ((type == type_traits<float64>::name ()) || (type == type_traits<float64>::old_name ()))
00283           {
00284             parse_scalar_property_definition<float64>(name);
00285           }
00286           else
00287           {
00288             if (error_callback_)
00289             {
00290               error_callback_ (line_number_, "parse error");
00291             }
00292             return false;
00293           }
00294           ++number_of_property_statements;
00295         }
00296         else
00297         {
00298           std::string name;
00299           std::string size_type_string, scalar_type_string;
00300           char space_list_size_type, space_size_type_scalar_type, space_scalar_type_name;
00301           stringstream >> space_list_size_type >> std::ws >> size_type_string >> space_size_type_scalar_type >> std::ws >> scalar_type_string >> space_scalar_type_name >> std::ws >> name >> std::ws;
00302           if (!stringstream || 
00303               !isspace (space_list_size_type) || 
00304               !isspace (space_size_type_scalar_type) || 
00305               !isspace (space_scalar_type_name))
00306           {
00307             if (error_callback_)
00308               error_callback_ (line_number_, "parse error");
00309             return false;
00310           }
00311           if (number_of_element_statements == 0)
00312           {
00313             if (error_callback_)
00314               error_callback_ (line_number_, "parse error");
00315             return false;
00316           }
00317           std::vector< boost::shared_ptr<property> >::const_iterator iterator;
00318           for (iterator = current_element_->properties.begin (); 
00319                iterator != current_element_->properties.end (); 
00320                ++iterator) 
00321           {
00322             const struct property& property = *(iterator->get ());
00323             if (property.name == name)
00324               break;
00325           }
00326           if (iterator != current_element_->properties.end ())
00327           {
00328             if (error_callback_)
00329               error_callback_ (line_number_, "parse error");
00330             return false;
00331           }
00332           if ((size_type_string == type_traits<uint8>::name ()) || (size_type_string == type_traits<uint8>::old_name ()))
00333           {
00334             typedef uint8 size_type;
00335             if ((scalar_type_string == type_traits<int8>::name ()) || (scalar_type_string == type_traits<int8>::old_name ()))
00336             {
00337               parse_list_property_definition<size_type, int8>(name);
00338             }
00339             else if ((scalar_type_string == type_traits<int16>::name ()) || (scalar_type_string == type_traits<int16>::old_name ()))
00340             {
00341               parse_list_property_definition<size_type, int16>(name);
00342             }
00343             else if ((scalar_type_string == type_traits<int32>::name ()) || (scalar_type_string == type_traits<int32>::old_name ()))
00344             {
00345               parse_list_property_definition<size_type, int32>(name);
00346             }
00347             else if ((scalar_type_string == type_traits<uint8>::name ()) || (scalar_type_string == type_traits<uint8>::old_name ()))
00348             {
00349               parse_list_property_definition<size_type, uint8>(name);
00350             }
00351             else if ((scalar_type_string == type_traits<uint16>::name ()) || (scalar_type_string == type_traits<uint16>::old_name ()))
00352             {
00353               parse_list_property_definition<size_type, uint16>(name);
00354             }
00355             else if ((scalar_type_string == type_traits<uint32>::name ()) || (scalar_type_string == type_traits<uint32>::old_name ()))
00356             {
00357               parse_list_property_definition<size_type, uint32>(name);
00358             }
00359             else if ((scalar_type_string == type_traits<float32>::name ()) || (scalar_type_string == type_traits<float32>::old_name ()))
00360             {
00361               parse_list_property_definition<size_type, float32>(name);
00362             }
00363             else if ((scalar_type_string == type_traits<float64>::name ()) || (scalar_type_string == type_traits<float64>::old_name ()))
00364             {
00365               parse_list_property_definition<size_type, float64>(name);
00366             }
00367             else
00368             {
00369               if (error_callback_)
00370               {
00371                 error_callback_ (line_number_, "parse error");
00372               }
00373               return false;
00374             }
00375           }
00376           else if ((size_type_string == type_traits<uint16>::name ()) || (size_type_string == type_traits<uint16>::old_name ()))
00377           {
00378             typedef uint16 size_type;
00379             if ((scalar_type_string == type_traits<int8>::name ()) || (scalar_type_string == type_traits<int8>::old_name ()))
00380             {
00381               parse_list_property_definition<size_type, int8>(name);
00382             }
00383             else if ((scalar_type_string == type_traits<int16>::name ()) || (scalar_type_string == type_traits<int16>::old_name ()))
00384             {
00385               parse_list_property_definition<size_type, int16>(name);
00386             }
00387             else if ((scalar_type_string == type_traits<int32>::name ()) || (scalar_type_string == type_traits<int32>::old_name ()))
00388             {
00389               parse_list_property_definition<size_type, int32>(name);
00390             }
00391             else if ((scalar_type_string == type_traits<uint8>::name ()) || (scalar_type_string == type_traits<uint8>::old_name ()))
00392             {
00393               parse_list_property_definition<size_type, uint8>(name);
00394             }
00395             else if ((scalar_type_string == type_traits<uint16>::name ()) || (scalar_type_string == type_traits<uint16>::old_name ()))
00396             {
00397               parse_list_property_definition<size_type, uint16>(name);
00398             }
00399             else if ((scalar_type_string == type_traits<uint32>::name ()) || (scalar_type_string == type_traits<uint32>::old_name ()))
00400             {
00401               parse_list_property_definition<size_type, uint32>(name);
00402             }
00403             else if ((scalar_type_string == type_traits<float32>::name ()) || (scalar_type_string == type_traits<float32>::old_name ()))
00404             {
00405               parse_list_property_definition<size_type, float32>(name);
00406             }
00407             else if ((scalar_type_string == type_traits<float64>::name ()) || (scalar_type_string == type_traits<float64>::old_name ()))
00408             {
00409               parse_list_property_definition<size_type, float64>(name);
00410             }
00411             else
00412             {
00413               if (error_callback_)
00414                 error_callback_ (line_number_, "parse error");
00415               return false;
00416             }
00417           }
00418           else if ((size_type_string == type_traits<uint32>::name ()) || (size_type_string == type_traits<uint32>::old_name ()))
00419           {
00420             typedef uint32 size_type;
00421             if ((scalar_type_string == type_traits<int8>::name ()) || (scalar_type_string == type_traits<int8>::old_name ()))
00422             {
00423               parse_list_property_definition<size_type, int8>(name);
00424             }
00425             else if ((scalar_type_string == type_traits<int16>::name ()) || (scalar_type_string == type_traits<int16>::old_name ()))
00426             {
00427               parse_list_property_definition<size_type, int16>(name);
00428             }
00429             else if ((scalar_type_string == type_traits<int32>::name ()) || (scalar_type_string == type_traits<int32>::old_name ()))
00430             {
00431               parse_list_property_definition<size_type, int32>(name);
00432             }
00433             else if ((scalar_type_string == type_traits<uint8>::name ()) || (scalar_type_string == type_traits<uint8>::old_name ()))
00434             {
00435               parse_list_property_definition<size_type, uint8>(name);
00436             }
00437             else if ((scalar_type_string == type_traits<uint16>::name ()) || (scalar_type_string == type_traits<uint16>::old_name ()))
00438             {
00439               parse_list_property_definition<size_type, uint16>(name);
00440             }
00441             else if ((scalar_type_string == type_traits<uint32>::name ()) || (scalar_type_string == type_traits<uint32>::old_name ()))
00442             {
00443               parse_list_property_definition<size_type, uint32>(name);
00444             }
00445             else if ((scalar_type_string == type_traits<float32>::name ()) || (scalar_type_string == type_traits<float32>::old_name ()))
00446             {
00447               parse_list_property_definition<size_type, float32>(name);
00448             }
00449             else if ((scalar_type_string == type_traits<float64>::name ()) || (scalar_type_string == type_traits<float64>::old_name ()))
00450             {
00451               parse_list_property_definition<size_type, float64>(name);
00452             }
00453             else
00454             {
00455               if (error_callback_)
00456                 error_callback_ (line_number_, "parse error");
00457               return false;
00458             }
00459           }
00460           else
00461           {
00462             if (error_callback_)
00463               error_callback_ (line_number_, "parse error");
00464             return false;
00465           }
00466           ++number_of_property_statements;
00467         }
00468       }
00469 
00470       // comment
00471       else if (keyword == "comment")
00472       {
00473         if (comment_callback_)
00474           comment_callback_ (line);
00475         ++number_of_comment_statements;
00476       }
00477 
00478       // obj_info
00479       else if (keyword == "obj_info")
00480       {
00481         if (obj_info_callback_)
00482           obj_info_callback_ (line);
00483         ++number_of_obj_info_statements;
00484       }
00485 
00486       // end_header
00487       else if (keyword == "end_header")
00488       {
00489         if (end_header_callback_)
00490         {
00491           if (end_header_callback_ () == false)
00492             return true;
00493         }
00494         break;
00495       }
00496       // unknown keyword
00497       else
00498       {
00499         if (warning_callback_)
00500           warning_callback_ (line_number_, "ignoring line '" + line + "'");
00501       }
00502     }
00503   }
00504 
00505   if (number_of_format_statements == 0)
00506   {
00507     if (error_callback_) 
00508      error_callback_ (line_number_, "parse error");
00509     return false;
00510   }
00511 
00512   // ascii
00513   if (format == ascii_format)
00514   {
00515     for (std::vector< boost::shared_ptr<element> >::const_iterator element_iterator = elements.begin (); 
00516          element_iterator != elements.end (); 
00517          ++element_iterator)
00518     {
00519       struct element& element = *(element_iterator->get ());
00520       for (std::size_t element_index = 0; element_index < element.count; ++element_index)
00521       {
00522         if (element.begin_element_callback) 
00523           element.begin_element_callback ();
00524         if (!std::getline (istream, line))
00525         {
00526           if (error_callback_)
00527             error_callback_ (line_number_, "parse error");
00528           return false;
00529         }
00530         ++line_number_;
00531         std::istringstream stringstream (line);
00532         stringstream.unsetf (std::ios_base::skipws);
00533         stringstream >> std::ws;
00534         for (std::vector< boost::shared_ptr<property> >::const_iterator property_iterator = element.properties.begin (); 
00535              property_iterator != element.properties.end (); 
00536              ++property_iterator)
00537         {
00538           struct property& property = *(property_iterator->get ());
00539           if (property.parse (*this, format, stringstream) == false)
00540             return false;
00541         }
00542         if (!stringstream.eof ())
00543         {
00544           if (error_callback_)
00545             error_callback_ (line_number_, "parse error");
00546           return false;
00547         }
00548         if (element.end_element_callback)
00549           element.end_element_callback ();
00550       }
00551     }
00552     istream >> std::ws;
00553     if (istream.fail () || !istream.eof () || istream.bad ())
00554     {
00555       if (error_callback_)
00556         error_callback_ (line_number_, "parse error");
00557       return false;
00558     }
00559     return true;
00560   }
00561 
00562   // binary
00563   else
00564   {
00565     std::streampos data_start = istream.tellg ();
00566     istream.close ();
00567     istream.open (filename.c_str (), std::ios::in | std::ios::binary);
00568     istream.seekg (data_start);
00569 
00570     for (std::vector< boost::shared_ptr<element> >::const_iterator element_iterator = elements.begin (); 
00571          element_iterator != elements.end (); 
00572          ++element_iterator)
00573     {
00574       struct element& element = *(element_iterator->get ());
00575       for (std::size_t element_index = 0; element_index < element.count; ++element_index)
00576       {
00577         if (element.begin_element_callback) {
00578           element.begin_element_callback ();
00579         }
00580         for (std::vector< boost::shared_ptr<property> >::const_iterator property_iterator = element.properties.begin (); 
00581              property_iterator != element.properties.end (); 
00582              ++property_iterator)
00583         {
00584           struct property& property = *(property_iterator->get ());
00585           if (property.parse (*this, format, istream) == false)
00586           {
00587             return false;
00588           }
00589         }
00590         if (element.end_element_callback)
00591         {
00592           element.end_element_callback ();
00593         }
00594       }
00595     }
00596     if (istream.fail () || (istream.rdbuf ()->sgetc () != std::char_traits<char>::eof ()) || istream.bad ())
00597     {
00598       if (error_callback_)
00599       {
00600         error_callback_ (line_number_, "parse error");
00601       }
00602       return false;
00603     }
00604     return true;
00605   }
00606 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:32