|
Point Cloud Library (PCL)
1.5.1
|
|
Point Cloud Library (PCL)
1.5.1
|
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 4702 2012-02-23 09:39:33Z gedikli $ 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 inline 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 inline 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 inline 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 struct property 00148 { 00149 std::string name_; 00150 int data_type_; 00151 size_t offset_; 00152 property(const std::string& name) : name_(name), offset_(0) {} 00153 property(const std::string& name, int data_type) 00154 : name_(name), data_type_(data_type) 00155 { 00156 offset_ = pcl::getFieldSize(data_type_); 00157 } 00158 }; 00159 00160 struct list_property : public property 00161 { 00162 int size_type_; 00163 list_property(const std::string& name, int size_type, int data_type) 00164 : property(name, data_type), size_type_(size_type) 00165 { 00166 offset_ = pcl::getFieldSize(size_type_) + 00167 getMaximumCapacity(size_type_) * pcl::getFieldSize(data_type_); 00168 } 00169 00170 void set_size(const void* size) 00171 { 00172 offset_ = pcl::getFieldSize(size_type_); 00173 switch(size_type_) 00174 { 00175 case sensor_msgs::PointField::UINT8 : 00176 { 00177 const unsigned char *size_; size_ = (unsigned char*) size; 00178 offset_ += (*size_) * pcl::getFieldSize(size_type_); 00179 } 00180 break; 00181 case sensor_msgs::PointField::UINT16 : 00182 { 00183 const unsigned short *size_; size_ = (unsigned short*) size; 00184 offset_ += (*size_) * pcl::getFieldSize(size_type_); 00185 } 00186 break; 00187 case sensor_msgs::PointField::UINT32 : 00188 { 00189 const unsigned int *size_; size_ = (unsigned int*) size; 00190 offset_ += (*size_) * pcl::getFieldSize(size_type_); 00191 } 00192 break; 00193 } 00194 } 00195 }; 00196 00197 class element 00198 { 00199 public: 00200 std::string name_; 00201 size_t count_; 00202 size_t offset_; 00203 element(const std::string& name, size_t count) 00204 : name_(name) 00205 , count_(count) 00206 , offset_(0) 00207 , properties_(0) 00208 , list_properties_(0) 00209 {} 00210 00211 typedef std::vector<property*>::iterator iterator; 00212 typedef std::vector<property*>::const_iterator const_iterator; 00213 typedef std::vector<iterator>::iterator iterator_iterator; 00214 typedef std::vector<iterator>::const_iterator const_iterator_iterator; 00215 size_t properties_size() { return properties_.size(); } 00216 property* operator[](const std::string &prop_name) 00217 { 00218 std::vector<property*>::iterator properties_it = properties_.begin(); 00219 for(; properties_it != properties_.end(); ++properties_it) 00220 if((*properties_it)->name_ == prop_name) break; 00221 if (properties_it == properties_.end()) 00222 return NULL; 00223 else 00224 return *properties_it; 00225 } 00226 00227 const property* operator[](const std::string &prop_name) const 00228 { 00229 std::vector<property*>::const_iterator properties_it = properties_.begin (); 00230 for(; properties_it != properties_.end (); ++properties_it) 00231 if((*properties_it)->name_ == prop_name) break; 00232 if (properties_it == properties_.end ()) 00233 return NULL; 00234 else 00235 return *properties_it; 00236 } 00237 00238 int push_property(const std::string& name, int data_type) 00239 { 00240 property* p = new property (name, data_type); 00241 properties_.push_back (p); 00242 offset_+= p->offset_; 00243 return int (properties_.size ()); 00244 } 00245 00246 int push_property(const std::string& name, int size_type, int data_type) 00247 { 00248 property *lp = new list_property (name, size_type, data_type); 00249 properties_.push_back (lp); 00250 list_properties_.push_back (properties_.end() - 1); 00251 offset_+= lp->offset_; 00252 return int (properties_.size ()); 00253 } 00254 00255 bool has_list_properties() const { return (!list_properties_.empty()); } 00256 00257 size_t offset_before(const std::string& prop_name) 00258 { 00259 size_t offset = 0; 00260 std::vector<property*>::const_iterator properties_it = properties_.begin(); 00261 for(; properties_it != properties_.end(); ++properties_it) 00262 if((*properties_it)->name_ == prop_name) 00263 break; 00264 else 00265 offset+= (*properties_it)->offset_; 00266 if (properties_it == properties_.end()) 00267 return -1; 00268 return offset; 00269 } 00270 00271 void update_offset() 00272 { 00273 offset_ = 0; 00274 std::vector<property*>::const_iterator properties_it = properties_.begin(); 00275 for(; properties_it != properties_.end(); ++properties_it) 00276 offset_+= (*properties_it)->offset_; 00277 } 00278 00279 bool is_list_property(const_iterator property_pos) 00280 { 00281 return std::find_if(list_properties_.begin(), 00282 list_properties_.end(), 00283 std::bind1st(std::equal_to<const_iterator>(),property_pos)) != list_properties_.end(); 00284 } 00285 std::vector<property*> properties_; 00286 std::vector<iterator> list_properties_; 00287 }; 00288 00289 struct obj_info 00290 { 00291 std::string name_; 00292 union 00293 { 00294 float float_data_; 00295 int int_data_; 00296 }; 00297 obj_info (std::string name, float data) 00298 : name_(name), float_data_(data) {} 00299 obj_info (std::string name, int data) 00300 : name_(name), int_data_(data) {} 00301 }; 00302 00303 class parser 00304 { 00305 public: 00306 parser() : elements_(0), last_element_(0), infos_(0) {} 00307 00308 typedef std::vector<element*>::iterator iterator; 00309 typedef std::vector<element*>::const_iterator const_iterator; 00310 iterator begin() { return elements_.begin(); } 00311 iterator end() { return elements_.end(); } 00312 00313 element* operator[](const std::string &element_name) 00314 { 00315 std::vector<element*>::iterator elements_it = elements_.begin(); 00316 for(; elements_it != elements_.end(); ++elements_it) 00317 if((*elements_it)->name_ == element_name) break; 00318 if (elements_it == elements_.end()) 00319 return NULL; 00320 else 00321 return *elements_it; 00322 } 00323 00324 const element* operator[](const std::string &element_name) const 00325 { 00326 std::vector<element*>::const_iterator elements_it = elements_.begin(); 00327 for(; elements_it != elements_.end(); ++elements_it) 00328 if((*elements_it)->name_ == element_name) break; 00329 if (elements_it == elements_.end()) 00330 return NULL; 00331 else 00332 return *elements_it; 00333 } 00334 00335 int push_element(const std::string& name, size_t count) 00336 { 00337 last_element_ = new element(name, count); 00338 elements_.push_back(last_element_); 00339 return int (elements_.size()); 00340 } 00341 00342 int push_property(const std::string& name, int data_type) 00343 { 00344 return last_element_->push_property(name, data_type); 00345 } 00346 00347 int push_property(const std::string& name, int size_type, int data_type) 00348 { 00349 return last_element_->push_property(name, size_type, data_type); 00350 } 00351 00352 void push_obj_info(const std::string& name, const std::string& data) 00353 { 00354 if (data.find ('.') != std::string::npos || 00355 data.find ('e') != std::string::npos || 00356 data.find ('E') != std::string::npos || 00357 data.find ('f') != std::string::npos) 00358 infos_.push_back (new obj_info (name, float (atof (data.c_str ())))); 00359 else 00360 infos_.push_back (new obj_info (name, atoi (data.c_str ()))); 00361 } 00362 00363 obj_info* 00364 get_obj_info(const std::string& name) const 00365 { 00366 std::vector <obj_info*>::const_iterator info = infos_.begin (); 00367 for ( ;info!=infos_.end (); ++info) 00368 if ( (*info)->name_ == name ) break; 00369 return *info; 00370 } 00371 00372 size_t offset_before(const std::string& element_name) 00373 { 00374 size_t offset = 0; 00375 std::vector<element*>::const_iterator elements_it = elements_.begin(); 00376 for(; elements_it != elements_.end(); ++elements_it) 00377 if((*elements_it)->name_ == element_name) 00378 break; 00379 else 00380 offset+= (*elements_it)->offset_ * (*elements_it)->count_; 00381 if (elements_it == this->end()) 00382 return -1; 00383 return offset; 00384 } 00385 00386 00387 int parse_header(const std::string& file_name, int& data_type, int& data_idx, bool& is_swap_required) 00388 { 00389 std::ifstream fs; 00390 std::string line; 00391 00392 // Open file in binary mode to avoid problem of 00393 // std::getline() corrupting the result of ifstream::tellg() 00394 fs.open (file_name.c_str (), std::ios::binary); 00395 if (!fs.is_open () || fs.fail ()) 00396 { 00397 PCL_ERROR ("[pcl::io::ply::parser::parse_header] Could not open file %s.\n", file_name.c_str ()); 00398 return (-1); 00399 } 00400 00401 std::vector<std::string> st; 00402 // Read the header and fill it in with wonderful values 00403 try 00404 { 00405 getline (fs, line); 00406 boost::trim (line); 00407 boost::split (st, line, boost::is_any_of ( std::string ("\t\r ")), boost::token_compress_on); 00408 00409 // PLY file always start with magic line "ply" 00410 if (st.at (0) != "ply") 00411 { 00412 PCL_ERROR ("[pcl::io::ply::parser::parse_header] %s is not a valid ply file\n", st[0].c_str()); 00413 return(-1); 00414 } 00415 00416 while (!fs.eof ()) 00417 { 00418 getline (fs, line); 00419 // Ignore empty lines 00420 if (line == "") 00421 continue; 00422 00423 // Tokenize the line 00424 boost::trim (line); 00425 boost::split (st, line, boost::is_any_of (std::string ( "\t\r ")), boost::token_compress_on); 00426 00427 std::string line_type = st.at (0); 00428 00429 // read format 00430 if (line_type.substr (0, 6) == "format") 00431 { 00432 float version = atof(st.at(2).c_str()); 00433 //check version number 00434 if (version != 1.0) 00435 { 00436 PCL_ERROR ("[pcl::io::ply::parser::parse_header] can't handle this PLY format version %f\n", version); 00437 return (-1); 00438 } 00439 //check format 00440 if ("ascii" == st.at (1)) 00441 data_type = 0; 00442 else 00443 { 00444 if ("binary_big_endian" == st.at (1) || "binary_little_endian" == st.at (1)) 00445 { 00446 data_type = 1; 00447 pcl::io::ply::Format format = pcl::io::ply::getEndianess(); 00448 if ((("binary_big_endian" == st.at(1)) && 00449 (format == pcl::io::ply::LITTLE_ENDIAN_FORMAT)) || 00450 (("binary_little_endian" == st.at(1)) && 00451 (format == pcl::io::ply::BIG_ENDIAN_FORMAT))) 00452 is_swap_required = true; 00453 } 00454 else 00455 { 00456 PCL_ERROR ("[pcl::io::ply::parser::parse_header] unknown format %f\n", st[1].c_str()); 00457 return (-1); 00458 } 00459 } 00460 continue; 00461 } 00462 // ignore comments 00463 if (line_type.substr (0, 7) == "comment") 00464 continue; 00465 // read obj_info 00466 if (line_type.substr (0, 8) == "obj_info") 00467 { 00468 if(st.size() == 3) 00469 push_obj_info(st.at(1), st.at(2)); 00470 else 00471 PCL_ERROR ("[pcl::io::ply::parser::parse_header] parse error obj_info %s\n", 00472 st.at(2).c_str ()); 00473 continue; 00474 } 00475 // read element 00476 if (line_type.substr (0, 7) == "element") 00477 { 00478 if(st.size() == 3) 00479 push_element(st.at(1), atoi(st.at(2).c_str())); 00480 else 00481 push_element(st.at(1), 1); 00482 continue; 00483 } 00484 // read property 00485 if (line_type.substr (0, 8) == "property") 00486 { 00487 // list property 00488 if(st.at(1) == "list") 00489 { 00490 int size_type = pcl::io::ply::getTypeFromTypeName(st.at(2)); 00491 int data_type = pcl::io::ply::getTypeFromTypeName(st.at(3)); 00492 if(data_type < -1 || size_type < -1) 00493 { 00494 PCL_ERROR ("[pcl::io::ply::parser::parse_header] parse error property list %s %s %s.\n", 00495 st[2].c_str(), st[3].c_str (), st[4].c_str ()); 00496 return -1; 00497 } 00498 else 00499 { 00500 size_t capacity = pcl::io::ply::getMaximumCapacity(size_type); 00501 if(capacity == 0) 00502 { 00503 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 ()); 00504 return -1; 00505 } 00506 push_property(st.at(4), size_type, data_type); 00507 } 00508 } 00509 // scalar property 00510 else 00511 { 00512 int type = pcl::io::ply::getTypeFromTypeName(st.at(1)); 00513 if(type < -1) 00514 { 00515 PCL_ERROR ("[pcl::io::ply::parser::parse_header] parse error property %s %s.\n", st[1].c_str (), st[2].c_str ()); 00516 return -1; 00517 } 00518 else 00519 push_property (st.at(2), type); 00520 } 00521 continue; 00522 } 00523 // end of header 00524 if (line_type.substr (0, 10) == "end_header") 00525 data_idx = fs.tellg (); 00526 break; 00527 } 00528 } 00529 catch (const char *exception) 00530 { 00531 PCL_ERROR ("[pcl::io::ply::parser::parse_header] %s\n", exception); 00532 return (-1); 00533 } 00534 00535 fs.close(); 00536 return (0); 00537 } 00538 00539 private: 00540 std::vector<element*> elements_; 00541 element* last_element_; 00542 std::vector <obj_info*> infos_; 00543 }; 00544 00546 struct camera 00547 { 00548 float view_px; float view_py; float view_pz; 00549 float x_axisx; float x_axisy; float x_axisz; 00550 float y_axisx; float y_axisy; float y_axisz; 00551 float z_axisx; float z_axisy; float z_axisz; 00552 float focal; float scalex; float scaley; float centerx; float centery; 00553 int viewportx; int viewporty; float k1; float k2; 00554 00558 camera () : 00559 view_px(0), view_py(0), view_pz(0), 00560 x_axisx(0), x_axisy(0), x_axisz(0), 00561 y_axisx(0), y_axisy(0), y_axisz(0), 00562 z_axisx(0), z_axisy(0), z_axisz(0), 00563 focal(0), scalex(0), centerx(0), centery(0), viewportx(0), viewporty(0), 00564 k1(0), k2(0) {} 00565 00571 camera(const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) : 00572 view_px(origin[0]), view_py(origin[1]), view_pz(origin[2]), 00573 focal(0), scalex(0), centerx(0), centery(0), viewportx(0), viewporty(0), 00574 k1(0), k2(0) 00575 { 00576 Eigen::Matrix3f R = orientation.toRotationMatrix (); 00577 x_axisx = R(0,0); x_axisy = R(0,1); x_axisz = R(0,2); 00578 y_axisx = R(1,0); y_axisy = R(1,1); y_axisz = R(1,2); 00579 z_axisx = R(2,0); z_axisy = R(2,1); z_axisz = R(2,2); 00580 } 00581 00583 void 00584 ext_to_eigen (Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) 00585 { 00586 origin[0] = view_px; origin[1] = view_py; origin[2] = view_pz; origin[3] = 1.0; 00587 Eigen::Matrix3f R; 00588 R << x_axisx, x_axisy, x_axisz, 00589 y_axisx, y_axisy, y_axisz, 00590 z_axisx, z_axisy, z_axisz; 00591 orientation = Eigen::Quaternionf(R); 00592 } 00593 }; 00594 00596 inline void write(const pcl::io::ply::camera& c, std::ostream& out, bool binary) 00597 { 00598 if(!binary) 00599 { 00600 out << c.view_px << " " << c.view_py << " " << c.view_pz << " "; 00601 out << c.x_axisx << " " << c.x_axisy << " " << c.x_axisz << " "; 00602 out << c.y_axisx << " " << c.y_axisy << " " << c.y_axisz << " "; 00603 out << c.z_axisx << " " << c.z_axisy << " " << c.z_axisz << " "; 00604 out << c.focal << " "; 00605 out << c.scalex << " " << c.scaley << " "; 00606 out << c.centerx << " " << c.centery << " "; 00607 out << c.viewportx << " " << c.viewporty << " "; 00608 out << c.k1 << " " << c.k2; 00609 } 00610 else { 00611 out.write ((const char*) &c, sizeof(camera)); 00612 } 00613 }; 00614 00615 struct range_grid 00616 { 00617 std::vector<int> indices_; 00618 void resize (const std::string&, size_t size) 00619 { 00620 indices_.resize (size); 00621 } 00622 }; 00623 } //namespace ply 00624 } //namespace io 00625 }//namespace pcl 00626 00627 #endif
1.8.0
