00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <map>
00022 #include <assert.h>
00023 #include "rtc/rtcVertex3D.h"
00024 #include "rtc/rtcMesh3D.h"
00025
00026
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
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
00076 void Mesh3D::set(const Mesh3D& other)
00077 {
00078
00079 clear();
00080
00081
00082 pose = other.pose;
00083 flags = other.flags;
00084
00085
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
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
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
00119 updateFaces();
00120
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
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
00250 int num_vertices = (int)vertices.size();
00251 int num_faces = (int)faces.size();
00252
00253
00254 rtc_write(oh,pose);
00255 rtc_write(oh,"flags",flags);
00256
00257
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
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
00276 if(hasTexture()) teximage.write(oh);
00277 if(getFlag(MESH_FLAG_HAS_TEXTUREMASK)) texmask.write(oh);
00278
00279
00280
00281 if(hasImagePose()) {
00282 rtc_write(oh,image_pose);
00283 }
00284
00285
00286 if(hasCameraInfo()) {
00287
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
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
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
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
00330 rtc_read(ih,pose);
00331 rtc_read(ih,"flags",flags);
00332
00333
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
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
00378 if(hasTexture()) {
00379 teximage.read(ih);
00380 }
00381
00382 if (getFlag(MESH_FLAG_HAS_TEXTUREMASK))
00383 texmask.read(ih);
00384
00385
00386 if(hasImagePose()) {
00387 rtc_read(ih,image_pose);
00388 }
00389
00390
00391 if(hasCameraInfo()) {
00392
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
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
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
00446 int num_vertices = (int)vertices.size();
00447 int num_faces = (int)faces.size();
00448
00449
00450 fwrite(&pose,sizeof(pose),1,fp);
00451 fwrite(&flags,sizeof(flags),1,fp);
00452
00453
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
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
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
00482 if(hasImagePose()) {
00483 fwrite(&image_pose,sizeof(image_pose),1,fp);
00484 }
00485
00486
00487 if(hasCameraInfo()) {
00488
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
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
00519 res+=fread(&pose,sizeof(pose),1,fp);
00520 res+=fread(&flags,sizeof(flags),1,fp);
00521
00522
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
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
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
00580 if(hasImagePose()) {
00581 res+=fread(&image_pose,sizeof(image_pose),1,fp);
00582 }
00583
00584
00585 if(hasCameraInfo()) {
00586
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
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 }
00614
