rtcMesh3D.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2007
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: Autonomous Technologies
 * file .......: Mesh3D.cpp
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 02/29/2008
 * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
 * changed by .: $Author:benjaminpitzer $
 * revision ...: $Revision:141 $
 */

//== INCLUDES ==================================================================
#include <map>
#include <assert.h>
#include "rtc/rtcVertex3D.h"
#include "rtc/rtcMesh3D.h"

//== NAMESPACES ================================================================
namespace rtc {

Mesh3D::Mesh3D()
:IOObject(), pose(), flags(0)
{
  flags|=MESH_FLAG_HAS_VERTEX_NORMAL;
  flags|=MESH_FLAG_HAS_FACE_NORMAL;
  flags|=MESH_FLAG_HAS_VERTEX_COLOR;
  flags|=MESH_FLAG_HAS_VERTEX_FLAG;
  flags|=MESH_FLAG_HAS_FACE_FLAG;
  flags|=MESH_FLAG_HAS_IMAGE_POSE;
  flags|=MESH_FLAG_HAS_CAMERA_INFO;
  flags|=MESH_FLAG_HAS_FRAME_ID;
}

Mesh3D::~Mesh3D()
{
  clear();
}

Mesh3D::Mesh3D(const Mesh3D& other)
: IOObject()
{
  // copy properties
  *this=other;
}

void Mesh3D::clear()
{
  for(unsigned int i=0;i<vertices.size();++i) {
    Vertex3D* v = vertices[i];
    delete v;
  }
  vertices.clear();

  for(unsigned int i=0;i<faces.size();++i) {
    Face3D* f = faces[i];
    delete f;
  }
  faces.clear();
}

const Mesh3D &Mesh3D::operator=(const Mesh3D& other)
{
  set(other);
  return *this;
}

// create mesh from scan
void Mesh3D::set(const Mesh3D& other)
{
  // clean up first
  clear();

  // copy properties
  pose = other.pose;
  flags = other.flags;

  // copy vertices
  VecVertex3D::const_iterator itv,itv_end;
  for (itv=other.vertices.begin(),itv_end=other.vertices.end(); itv!=itv_end; ++itv){
    Vertex3D* vertex = new Vertex3D(*(*itv));
    vertex->mesh = this;
    vertices.push_back(vertex);
  }

  // copy faces
  VecFace3D::const_iterator itf,itf_end;
  for (itf=other.faces.begin(),itf_end=other.faces.end(); itf!=itf_end; ++itf){
    Face3D* face = new Face3D(*(*itf));
    face->mesh = this;
    faces.push_back(face);
  }

  //texture mapping
  #ifndef NO_GRAPHICS
  texture = other.texture;
  #endif
  teximage = other.teximage;
  texmask = other.texmask;
  image_pose = other.image_pose;
  camera_info = other.camera_info;
  frame_id = other.frame_id;
}

void Mesh3D::update() {
  updateNormals();
}

void Mesh3D::updateNormals()
{
  // update face normals
  updateFaces();
  // update vertex normals
  updateVertices();
}

int Mesh3D::numVertices() const
{
  return (int)vertices.size();
}

void Mesh3D::updateVertices() {
  for (unsigned int i=0;i<vertices.size();++i) {
    vertices[i]->update();
  }
}

Vertex3D* Mesh3D::addVertex(Vertex3D* vertex)
{
  assert(vertex);
  vertices.push_back(vertex);
  return vertex;
}

Vertex3D* Mesh3D::addVertex(const Point3Df& p)
{
  Vertex3D* vertex = new Vertex3D(this,p);
  return addVertex(vertex);
}

int Mesh3D::numFaces() const
{
  return (int)faces.size();
}

void Mesh3D::updateFaces() {
  for (unsigned int i=0;i<faces.size();++i) {
    faces[i]->update();
  }
}

Face3D* Mesh3D::addFace(Face3D* face)
{
  faces.push_back(face);
  return face;
}

Face3D* Mesh3D::addFace(int v0, int v1, int v2)
{
  assert(v0>=0&&v0<(int)vertices.size());
  assert(v1>=0&&v1<(int)vertices.size());
  assert(v2>=0&&v2<(int)vertices.size());
  Face3D* face = new Face3D(this,v0,v1,v2);
  int index = faces.size();
  vertices[v0]->faces.push_back(index);
  vertices[v1]->faces.push_back(index);
  vertices[v2]->faces.push_back(index);
  return addFace(face);
}

Face3D* Mesh3D::addFace(int vhandles[3])
{
  return addFace(vhandles[0],vhandles[1],vhandles[2]);
}

// flags
bool Mesh3D::hasVertexNormals() const
{
  return (flags&MESH_FLAG_HAS_VERTEX_NORMAL);
}

bool Mesh3D::hasFaceNormals() const
{
  return (flags&MESH_FLAG_HAS_FACE_NORMAL);
}

bool Mesh3D::hasVertexColors() const
{
  return (flags&MESH_FLAG_HAS_VERTEX_COLOR);
}

bool Mesh3D::hasFaceColors() const
{
  return (flags&MESH_FLAG_HAS_FACE_COLOR);
}

bool Mesh3D::hasVertexFlags() const
{
  return (flags&MESH_FLAG_HAS_VERTEX_FLAG);
}

bool Mesh3D::hasFaceFlags() const
{
  return (flags&MESH_FLAG_HAS_FACE_FLAG);
}

bool Mesh3D::hasTexture() const
{
  return (flags&MESH_FLAG_HAS_TEXTURE);
}

bool Mesh3D::hasImagePose() const
{
  return (flags&MESH_FLAG_HAS_IMAGE_POSE);
}

bool Mesh3D::hasCameraInfo() const
{
  return (flags&MESH_FLAG_HAS_CAMERA_INFO);
}

bool Mesh3D::hasFrameID() const
{
  return (flags&MESH_FLAG_HAS_FRAME_ID);
}

bool Mesh3D::getFlag(int flag) const
{
  return (flags&flag);
}

void Mesh3D::setFlag(int flag, bool value)
{
  if(value)
    flags|=flag;
  else
    flags&=flag;
}

bool Mesh3D::write(OutputHandler &oh) const
{
  // get other properties
  int num_vertices = (int)vertices.size();
  int num_faces = (int)faces.size();

  // write pose
  rtc_write(oh,pose);
  rtc_write(oh,"flags",flags);

  // write vertices
  rtc_write(oh,"num_vertices",num_vertices);
  Vec3f vertex,normal;
  Vec3uc color;
  VecVertex3D::const_iterator it,it_end;
  for (it=vertices.begin(),it_end=vertices.end(); it!=it_end; ++it) {
    Vertex3D* vertex = *it;
    vertex->write(oh);
  }

  // write faces
  rtc_write(oh,"num_faces",num_faces);
  VecFace3D::const_iterator itf,itf_end;
  for (itf=faces.begin(),itf_end=faces.end(); itf!=itf_end; ++itf){
    Face3D* face = *itf;
    face->write(oh);
  }

  // write texture
  if(hasTexture()) teximage.write(oh);
  if(getFlag(MESH_FLAG_HAS_TEXTUREMASK)) texmask.write(oh);


  // write image pose
  if(hasImagePose()) {
    rtc_write(oh,image_pose);
  }

  // write camera info
  if(hasCameraInfo()) {
    // header
    rtc_write(oh,"seq",camera_info.header.seq);
    rtc_write(oh,"sec",camera_info.header.stamp.sec);
    rtc_write(oh,"nsec",camera_info.header.stamp.nsec);
    rtc_write(oh,"frame_id",camera_info.header.frame_id);

    rtc_write(oh,"height",camera_info.height);
    rtc_write(oh,"width",camera_info.width);

    rtc_write(oh,"distortion_model",camera_info.distortion_model);
    size_t size = camera_info.D.size();
    rtc_write(oh,"D_size",size);
    for (size_t i = 0; i < size; ++i) rtc_write(oh,"D",camera_info.D[i]);

    for (size_t i = 0; i < camera_info.K.size(); ++i) rtc_write(oh,"K",camera_info.K[i]);
    // for (size_t i = 0; i < camera_info.K.size(); ++i) rtc_write(oh,"K",camera_info.K[i]);
    for (size_t i = 0; i < camera_info.R.size(); ++i) rtc_write(oh,"R",camera_info.R[i]);
    for (size_t i = 0; i < camera_info.P.size(); ++i) rtc_write(oh,"P",camera_info.P[i]);

    rtc_write(oh,"binning_x",camera_info.binning_x);
    rtc_write(oh,"binning_y",camera_info.binning_y);

    // ROI
    rtc_write(oh,"x_offset",camera_info.roi.x_offset);
    rtc_write(oh,"y_offset",camera_info.roi.y_offset);
    rtc_write(oh,"height",camera_info.roi.height);
    rtc_write(oh,"width",camera_info.roi.width);
    rtc_write(oh,"do_rectify",camera_info.roi.do_rectify);
  }

  // write frame id
  if(hasFrameID()) {
    rtc_write(oh,"frame_id",frame_id);
  }

  return oh.good();
}

bool Mesh3D::read(InputHandler &ih)
{
  int num_vertices,num_faces;

  // start reading
  rtc_read(ih,pose);
  rtc_read(ih,"flags",flags);

  // read mesh vertices
  rtc_read(ih,"num_vertices",num_vertices);
  Vec3f vertex,normal;
  Vec3uc color;
  for(int i=0;i<num_vertices;++i) {
    Vertex3D* v = new Vertex3D(this);
    v->read(ih);
    vertices.push_back(v);
    if(!ih.good())
      throw Exception("format error: unexpected end of file");
  }

  // read mesh faces
  rtc_read(ih,"num_faces",num_faces);
  for(int i=0;i<num_faces;++i) {
    Face3D* face = new Face3D(this,0,0,0);
    face->read(ih);
    int index = faces.size();
    if(face->v[0]>=0) {
      Vertex3D* v0 = vertices[face->v(0)];
      v0->faces.push_back(index);
    }
    else
      return false;

    if(face->v[1]>=0) {
      Vertex3D* v1 = vertices[face->v(1)];
      v1->faces.push_back(index);
    }
    else
      return false;

    if(face->v[2]>=0) {
      Vertex3D* v2 = vertices[face->v(2)];
      v2->faces.push_back(index);
    }
    else
      return false;

    faces.push_back(face);
    if(!ih.good())
      throw Exception("format error: unexpected end of file");
  }

  // read texture
  if(hasTexture()) {
    teximage.read(ih);
  }

  if (getFlag(MESH_FLAG_HAS_TEXTUREMASK))
    texmask.read(ih);

  // read image pose
  if(hasImagePose()) {
    rtc_read(ih,image_pose);
  }

  // read camera info
  if(hasCameraInfo()) {
    // header
    rtc_read(ih,"seq",camera_info.header.seq);
    rtc_read(ih,"sec",camera_info.header.stamp.sec);
    rtc_read(ih,"nsec",camera_info.header.stamp.nsec);
    rtc_read(ih,"frame_id",camera_info.header.frame_id);;

    rtc_read(ih,"height",camera_info.height);
    rtc_read(ih,"width",camera_info.width);

    rtc_read(ih,"distortion_model",camera_info.distortion_model);
    size_t size = camera_info.D.size();
    rtc_read(ih,"D_size",size);
    camera_info.D.resize(size);
    for (size_t i = 0; i < size; ++i) rtc_read(ih,"D",camera_info.D[i]);

    for (size_t i = 0; i < camera_info.K.size(); ++i) rtc_read(ih,"K",camera_info.K[i]);
    for (size_t i = 0; i < camera_info.R.size(); ++i) rtc_read(ih,"R",camera_info.R[i]);
    for (size_t i = 0; i < camera_info.P.size(); ++i) rtc_read(ih,"P",camera_info.P[i]);
    rtc_read(ih,"binning_x",camera_info.binning_x);
    rtc_read(ih,"binning_y",camera_info.binning_y);

    // ROI
    rtc_read(ih,"x_offset",camera_info.roi.x_offset);
    rtc_read(ih,"y_offset",camera_info.roi.y_offset);
    rtc_read(ih,"height",camera_info.roi.height);
    rtc_read(ih,"width",camera_info.roi.width);
    rtc_read(ih,"do_rectify",camera_info.roi.do_rectify);
  }

  // read frame id
  if(hasFrameID()) {
    rtc_read(ih,"frame_id",frame_id);;
  }

  return ih.good();
}

void Mesh3D::loadTexture()
{
#ifndef NO_GRAPHICS
  texture.fromImage(teximage);
#endif
}

void Mesh3D::unloadTexture()
{
#ifndef NO_GRAPHICS
  texture.unloadTexture();
#endif
}

void Mesh3D::write( FILE *fp ) const
{
  // get other properties
  int num_vertices = (int)vertices.size();
  int num_faces = (int)faces.size();

  // write pose
  fwrite(&pose,sizeof(pose),1,fp);
  fwrite(&flags,sizeof(flags),1,fp);

  // write vertices
  fwrite(&num_vertices,sizeof(num_vertices),1,fp);
  Vec3f vertex,normal;
  Vec3uc color;
  VecVertex3D::const_iterator it,it_end;
  for (it=vertices.begin(),it_end=vertices.end(); it!=it_end; ++it) {
    Vertex3D* vertex = *it;
    vertex->write(fp);
  }

  // write faces
  fwrite(&num_faces,sizeof(num_faces),1,fp);
  VecFace3D::const_iterator itf,itf_end;
  for (itf=faces.begin(),itf_end=faces.end(); itf!=itf_end; ++itf){
    Face3D* face = *itf;
    face->write(fp);
  }

  // write texture
  if(hasTexture()) teximage.write(fp);
  if(getFlag(MESH_FLAG_HAS_TEXTUREMASK)) 
  {
    int nrow=texmask.rows(),ncol=texmask.columns();
    fwrite(&nrow,sizeof(nrow),1,fp);
    fwrite(&ncol,sizeof(ncol),1,fp);
    fwrite(texmask.x,sizeof(int)*2,nrow*ncol,fp);
  }

  // read image pose
  if(hasImagePose()) {
    fwrite(&image_pose,sizeof(image_pose),1,fp);
  }

  // read camera info
  if(hasCameraInfo()) {
    // header
    fwrite(&camera_info.header.seq,sizeof(camera_info.header.seq),1,fp);
    fwrite(&camera_info.header.stamp.sec,sizeof(camera_info.header.stamp.sec),1,fp);
    fwrite(&camera_info.header.stamp.nsec,sizeof(camera_info.header.stamp.nsec),1,fp);
    fwrite(&camera_info.header.frame_id,sizeof(camera_info.header.frame_id),1,fp);

    fwrite(&camera_info.height,sizeof(camera_info.height),1,fp);
    fwrite(&camera_info.width,sizeof(camera_info.width),1,fp);
    for (size_t i = 0; i < camera_info.K.size(); ++i) fwrite(&camera_info.K[i],sizeof(camera_info.K[i]),1,fp);
    for (size_t i = 0; i < camera_info.R.size(); ++i) fwrite(&camera_info.R[i],sizeof(camera_info.R[i]),1,fp);
    for (size_t i = 0; i < camera_info.P.size(); ++i) fwrite(&camera_info.P[i],sizeof(camera_info.P[i]),1,fp);
    fwrite(&camera_info.binning_x,sizeof(camera_info.binning_x),1,fp);
    fwrite(&camera_info.binning_y,sizeof(camera_info.binning_y),1,fp);

    // ROI
    fwrite(&camera_info.roi.x_offset,sizeof(camera_info.roi.x_offset),1,fp);
    fwrite(&camera_info.roi.y_offset,sizeof(camera_info.roi.y_offset),1,fp);
    fwrite(&camera_info.roi.height,sizeof(camera_info.roi.height),1,fp);
    fwrite(&camera_info.roi.width,sizeof(camera_info.roi.width),1,fp);
    fwrite(&camera_info.roi.do_rectify,sizeof(camera_info.roi.do_rectify),1,fp);
  }

  return;
}

void Mesh3D::read(FILE *fp)
{
  size_t res = 0;
  int num_vertices,num_faces;

  // start reading
  res+=fread(&pose,sizeof(pose),1,fp);
  res+=fread(&flags,sizeof(flags),1,fp);

  // read mesh vertices
  res+=fread(&num_vertices,sizeof(num_vertices),1,fp);
  Vec3f vertex,normal;
  Vec3uc color;
  for(int i=0;i<num_vertices;++i) {
    Vertex3D* v = new Vertex3D(this);
    v->read(fp);
    vertices.push_back(v);
    if(feof(fp))
      throw Exception("format error: unexpected end of file");
  }

  // read mesh faces
  res+=fread(&num_faces,sizeof(num_faces),1,fp);
  for(int i=0;i<num_faces;++i) {
    Face3D* face = new Face3D(this,0,0,0);
    face->read(fp);
    int index = faces.size();
    if(face->v[0]>=0) {
      Vertex3D* v0 = vertices[face->v(0)];
      v0->faces.push_back(index);
    }
    else
      return ;

    if(face->v[1]>=0) {
      Vertex3D* v1 = vertices[face->v(1)];
      v1->faces.push_back(index);
    }
    else
      return ;
    if(face->v[2]>=0) {
      Vertex3D* v2 = vertices[face->v(2)];
      v2->faces.push_back(index);
    }
    else
      return ;

    faces.push_back(face);
    if(feof(fp))
      throw Exception("format error: unexpected end of file");
  }

  // write texture
  if(hasTexture()) {
    teximage.read(fp);
  }

  if (getFlag(MESH_FLAG_HAS_TEXTUREMASK))
  {
    int nrow,ncol;
    res+=fread(&nrow,sizeof(nrow),1,fp);
    res+=fread(&ncol,sizeof(ncol),1,fp);
    texmask.setSize(nrow,ncol);
    res+=fread(texmask.x,sizeof(int)*2,nrow*ncol,fp);
  }

  // read image pose
  if(hasImagePose()) {
    res+=fread(&image_pose,sizeof(image_pose),1,fp);
  }

  // read camera info
  if(hasCameraInfo()) {
    // header
    res+=fread(&camera_info.header.seq,sizeof(camera_info.header.seq),1,fp);
    res+=fread(&camera_info.header.stamp.sec,sizeof(camera_info.header.stamp.sec),1,fp);
    res+=fread(&camera_info.header.stamp.nsec,sizeof(camera_info.header.stamp.nsec),1,fp);
    res+=fread(&camera_info.header.frame_id,sizeof(camera_info.header.frame_id),1,fp);

    res+=fread(&camera_info.height,sizeof(camera_info.height),1,fp);
    res+=fread(&camera_info.width,sizeof(camera_info.width),1,fp);
    for (size_t i = 0; i < camera_info.K.size(); ++i) res+=fread(&camera_info.K[i],sizeof(camera_info.K[i]),1,fp);
    for (size_t i = 0; i < camera_info.R.size(); ++i) res+=fread(&camera_info.R[i],sizeof(camera_info.R[i]),1,fp);
    for (size_t i = 0; i < camera_info.P.size(); ++i) res+=fread(&camera_info.P[i],sizeof(camera_info.P[i]),1,fp);
    res+=fread(&camera_info.binning_x,sizeof(camera_info.binning_x),1,fp);
    res+=fread(&camera_info.binning_y,sizeof(camera_info.binning_y),1,fp);

    // ROI
    res+=fread(&camera_info.roi.x_offset,sizeof(camera_info.roi.x_offset),1,fp);
    res+=fread(&camera_info.roi.y_offset,sizeof(camera_info.roi.y_offset),1,fp);
    res+=fread(&camera_info.roi.height,sizeof(camera_info.roi.height),1,fp);
    res+=fread(&camera_info.roi.width,sizeof(camera_info.roi.width),1,fp);
    res+=fread(&camera_info.roi.do_rectify,sizeof(camera_info.roi.do_rectify),1,fp);
  }

}



//==============================================================================
} // NAMESPACE rtc
//==============================================================================