Point Cloud Library (PCL)
1.3.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 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