rtcMesh3D.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2007
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: Mesh3D.cpp
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 02/29/2008
00015  * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
00016  * changed by .: $Author:benjaminpitzer $
00017  * revision ...: $Revision:141 $
00018  */
00019 
00020 //== INCLUDES ==================================================================
00021 #include <map>
00022 #include <assert.h>
00023 #include "rtc/rtcVertex3D.h"
00024 #include "rtc/rtcMesh3D.h"
00025 
00026 //== NAMESPACES ================================================================
00027 namespace rtc {
00028 
00029 Mesh3D::Mesh3D()
00030 :IOObject(), pose(), flags(0)
00031 {
00032   flags|=MESH_FLAG_HAS_VERTEX_NORMAL;
00033   flags|=MESH_FLAG_HAS_FACE_NORMAL;
00034   flags|=MESH_FLAG_HAS_VERTEX_COLOR;
00035   flags|=MESH_FLAG_HAS_VERTEX_FLAG;
00036   flags|=MESH_FLAG_HAS_FACE_FLAG;
00037   flags|=MESH_FLAG_HAS_IMAGE_POSE;
00038   flags|=MESH_FLAG_HAS_CAMERA_INFO;
00039   flags|=MESH_FLAG_HAS_FRAME_ID;
00040 }
00041 
00042 Mesh3D::~Mesh3D()
00043 {
00044   clear();
00045 }
00046 
00047 Mesh3D::Mesh3D(const Mesh3D& other)
00048 : IOObject()
00049 {
00050   // copy properties
00051   *this=other;
00052 }
00053 
00054 void Mesh3D::clear()
00055 {
00056   for(unsigned int i=0;i<vertices.size();++i) {
00057     Vertex3D* v = vertices[i];
00058     delete v;
00059   }
00060   vertices.clear();
00061 
00062   for(unsigned int i=0;i<faces.size();++i) {
00063     Face3D* f = faces[i];
00064     delete f;
00065   }
00066   faces.clear();
00067 }
00068 
00069 const Mesh3D &Mesh3D::operator=(const Mesh3D& other)
00070 {
00071   set(other);
00072   return *this;
00073 }
00074 
00075 // create mesh from scan
00076 void Mesh3D::set(const Mesh3D& other)
00077 {
00078   // clean up first
00079   clear();
00080 
00081   // copy properties
00082   pose = other.pose;
00083   flags = other.flags;
00084 
00085   // copy vertices
00086   VecVertex3D::const_iterator itv,itv_end;
00087   for (itv=other.vertices.begin(),itv_end=other.vertices.end(); itv!=itv_end; ++itv){
00088     Vertex3D* vertex = new Vertex3D(*(*itv));
00089     vertex->mesh = this;
00090     vertices.push_back(vertex);
00091   }
00092 
00093   // copy faces
00094   VecFace3D::const_iterator itf,itf_end;
00095   for (itf=other.faces.begin(),itf_end=other.faces.end(); itf!=itf_end; ++itf){
00096     Face3D* face = new Face3D(*(*itf));
00097     face->mesh = this;
00098     faces.push_back(face);
00099   }
00100 
00101   //texture mapping
00102   #ifndef NO_GRAPHICS
00103   texture = other.texture;
00104   #endif
00105   teximage = other.teximage;
00106   texmask = other.texmask;
00107   image_pose = other.image_pose;
00108   camera_info = other.camera_info;
00109   frame_id = other.frame_id;
00110 }
00111 
00112 void Mesh3D::update() {
00113   updateNormals();
00114 }
00115 
00116 void Mesh3D::updateNormals()
00117 {
00118   // update face normals
00119   updateFaces();
00120   // update vertex normals
00121   updateVertices();
00122 }
00123 
00124 int Mesh3D::numVertices() const
00125 {
00126   return (int)vertices.size();
00127 }
00128 
00129 void Mesh3D::updateVertices() {
00130   for (unsigned int i=0;i<vertices.size();++i) {
00131     vertices[i]->update();
00132   }
00133 }
00134 
00135 Vertex3D* Mesh3D::addVertex(Vertex3D* vertex)
00136 {
00137   assert(vertex);
00138   vertices.push_back(vertex);
00139   return vertex;
00140 }
00141 
00142 Vertex3D* Mesh3D::addVertex(const Point3Df& p)
00143 {
00144   Vertex3D* vertex = new Vertex3D(this,p);
00145   return addVertex(vertex);
00146 }
00147 
00148 int Mesh3D::numFaces() const
00149 {
00150   return (int)faces.size();
00151 }
00152 
00153 void Mesh3D::updateFaces() {
00154   for (unsigned int i=0;i<faces.size();++i) {
00155     faces[i]->update();
00156   }
00157 }
00158 
00159 Face3D* Mesh3D::addFace(Face3D* face)
00160 {
00161   faces.push_back(face);
00162   return face;
00163 }
00164 
00165 Face3D* Mesh3D::addFace(int v0, int v1, int v2)
00166 {
00167   assert(v0>=0&&v0<(int)vertices.size());
00168   assert(v1>=0&&v1<(int)vertices.size());
00169   assert(v2>=0&&v2<(int)vertices.size());
00170   Face3D* face = new Face3D(this,v0,v1,v2);
00171   int index = faces.size();
00172   vertices[v0]->faces.push_back(index);
00173   vertices[v1]->faces.push_back(index);
00174   vertices[v2]->faces.push_back(index);
00175   return addFace(face);
00176 }
00177 
00178 Face3D* Mesh3D::addFace(int vhandles[3])
00179 {
00180   return addFace(vhandles[0],vhandles[1],vhandles[2]);
00181 }
00182 
00183 // flags
00184 bool Mesh3D::hasVertexNormals() const
00185 {
00186   return (flags&MESH_FLAG_HAS_VERTEX_NORMAL);
00187 }
00188 
00189 bool Mesh3D::hasFaceNormals() const
00190 {
00191   return (flags&MESH_FLAG_HAS_FACE_NORMAL);
00192 }
00193 
00194 bool Mesh3D::hasVertexColors() const
00195 {
00196   return (flags&MESH_FLAG_HAS_VERTEX_COLOR);
00197 }
00198 
00199 bool Mesh3D::hasFaceColors() const
00200 {
00201   return (flags&MESH_FLAG_HAS_FACE_COLOR);
00202 }
00203 
00204 bool Mesh3D::hasVertexFlags() const
00205 {
00206   return (flags&MESH_FLAG_HAS_VERTEX_FLAG);
00207 }
00208 
00209 bool Mesh3D::hasFaceFlags() const
00210 {
00211   return (flags&MESH_FLAG_HAS_FACE_FLAG);
00212 }
00213 
00214 bool Mesh3D::hasTexture() const
00215 {
00216   return (flags&MESH_FLAG_HAS_TEXTURE);
00217 }
00218 
00219 bool Mesh3D::hasImagePose() const
00220 {
00221   return (flags&MESH_FLAG_HAS_IMAGE_POSE);
00222 }
00223 
00224 bool Mesh3D::hasCameraInfo() const
00225 {
00226   return (flags&MESH_FLAG_HAS_CAMERA_INFO);
00227 }
00228 
00229 bool Mesh3D::hasFrameID() const
00230 {
00231   return (flags&MESH_FLAG_HAS_FRAME_ID);
00232 }
00233 
00234 bool Mesh3D::getFlag(int flag) const
00235 {
00236   return (flags&flag);
00237 }
00238 
00239 void Mesh3D::setFlag(int flag, bool value)
00240 {
00241   if(value)
00242     flags|=flag;
00243   else
00244     flags&=flag;
00245 }
00246 
00247 bool Mesh3D::write(OutputHandler &oh) const
00248 {
00249   // get other properties
00250   int num_vertices = (int)vertices.size();
00251   int num_faces = (int)faces.size();
00252 
00253   // write pose
00254   rtc_write(oh,pose);
00255   rtc_write(oh,"flags",flags);
00256 
00257   // write vertices
00258   rtc_write(oh,"num_vertices",num_vertices);
00259   Vec3f vertex,normal;
00260   Vec3uc color;
00261   VecVertex3D::const_iterator it,it_end;
00262   for (it=vertices.begin(),it_end=vertices.end(); it!=it_end; ++it) {
00263     Vertex3D* vertex = *it;
00264     vertex->write(oh);
00265   }
00266 
00267   // write faces
00268   rtc_write(oh,"num_faces",num_faces);
00269   VecFace3D::const_iterator itf,itf_end;
00270   for (itf=faces.begin(),itf_end=faces.end(); itf!=itf_end; ++itf){
00271     Face3D* face = *itf;
00272     face->write(oh);
00273   }
00274 
00275   // write texture
00276   if(hasTexture()) teximage.write(oh);
00277   if(getFlag(MESH_FLAG_HAS_TEXTUREMASK)) texmask.write(oh);
00278 
00279 
00280   // write image pose
00281   if(hasImagePose()) {
00282     rtc_write(oh,image_pose);
00283   }
00284 
00285   // write camera info
00286   if(hasCameraInfo()) {
00287     // header
00288     rtc_write(oh,"seq",camera_info.header.seq);
00289     rtc_write(oh,"sec",camera_info.header.stamp.sec);
00290     rtc_write(oh,"nsec",camera_info.header.stamp.nsec);
00291     rtc_write(oh,"frame_id",camera_info.header.frame_id);
00292 
00293     rtc_write(oh,"height",camera_info.height);
00294     rtc_write(oh,"width",camera_info.width);
00295 
00296     rtc_write(oh,"distortion_model",camera_info.distortion_model);
00297     size_t size = camera_info.D.size();
00298     rtc_write(oh,"D_size",size);
00299     for (size_t i = 0; i < size; ++i) rtc_write(oh,"D",camera_info.D[i]);
00300 
00301     for (size_t i = 0; i < camera_info.K.size(); ++i) rtc_write(oh,"K",camera_info.K[i]);
00302     // for (size_t i = 0; i < camera_info.K.size(); ++i) rtc_write(oh,"K",camera_info.K[i]);
00303     for (size_t i = 0; i < camera_info.R.size(); ++i) rtc_write(oh,"R",camera_info.R[i]);
00304     for (size_t i = 0; i < camera_info.P.size(); ++i) rtc_write(oh,"P",camera_info.P[i]);
00305 
00306     rtc_write(oh,"binning_x",camera_info.binning_x);
00307     rtc_write(oh,"binning_y",camera_info.binning_y);
00308 
00309     // ROI
00310     rtc_write(oh,"x_offset",camera_info.roi.x_offset);
00311     rtc_write(oh,"y_offset",camera_info.roi.y_offset);
00312     rtc_write(oh,"height",camera_info.roi.height);
00313     rtc_write(oh,"width",camera_info.roi.width);
00314     rtc_write(oh,"do_rectify",camera_info.roi.do_rectify);
00315   }
00316 
00317   // write frame id
00318   if(hasFrameID()) {
00319     rtc_write(oh,"frame_id",frame_id);
00320   }
00321 
00322   return oh.good();
00323 }
00324 
00325 bool Mesh3D::read(InputHandler &ih)
00326 {
00327   int num_vertices,num_faces;
00328 
00329   // start reading
00330   rtc_read(ih,pose);
00331   rtc_read(ih,"flags",flags);
00332 
00333   // read mesh vertices
00334   rtc_read(ih,"num_vertices",num_vertices);
00335   Vec3f vertex,normal;
00336   Vec3uc color;
00337   for(int i=0;i<num_vertices;++i) {
00338     Vertex3D* v = new Vertex3D(this);
00339     v->read(ih);
00340     vertices.push_back(v);
00341     if(!ih.good())
00342       throw Exception("format error: unexpected end of file");
00343   }
00344 
00345   // read mesh faces
00346   rtc_read(ih,"num_faces",num_faces);
00347   for(int i=0;i<num_faces;++i) {
00348     Face3D* face = new Face3D(this,0,0,0);
00349     face->read(ih);
00350     int index = faces.size();
00351     if(face->v[0]>=0) {
00352       Vertex3D* v0 = vertices[face->v(0)];
00353       v0->faces.push_back(index);
00354     }
00355     else
00356       return false;
00357 
00358     if(face->v[1]>=0) {
00359       Vertex3D* v1 = vertices[face->v(1)];
00360       v1->faces.push_back(index);
00361     }
00362     else
00363       return false;
00364 
00365     if(face->v[2]>=0) {
00366       Vertex3D* v2 = vertices[face->v(2)];
00367       v2->faces.push_back(index);
00368     }
00369     else
00370       return false;
00371 
00372     faces.push_back(face);
00373     if(!ih.good())
00374       throw Exception("format error: unexpected end of file");
00375   }
00376 
00377   // read texture
00378   if(hasTexture()) {
00379     teximage.read(ih);
00380   }
00381 
00382   if (getFlag(MESH_FLAG_HAS_TEXTUREMASK))
00383     texmask.read(ih);
00384 
00385   // read image pose
00386   if(hasImagePose()) {
00387     rtc_read(ih,image_pose);
00388   }
00389 
00390   // read camera info
00391   if(hasCameraInfo()) {
00392     // header
00393     rtc_read(ih,"seq",camera_info.header.seq);
00394     rtc_read(ih,"sec",camera_info.header.stamp.sec);
00395     rtc_read(ih,"nsec",camera_info.header.stamp.nsec);
00396     rtc_read(ih,"frame_id",camera_info.header.frame_id);;
00397 
00398     rtc_read(ih,"height",camera_info.height);
00399     rtc_read(ih,"width",camera_info.width);
00400 
00401     rtc_read(ih,"distortion_model",camera_info.distortion_model);
00402     size_t size = camera_info.D.size();
00403     rtc_read(ih,"D_size",size);
00404     camera_info.D.resize(size);
00405     for (size_t i = 0; i < size; ++i) rtc_read(ih,"D",camera_info.D[i]);
00406 
00407     for (size_t i = 0; i < camera_info.K.size(); ++i) rtc_read(ih,"K",camera_info.K[i]);
00408     for (size_t i = 0; i < camera_info.R.size(); ++i) rtc_read(ih,"R",camera_info.R[i]);
00409     for (size_t i = 0; i < camera_info.P.size(); ++i) rtc_read(ih,"P",camera_info.P[i]);
00410     rtc_read(ih,"binning_x",camera_info.binning_x);
00411     rtc_read(ih,"binning_y",camera_info.binning_y);
00412 
00413     // ROI
00414     rtc_read(ih,"x_offset",camera_info.roi.x_offset);
00415     rtc_read(ih,"y_offset",camera_info.roi.y_offset);
00416     rtc_read(ih,"height",camera_info.roi.height);
00417     rtc_read(ih,"width",camera_info.roi.width);
00418     rtc_read(ih,"do_rectify",camera_info.roi.do_rectify);
00419   }
00420 
00421   // read frame id
00422   if(hasFrameID()) {
00423     rtc_read(ih,"frame_id",frame_id);;
00424   }
00425 
00426   return ih.good();
00427 }
00428 
00429 void Mesh3D::loadTexture()
00430 {
00431 #ifndef NO_GRAPHICS
00432   texture.fromImage(teximage);
00433 #endif
00434 }
00435 
00436 void Mesh3D::unloadTexture()
00437 {
00438 #ifndef NO_GRAPHICS
00439   texture.unloadTexture();
00440 #endif
00441 }
00442 
00443 void Mesh3D::write( FILE *fp ) const
00444 {
00445   // get other properties
00446   int num_vertices = (int)vertices.size();
00447   int num_faces = (int)faces.size();
00448 
00449   // write pose
00450   fwrite(&pose,sizeof(pose),1,fp);
00451   fwrite(&flags,sizeof(flags),1,fp);
00452 
00453   // write vertices
00454   fwrite(&num_vertices,sizeof(num_vertices),1,fp);
00455   Vec3f vertex,normal;
00456   Vec3uc color;
00457   VecVertex3D::const_iterator it,it_end;
00458   for (it=vertices.begin(),it_end=vertices.end(); it!=it_end; ++it) {
00459     Vertex3D* vertex = *it;
00460     vertex->write(fp);
00461   }
00462 
00463   // write faces
00464   fwrite(&num_faces,sizeof(num_faces),1,fp);
00465   VecFace3D::const_iterator itf,itf_end;
00466   for (itf=faces.begin(),itf_end=faces.end(); itf!=itf_end; ++itf){
00467     Face3D* face = *itf;
00468     face->write(fp);
00469   }
00470 
00471   // write texture
00472   if(hasTexture()) teximage.write(fp);
00473   if(getFlag(MESH_FLAG_HAS_TEXTUREMASK)) 
00474   {
00475     int nrow=texmask.rows(),ncol=texmask.columns();
00476     fwrite(&nrow,sizeof(nrow),1,fp);
00477     fwrite(&ncol,sizeof(ncol),1,fp);
00478     fwrite(texmask.x,sizeof(int)*2,nrow*ncol,fp);
00479   }
00480 
00481   // read image pose
00482   if(hasImagePose()) {
00483     fwrite(&image_pose,sizeof(image_pose),1,fp);
00484   }
00485 
00486   // read camera info
00487   if(hasCameraInfo()) {
00488     // header
00489     fwrite(&camera_info.header.seq,sizeof(camera_info.header.seq),1,fp);
00490     fwrite(&camera_info.header.stamp.sec,sizeof(camera_info.header.stamp.sec),1,fp);
00491     fwrite(&camera_info.header.stamp.nsec,sizeof(camera_info.header.stamp.nsec),1,fp);
00492     fwrite(&camera_info.header.frame_id,sizeof(camera_info.header.frame_id),1,fp);
00493 
00494     fwrite(&camera_info.height,sizeof(camera_info.height),1,fp);
00495     fwrite(&camera_info.width,sizeof(camera_info.width),1,fp);
00496     for (size_t i = 0; i < camera_info.K.size(); ++i) fwrite(&camera_info.K[i],sizeof(camera_info.K[i]),1,fp);
00497     for (size_t i = 0; i < camera_info.R.size(); ++i) fwrite(&camera_info.R[i],sizeof(camera_info.R[i]),1,fp);
00498     for (size_t i = 0; i < camera_info.P.size(); ++i) fwrite(&camera_info.P[i],sizeof(camera_info.P[i]),1,fp);
00499     fwrite(&camera_info.binning_x,sizeof(camera_info.binning_x),1,fp);
00500     fwrite(&camera_info.binning_y,sizeof(camera_info.binning_y),1,fp);
00501 
00502     // ROI
00503     fwrite(&camera_info.roi.x_offset,sizeof(camera_info.roi.x_offset),1,fp);
00504     fwrite(&camera_info.roi.y_offset,sizeof(camera_info.roi.y_offset),1,fp);
00505     fwrite(&camera_info.roi.height,sizeof(camera_info.roi.height),1,fp);
00506     fwrite(&camera_info.roi.width,sizeof(camera_info.roi.width),1,fp);
00507     fwrite(&camera_info.roi.do_rectify,sizeof(camera_info.roi.do_rectify),1,fp);
00508   }
00509 
00510   return;
00511 }
00512 
00513 void Mesh3D::read(FILE *fp)
00514 {
00515   size_t res = 0;
00516   int num_vertices,num_faces;
00517 
00518   // start reading
00519   res+=fread(&pose,sizeof(pose),1,fp);
00520   res+=fread(&flags,sizeof(flags),1,fp);
00521 
00522   // read mesh vertices
00523   res+=fread(&num_vertices,sizeof(num_vertices),1,fp);
00524   Vec3f vertex,normal;
00525   Vec3uc color;
00526   for(int i=0;i<num_vertices;++i) {
00527     Vertex3D* v = new Vertex3D(this);
00528     v->read(fp);
00529     vertices.push_back(v);
00530     if(feof(fp))
00531       throw Exception("format error: unexpected end of file");
00532   }
00533 
00534   // read mesh faces
00535   res+=fread(&num_faces,sizeof(num_faces),1,fp);
00536   for(int i=0;i<num_faces;++i) {
00537     Face3D* face = new Face3D(this,0,0,0);
00538     face->read(fp);
00539     int index = faces.size();
00540     if(face->v[0]>=0) {
00541       Vertex3D* v0 = vertices[face->v(0)];
00542       v0->faces.push_back(index);
00543     }
00544     else
00545       return ;
00546 
00547     if(face->v[1]>=0) {
00548       Vertex3D* v1 = vertices[face->v(1)];
00549       v1->faces.push_back(index);
00550     }
00551     else
00552       return ;
00553     if(face->v[2]>=0) {
00554       Vertex3D* v2 = vertices[face->v(2)];
00555       v2->faces.push_back(index);
00556     }
00557     else
00558       return ;
00559 
00560     faces.push_back(face);
00561     if(feof(fp))
00562       throw Exception("format error: unexpected end of file");
00563   }
00564 
00565   // write texture
00566   if(hasTexture()) {
00567     teximage.read(fp);
00568   }
00569 
00570   if (getFlag(MESH_FLAG_HAS_TEXTUREMASK))
00571   {
00572     int nrow,ncol;
00573     res+=fread(&nrow,sizeof(nrow),1,fp);
00574     res+=fread(&ncol,sizeof(ncol),1,fp);
00575     texmask.setSize(nrow,ncol);
00576     res+=fread(texmask.x,sizeof(int)*2,nrow*ncol,fp);
00577   }
00578 
00579   // read image pose
00580   if(hasImagePose()) {
00581     res+=fread(&image_pose,sizeof(image_pose),1,fp);
00582   }
00583 
00584   // read camera info
00585   if(hasCameraInfo()) {
00586     // header
00587     res+=fread(&camera_info.header.seq,sizeof(camera_info.header.seq),1,fp);
00588     res+=fread(&camera_info.header.stamp.sec,sizeof(camera_info.header.stamp.sec),1,fp);
00589     res+=fread(&camera_info.header.stamp.nsec,sizeof(camera_info.header.stamp.nsec),1,fp);
00590     res+=fread(&camera_info.header.frame_id,sizeof(camera_info.header.frame_id),1,fp);
00591 
00592     res+=fread(&camera_info.height,sizeof(camera_info.height),1,fp);
00593     res+=fread(&camera_info.width,sizeof(camera_info.width),1,fp);
00594     for (size_t i = 0; i < camera_info.K.size(); ++i) res+=fread(&camera_info.K[i],sizeof(camera_info.K[i]),1,fp);
00595     for (size_t i = 0; i < camera_info.R.size(); ++i) res+=fread(&camera_info.R[i],sizeof(camera_info.R[i]),1,fp);
00596     for (size_t i = 0; i < camera_info.P.size(); ++i) res+=fread(&camera_info.P[i],sizeof(camera_info.P[i]),1,fp);
00597     res+=fread(&camera_info.binning_x,sizeof(camera_info.binning_x),1,fp);
00598     res+=fread(&camera_info.binning_y,sizeof(camera_info.binning_y),1,fp);
00599 
00600     // ROI
00601     res+=fread(&camera_info.roi.x_offset,sizeof(camera_info.roi.x_offset),1,fp);
00602     res+=fread(&camera_info.roi.y_offset,sizeof(camera_info.roi.y_offset),1,fp);
00603     res+=fread(&camera_info.roi.height,sizeof(camera_info.roi.height),1,fp);
00604     res+=fread(&camera_info.roi.width,sizeof(camera_info.roi.width),1,fp);
00605     res+=fread(&camera_info.roi.do_rectify,sizeof(camera_info.roi.do_rectify),1,fp);
00606   }
00607 
00608 }
00609 
00610 
00611 
00612 //==============================================================================
00613 } // NAMESPACE rtc
00614 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Mon Oct 6 2014 10:07:34