Point Cloud Library (PCL)  1.3.1
ply.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: ply.h 3008 2011-10-31 23:50:29Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_IO_PLY_H_
00039 #define PCL_IO_PLY_H_
00040 
00041 #include "pcl/point_types.h"
00042 #include "pcl/common/io.h"
00043 #include <functional>
00044 #include <algorithm>
00045 #include <boost/algorithm/string.hpp>
00046 #include <fstream>
00047 
00048 namespace pcl
00049 {
00050   namespace io
00051   {
00052     namespace ply
00053     {
00055       enum Format
00056       {
00057         ASCII_FORMAT = 0,
00058         BIG_ENDIAN_FORMAT = 1,
00059         LITTLE_ENDIAN_FORMAT = 2
00060       };
00061 
00062 #if (defined(__powerpc) || defined(__powerpc__) || defined(__POWERPC__) || defined(__ppc__) || defined(_M_PPC) || defined(__ARCH_PPC))
00063 #  define PCL_BIG_ENDIAN
00064 #elif (defined(i386) || defined(__i386__) || defined(__i386) || defined(_M_IX86) || defined(_X86_) || defined(__THW_INTEL__) || defined(__I86__) || defined(__INTEL__)) \
00065   || (defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(_M_X64)) \
00066   || (defined(__ANDROID__))
00067 #  define PCL_LITTLE_ENDIAN
00068 #else
00069 #  error
00070 #endif
00071 
00072       Format getEndianess()
00073       {
00074 #ifdef PCL_LITTLE_ENDIAN
00075         return LITTLE_ENDIAN_FORMAT;
00076 #elif defined PCL_BIG_ENDIAN
00077         return BIG_ENDIAN_FORMAT;
00078 #else
00079 #error
00080 #endif
00081       }
00082 #undef PCL_BIG_ENDIAN
00083 #undef PCL_LITTLE_ENDIAN
00084       
00086       enum Flags
00087       {
00088         NONE              = 0x0000000,
00089         //vertex properties
00090         VERTEX_XYZ        = 0x0000001,
00091         VERTEX_NORMAL     = 0x0000002,
00092         VERTEX_COLOR      = 0x0000004,
00093         VERTEX_INTENSITY  = 0x0000008,
00094         VERTEX_NORNAL     = 0x0000010,
00095         VERTEX_RADIUS     = 0x0000020,
00096         VERTEX_CONFIDENCE = 0x0000040,
00097         VERTEX_VIEWPOINT  = 0x0000080,
00098         VERTEX_RANGE      = 0x0000100,
00099         VERTEX_STRENGTH   = 0x0000200,
00100         VERTEX_XY         = 0x0000400,
00101         //face properties
00102         FACE_VERTICES     = 0x0010000,
00103         //camera properties
00104         CAMERA            = 0x8000000,
00105         //all properties are set
00106         ALL               = 0xFFFFFFF
00107       };
00109       int getTypeFromTypeName(const std::string& type_name) 
00110       {
00111         if(!strcmp(type_name.c_str(), "char"))
00112           return sensor_msgs::PointField::INT8;
00113         if(!strcmp(type_name.c_str(), "uchar"))
00114           return sensor_msgs::PointField::UINT8;
00115         if(!strcmp(type_name.c_str(), "short"))
00116           return sensor_msgs::PointField::INT16;
00117         if(!strcmp(type_name.c_str(), "ushort"))
00118           return sensor_msgs::PointField::UINT16;
00119         if(!strcmp(type_name.c_str(), "int"))
00120           return sensor_msgs::PointField::INT32;
00121         if(!strcmp(type_name.c_str(), "uint"))
00122           return sensor_msgs::PointField::UINT32;
00123         if(!strcmp(type_name.c_str(), "float"))
00124           return sensor_msgs::PointField::FLOAT32;
00125         if(!strcmp(type_name.c_str(), "double"))
00126           return sensor_msgs::PointField::FLOAT64;
00127         if(!strcmp(type_name.c_str(), "list"))
00128           return -1;
00129         return -2;
00130       };
00131       
00132       size_t getMaximumCapacity(int size_type)
00133       {
00134         switch(size_type)
00135         {
00136         case sensor_msgs::PointField::UINT8 : 
00137           return std::numeric_limits<unsigned char>::max();
00138         case sensor_msgs::PointField::UINT16 : 
00139           return std::numeric_limits<unsigned short>::max();
00140         case sensor_msgs::PointField::UINT32 : 
00141           return std::numeric_limits<unsigned int>::max();
00142         default:
00143           return 0;
00144         }
00145       };
00146 
00147       const size_t CAMERA_SIZE = sizeof(float) * 19 + sizeof(int) * 2;
00148 
00149       struct property
00150       {
00151         std::string name_;
00152         int data_type_;
00153         size_t offset_;
00154       property(const std::string& name) : name_(name), offset_(0) {}
00155       property(const std::string& name, int data_type) : 
00156         name_(name), data_type_(data_type)
00157         {
00158           offset_ = pcl::getFieldSize(data_type_);
00159         }
00160       };
00161 
00162       struct list_property : public property
00163       {
00164         int size_type_;
00165       list_property(const std::string& name, int size_type, int data_type) : 
00166         property(name, data_type), size_type_(size_type) 
00167         {
00168           offset_ = pcl::getFieldSize(size_type_) + 
00169             getMaximumCapacity(size_type_) * pcl::getFieldSize(data_type_);
00170         }
00171         
00172         void set_size(const void* size)
00173         {
00174           offset_ = pcl::getFieldSize(size_type_);
00175           switch(size_type_)
00176           {
00177           case sensor_msgs::PointField::UINT8 : 
00178           {
00179             const unsigned char *size_; size_ = (unsigned char*) size;
00180             offset_ += (*size_) * pcl::getFieldSize(size_type_);
00181           }
00182           break;
00183           case sensor_msgs::PointField::UINT16 : 
00184           {
00185             const unsigned short *size_; size_ = (unsigned short*) size;
00186             offset_ += (*size_) * pcl::getFieldSize(size_type_);
00187           }
00188           break;
00189           case sensor_msgs::PointField::UINT32 : 
00190           {
00191             const unsigned int *size_; size_ = (unsigned int*) size;
00192             offset_ += (*size_) * pcl::getFieldSize(size_type_);
00193           }
00194           break;
00195           }
00196         }
00197       };
00198 
00199       class element
00200       {
00201       public:
00202         std::string name_;
00203         size_t count_;
00204         size_t offset_;
00205       element(const std::string& name, size_t count) : 
00206         name_(name), count_(count), offset_(0), properties_(0), list_properties_(0) {}
00207 
00208         typedef std::vector<property*>::iterator iterator;
00209         typedef std::vector<property*>::const_iterator const_iterator;
00210         typedef std::vector<iterator>::iterator iterator_iterator;
00211         typedef std::vector<iterator>::const_iterator const_iterator_iterator;
00212         size_t properties_size() { return properties_.size(); }
00213         property* operator[](const std::string &prop_name)
00214         {
00215           std::vector<property*>::iterator properties_it = properties_.begin();
00216           for(; properties_it != properties_.end(); ++properties_it)
00217             if((*properties_it)->name_ == prop_name) break;
00218           if (properties_it == properties_.end())
00219             return NULL;
00220           else
00221             return *properties_it;
00222         }
00223           
00224         const property* operator[](const std::string &prop_name) const 
00225         {
00226           std::vector<property*>::const_iterator properties_it = properties_.begin ();
00227           for(; properties_it != properties_.end (); ++properties_it)
00228             if((*properties_it)->name_ == prop_name) break;
00229           if (properties_it == properties_.end ())
00230             return NULL;
00231           else
00232             return *properties_it;
00233         }
00234 
00235         int push_property(const std::string& name, int data_type)
00236         {
00237           property* p = new property (name, data_type);
00238           properties_.push_back (p);
00239           offset_+= p->offset_;
00240           return int (properties_.size ());
00241         }
00242           
00243         int push_property(const std::string& name, int size_type, int data_type)
00244         {
00245           property *lp = new list_property (name, size_type, data_type);
00246           properties_.push_back (lp);
00247           list_properties_.push_back (properties_.end() - 1);
00248           offset_+= lp->offset_;
00249           return int (properties_.size ());
00250         }
00251 
00252         bool has_list_properties() const { return (!list_properties_.empty()); }
00253           
00254         size_t offset_before(const std::string& prop_name)
00255         {
00256           size_t offset = 0;
00257           std::vector<property*>::const_iterator properties_it = properties_.begin();
00258           for(; properties_it != properties_.end(); ++properties_it)
00259             if((*properties_it)->name_ == prop_name) 
00260               break;
00261             else
00262               offset+= (*properties_it)->offset_;
00263           if (properties_it == properties_.end())
00264             return -1;
00265           return offset;
00266         }
00267 
00268         void update_offset() 
00269         {
00270           offset_ = 0;
00271           std::vector<property*>::const_iterator properties_it = properties_.begin();
00272           for(; properties_it != properties_.end(); ++properties_it)
00273             offset_+= (*properties_it)->offset_;
00274         }
00275 
00276         bool is_list_property(const_iterator property_pos)
00277         {
00278           return std::find_if(list_properties_.begin(),
00279                               list_properties_.end(),
00280                               std::bind1st(std::equal_to<const_iterator>(),property_pos)) != list_properties_.end();
00281         }
00282 
00283         std::vector<property*> properties_;
00284         std::vector<iterator> list_properties_;
00285       };
00286 
00287       class parser
00288       {
00289       public:
00290       parser() : elements_(0), last_element_(0) {}
00291 
00292         typedef std::vector<element*>::iterator iterator;
00293         typedef std::vector<element*>::const_iterator const_iterator;
00294         iterator begin() { return elements_.begin(); }
00295         iterator end() { return elements_.end(); }
00296 
00297         element* operator[](const std::string &element_name)
00298         {
00299           std::vector<element*>::iterator elements_it = elements_.begin();
00300           for(; elements_it != elements_.end(); ++elements_it)
00301             if((*elements_it)->name_ == element_name) break;
00302           if (elements_it == elements_.end())
00303             return NULL;
00304           else
00305             return *elements_it;
00306         }
00307 
00308         const element* operator[](const std::string &element_name) const
00309         {
00310           std::vector<element*>::const_iterator elements_it = elements_.begin();
00311           for(; elements_it != elements_.end(); ++elements_it)
00312             if((*elements_it)->name_ == element_name) break;
00313           if (elements_it == elements_.end())
00314             return NULL;
00315           else
00316             return *elements_it;
00317         }
00318           
00319         int push_element(const std::string& name, size_t count)
00320         {
00321           last_element_ = new element(name, count);
00322           elements_.push_back(last_element_);
00323           return int (elements_.size());
00324         }
00325 
00326         int push_property(const std::string& name, int data_type)
00327         {
00328           return last_element_->push_property(name, data_type);
00329         }
00330 
00331         int push_property(const std::string& name, int size_type, int data_type)
00332         {
00333           return last_element_->push_property(name, size_type, data_type);
00334         }
00335 
00336         size_t offset_before(const std::string& element_name)
00337         {
00338           size_t offset = 0;
00339           std::vector<element*>::const_iterator elements_it = elements_.begin();
00340           for(; elements_it != elements_.end(); ++elements_it)
00341             if((*elements_it)->name_ == element_name) 
00342               break;
00343             else
00344               offset+= (*elements_it)->offset_ * (*elements_it)->count_;
00345           if (elements_it == this->end())
00346             return -1;
00347           return offset;
00348         }
00349 
00350 
00351         int parse_header(const std::string& file_name, int& data_type, int& data_idx, bool& is_swap_required)
00352         {
00353           std::ifstream fs;
00354           std::string line;
00355 
00356           // Open file in binary mode to avoid problem of 
00357           // std::getline() corrupting the result of ifstream::tellg()
00358           fs.open (file_name.c_str (), std::ios::binary);
00359           if (!fs.is_open () || fs.fail ())
00360           {
00361             PCL_ERROR ("[pcl::io::ply::parser::parse_header] Could not open file %s.\n", file_name.c_str ());
00362             return (-1);
00363           }
00364 
00365           std::vector<std::string> st;
00366           // Read the header and fill it in with wonderful values
00367           try
00368           {
00369             getline (fs, line);
00370             boost::trim (line);
00371             boost::split (st, line, boost::is_any_of ( std::string ("\t\r ")), boost::token_compress_on);
00372 
00373             // PLY file always start with magic line "ply"
00374             if (st.at (0) !=  "ply")
00375             {
00376               PCL_ERROR ("[pcl::io::ply::parser::parse_header] %s is not a valid ply file\n", st[0].c_str());
00377               return(-1);
00378             }
00379 
00380             while (!fs.eof ())
00381             {
00382               getline (fs, line);
00383               // Ignore empty lines
00384               if (line == "")
00385                 continue;
00386 
00387               // Tokenize the line
00388               boost::trim (line);
00389               boost::split (st, line, boost::is_any_of (std::string ( "\t\r ")), boost::token_compress_on);
00390 
00391               std::string line_type = st.at (0);
00392 
00393               // read format
00394               if (line_type.substr (0, 6) == "format")
00395               {
00396                 float version =  atof(st.at(2).c_str());
00397                 //check version number
00398                 if (version != 1.0)
00399                 {
00400                   PCL_ERROR ("[pcl::io::ply::parser::parse_header] can't handle this PLY format version %f\n", version);
00401                   return (-1);
00402                 }
00403                 //check format
00404                 if ("ascii" == st.at (1))
00405                   data_type = 0;
00406                 else
00407                 {
00408                   if ("binary_big_endian" == st.at (1) || "binary_little_endian" == st.at (1))
00409                   {
00410                     data_type = 1;
00411                     pcl::io::ply::Format format = pcl::io::ply::getEndianess();
00412                     if ((("binary_big_endian" == st.at(1)) && 
00413                          (format == pcl::io::ply::LITTLE_ENDIAN_FORMAT)) ||
00414                         (("binary_little_endian" == st.at(1)) && 
00415                          (format == pcl::io::ply::BIG_ENDIAN_FORMAT)))
00416                       is_swap_required = true;
00417                   }
00418                   else
00419                   {
00420                     PCL_ERROR ("[pcl::io::ply::parser::parse_header] unknown format %f\n", st[1].c_str());
00421                     return (-1);
00422                   }
00423                 }
00424                 continue;
00425               }
00426               // ignore comments
00427               if (line_type.substr (0, 7) == "comment")
00428                 continue;
00429               // read element
00430               if (line_type.substr (0, 7) == "element") 
00431               {
00432                 if(st.size() == 3)
00433                   push_element(st.at(1), atoi(st.at(2).c_str()));
00434                 else
00435                   push_element(st.at(1), 1);
00436                 continue;
00437               }
00438               // read property
00439               if (line_type.substr (0, 8) == "property")
00440               {
00441                 // list property
00442                 if(st.at(1) == "list")
00443                 {
00444                   int size_type = pcl::io::ply::getTypeFromTypeName(st.at(2));
00445                   int data_type = pcl::io::ply::getTypeFromTypeName(st.at(3));
00446                   if(data_type < -1 || size_type < -1)
00447                   {
00448                     PCL_ERROR ("[pcl::io::ply::parser::parse_header] parse error property list %s %s %s.\n", st[2].c_str(), st[3].c_str (), st[4].c_str ());
00449                     return -1;
00450                   }
00451                   else
00452                   {
00453                     size_t capacity = pcl::io::ply::getMaximumCapacity(size_type);
00454                     if(capacity == 0)
00455                     {
00456                       PCL_ERROR ("[pcl::io::ply::parser::parse_header] unhandled size type for property list %s %s %s.\n", st[2].c_str(), st[3].c_str (), st[4].c_str ());
00457                       return -1;
00458                     }
00459                     push_property(st.at(4), size_type, data_type);
00460                   }
00461                 }
00462                 // scalar property
00463                 else
00464                 {
00465                   int type = pcl::io::ply::getTypeFromTypeName(st.at(1));
00466                   if(type < -1)
00467                   {
00468                     PCL_ERROR ("[pcl::io::ply::parser::parse_header] parse error property %s %s.\n", st[1].c_str (), st[2].c_str ());
00469                     return -1;
00470                   }
00471                   else
00472                     push_property (st.at(2), type);
00473                 }
00474                 continue;
00475               }
00476               // end of header
00477               if (line_type.substr (0, 10) == "end_header") 
00478                 data_idx = fs.tellg ();
00479               break;
00480             }
00481           }
00482           catch (const char *exception)
00483           {
00484             PCL_ERROR ("[pcl::io::ply::parser::parse_header] %s\n", exception);
00485             return (-1);
00486           }
00487           
00488           fs.close();
00489           return (0);
00490         }
00491 
00492       private:
00493         std::vector<element*> elements_;
00494         element* last_element_;
00495       };
00496 
00498       struct camera
00499       {
00500         float view_px; float view_py; float view_pz;
00501         float x_axisx; float x_axisy; float x_axisz;
00502         float y_axisx; float y_axisy; float y_axisz;
00503         float z_axisx; float z_axisy; float z_axisz;
00504         float focal; float scalex; float scaley; float centerx; float centery;
00505         int viewportx; int viewporty; float k1; float k2;
00511       camera() :
00512         view_px(0), view_py(0), view_pz(0),
00513           x_axisx(0), x_axisy(0), x_axisz(0),
00514           y_axisx(0), y_axisy(0), y_axisz(0),
00515           z_axisx(0), z_axisy(0), z_axisz(0),
00516           focal(0), scalex(0), centerx(0), centery(0), viewportx(0), viewporty(0),
00517           k1(0), k2(0) {}
00518 
00519       camera(const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) :
00520         view_px(origin[0]), view_py(origin[1]), view_pz(origin[2]),
00521           focal(0), scalex(0), centerx(0), centery(0), viewportx(0), viewporty(0),
00522           k1(0), k2(0)
00523         {
00524           Eigen::Matrix3f R = orientation.toRotationMatrix ();
00525           x_axisx = R(0,0); x_axisy = R(0,1); x_axisz = R(0,2);
00526           y_axisx = R(1,0); y_axisy = R(1,1); y_axisz = R(1,2);
00527           z_axisx = R(2,0); z_axisy = R(2,1); z_axisz = R(2,2);
00528         }
00529         
00531         void ext_to_eigen(Eigen::Vector4f &origin, Eigen::Quaternionf &orientation)
00532         {
00533           origin[0] = view_px; origin[1] = view_py;  origin[2] = view_pz; origin[3] = 1.0;
00534           Eigen::Matrix3f R;
00535           R << x_axisx, x_axisy, x_axisz,
00536             y_axisx, y_axisy, y_axisz,
00537             z_axisx, z_axisy, z_axisz;
00538           orientation = Eigen::Quaternionf(R);
00539         }
00540       };
00541       
00543       void write(const pcl::io::ply::camera& c, std::ostream& out, bool binary)
00544       {
00545         if(!binary)
00546         {
00547           out << c.view_px << " " << c.view_py << " " << c.view_pz << " ";
00548           out << c.x_axisx << " " << c.x_axisy << " " << c.x_axisz << " ";
00549           out << c.y_axisx << " " << c.y_axisy << " " << c.y_axisz << " ";
00550           out << c.z_axisx << " " << c.z_axisy << " " << c.z_axisz << " ";
00551           out << c.focal << " ";
00552           out << c.scalex << " " << c.scaley << " ";
00553           out << c.centerx << " " << c.centery << " ";
00554           out << c.viewportx << " " << c.viewporty << " ";
00555           out << c.k1 << " " << c.k2;
00556         }
00557         else {
00558           out.write ((const char*) &c, sizeof(camera));
00559         }
00560       };
00561     } //namespace ply
00562   } //namespace io
00563 }//namespace pcl
00564 
00565 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines