mesher.hpp

/* $Id: bpa.hpp 4800 2010-08-24 22:10:30Z nksallem $ */

#ifndef MODELER_MESHER_HPP
#define MODELER_MESHER_HPP

//opencv
// #include <cxtypes.h>
// #include <cxcore.h>
// #include <cxcore.hpp>
// #include <cv.h>
//jafar
#include "camera/cameraPinhole.hpp"
#include "fdetect/InterestFeature.hpp"
#include "geom/t3d.hpp"
#include "jmath/jblas.hpp"
#include "kernel/jafarMacro.hpp"
#include "modeler/jfr_export_ply.h"
#include "modeler/jfr_import_ply.h"
#include "modeler/jfr_io_mask.h"
#include "modeler/modelerException.hpp"
#include "modeler/vcgDeclarations.hpp"
#include "modeler/xyzMayBeN.hpp"
//vcg
#include <vcg/complex/trimesh/update/bounding.h>
#include <vcg/complex/trimesh/update/topology.h>
#include <vcg/complex/trimesh/update/normal.h>
#include <vcg/complex/trimesh/allocate.h>

// std
#include <iostream>
#include <vector>
#include <time.h>


namespace jafar {
  namespace modeler {
    class Mesher {

    public:
      enum HandledTypes {XYZ, PLY, UNKNOWN};
    private:
      void typeFromFileName(const std::string& filename,
                            HandledTypes& type, std::string& extension)
      {
        size_t point_position = filename.find_last_of('.');
        extension = filename.substr(point_position+1);
        if (!extension.compare("xyz") || !extension.compare("XYZ")) {
          type = XYZ;
        }
        else if (!extension.compare("ply") || !extension.compare("PLY"))
          type = PLY;
        else
          type = UNKNOWN;
      };

    public:
      typedef std::vector< fdetect_v2::InterestFeature<fdetect_v2::FloatDescriptor>* > Origines;

      JfrMesh mesh;
      vcg::tri::Allocator<JfrMesh>::PointerUpdater<JfrMesh::VertexPointer> pu;

      Mesher() {};
      Mesher(JfrMesh &_mesh) : mesh(_mesh) {};
      Mesher(const std::string& filename)
      {
        std::string extension;
        HandledTypes type;
        typeFromFileName(filename, type, extension);
        switch (type) {
        case XYZ :
          {
            XYZMayBeN points;
            points.load(filename);
            JfrMesh::VertexIterator v_iter = vcg::tri::Allocator<JfrMesh>::AddVertices(mesh, points.matrix.rows);
            JfrMesh::CoordType p, n;
            if(points.matrix.cols == 3) {
              for (int i = 0; i < points.matrix.rows; ++i, ++v_iter) {
                for (int j=0; j < 3; ++j) {
                  p[j] = points.matrix.at<float>(i,j);
                }
                v_iter->P() = p;
              }
            } else {
              if (points.matrix.cols == 6){
                for (int i = 0; i < points.matrix.rows; ++i, ++v_iter) {
                  for (int j=0; j < 3; ++j) {
                    p[j] = points.matrix.at<float>(i,j);
                    n[j] = points.matrix.at<float>(i,3+j);
                  }
                  v_iter->P() = p;
                  v_iter->N() = n;
                }
              }
            }
          }
          break;
        case PLY :
          {
            int mask = 0;
            if (vcg::tri::io::JfrImporterPLY<JfrMesh>::Open(mesh, filename.c_str(), mask) != 0)
              JFR_ERROR(ModelerException,
                        ModelerException::FILE_READING_ERROR,
                        "Error reading ply file "<<filename)
          }
          break;
        default:
          JFR_ERROR(ModelerException,
                    ModelerException::WRONG_TYPE,
                    "Unknown file extension " << extension)
          break;
        }
      }
      virtual void run(const float&, const float&, const float&) = 0;
      /*
       * saves the mesh to a ply file.See demo/dino.ply for an example
       * @param file_name: name of the file to save in
       * @param save_binary: indicates whether to save in binary mode or ascii. Default is ascii
       */
      void save(const std::string& file_name, bool save_binary = false) {
        //        vcg::tri::io::PlyInfo pi;
        int mask = 0;
        mask |= vcg::tri::io::JfrMask::IOM_VERTORIGINES; //save vertex origin
        mask |= vcg::tri::io::JfrMask::IOM_VERTID; //save vertex id
        mask |= vcg::tri::io::JfrMask::IOM_CAMERA;
        vcg::tri::io::JfrExporterPLY<JfrMesh>::Save(mesh,file_name.c_str(), mask, save_binary);
      }
      /*
       * loads the mesh from a ply file.
       * @param file_name: name of the file to load from
       * @param the found mask
       * @note: the mask is never read it is just assigned the value found in file
       */
      void load(const std::string& file_name, int& mask) {
        //        vcg::tri::io::PlyInfo pi;
        mask = 0;
        vcg::tri::io::JfrImporterPLY<JfrMesh>::Open(mesh,file_name.c_str(), mask);
      }

      virtual ~Mesher() {};
      void setVertices(const std::map<unsigned int, jblas::vec3> &points);
      void setVertices(const std::map<unsigned int, jblas::vec3> &points,
                       const std::map<unsigned int, Mesher::Origines >& origines);
      void addVertex(unsigned int id, const JfrMesh::CoordType& v, 
                     unsigned int& internal_id);

      void addVertex(unsigned int id, const JfrMesh::CoordType& v,
                     const std::vector<unsigned int>& origines, 
                     unsigned int& internal_id);
      void addVertex(unsigned int id, const JfrMesh::CoordType& v,
                     const std::vector<unsigned int>& origines);

      JfrMesh::VertexPointer pointerFromPos(unsigned int position)
      {
        return &(mesh.vert[position]);
      }
      void setShots(const std::map<unsigned int, jblas::vec>& cameras,
                    const std::map<unsigned int, geom::T3D*>& frames);
      void setShots(const jblas::vec& intrinsic,
                    const std::map<unsigned int, geom::T3D*>& frames);

      void addShot(unsigned int id, const jblas::vec& intr, const geom::T3D* extr);
      void addFeature(fdetect_v2::InterestFeature<fdetect_v2::FloatDescriptor>* ft)
      {
        mesh.addFeature(ft);
      }
      void update()
      {
        vcg::tri::UpdateBounding<JfrMesh>::Box(mesh);
        vcg::tri::UpdateNormals<JfrMesh>::PerFace(mesh);
      }
      void update(JfrMesh::VertexPointer& vp)
      {
        if(pu.NeedUpdate()) pu.Update(vp);
      }
      unsigned int lastShotId()
      {
        std::map<unsigned int, JfrMesh::Shot>::reverse_iterator last = mesh.shots.rbegin();
        return last->first;
      }
    };

    template<typename S>
    static void convert(const vcg::Shot<S> &shot, camera::CameraPinhole& camera, geom::T3D* frame)
    {
      camera.alphaU = double(shot.Intrinsics.PixelSizeMm[0]);
      camera.alphaV = double(shot.Intrinsics.PixelSizeMm[1]);
      camera.u0     = double(shot.Intrinsics.CenterPx[0]   );
      camera.v0     = double(shot.Intrinsics.CenterPx[1]   );
      camera.width  = shot.Intrinsics.ViewportPx[0];
      camera.height = shot.Intrinsics.ViewportPx[1];

      vcg::Matrix44<S> rm = shot.Extrinsics.Rot();
      jblas::mat33 r;
      r(0,0) = double(rm[0][0]);
      r(0,1) = double(rm[0][1]);
      r(0,2) = double(rm[0][2]);

      r(1,0) = double(rm[1][0]);
      r(1,1) = double(rm[1][1]);
      r(1,2) = double(rm[1][2]);

      r(2,0) = double(rm[2][0]);
      r(2,1) = double(rm[2][1]);
      r(2,2) = double(rm[2][2]);

      vcg::Point3<S> tv = shot.Extrinsics.Tra();
      jblas::vec3 t;
      t[0] = double(tv[0]);
      t[1] = double(tv[1]);
      t[2] = double(tv[2]);

      frame->set(r,t);
    }
    template<typename S>
    static void convert(const vcg::Shot<S> &shot,
                        jblas::vec& intrinsics, geom::T3D* extrinsics)
    {
      intrinsics.resize(6);
      intrinsics[0] = double(shot.Intrinsics.ViewportPx[0] );
      intrinsics[1] = double(shot.Intrinsics.ViewportPx[1] );
      intrinsics[2] = double(shot.Intrinsics.CenterPx[0]   );
      intrinsics[3] = double(shot.Intrinsics.CenterPx[1]   );
      intrinsics[4] = double(shot.Intrinsics.PixelSizeMm[0]);
      intrinsics[5] = double(shot.Intrinsics.PixelSizeMm[1]);

      vcg::Matrix44<S> rm = shot.Extrinsics.Rot();
      jblas::mat33 r;
      r(0,0) = double(rm[0][0]);
      r(0,1) = double(rm[0][1]);
      r(0,2) = double(rm[0][2]);

      r(1,0) = double(rm[1][0]);
      r(1,1) = double(rm[1][1]);
      r(1,2) = double(rm[1][2]);

      r(2,0) = double(rm[2][0]);
      r(2,1) = double(rm[2][1]);
      r(2,2) = double(rm[2][2]);

      vcg::Point3<S> tv = shot.Extrinsics.Tra();
      jblas::vec3 t;
      t[0] = double(tv[0]);
      t[1] = double(tv[1]);
      t[2] = double(tv[2]);

      extrinsics->set(r,t);
    }

    template<typename S>
    static void convert(const jblas::vec& intrinsics, const geom::T3D* extrinsics,
                        vcg::Shot<S> &shot)
    {
      assert(intrinsics.size() == 6);
      shot.Intrinsics.ViewportPx[0]  = (int)intrinsics[0];
      shot.Intrinsics.ViewportPx[1]  = (int)intrinsics[1];
      shot.Intrinsics.CenterPx[0]    = (S)intrinsics[2];
      shot.Intrinsics.CenterPx[1]    = (S)intrinsics[3];
      shot.Intrinsics.PixelSizeMm[0] = (S)intrinsics[4];
      shot.Intrinsics.PixelSizeMm[1] = (S)intrinsics[5];

      vcg::Matrix44<S> rm;
      jblas::mat33 r = extrinsics->getR();
      rm[0][0] = (S)r(0,0);
      rm[0][1] = (S)r(0,1);
      rm[0][2] = (S)r(0,2);

      rm[1][0] = (S)r(1,0);
      rm[1][1] = (S)r(1,1);
      rm[1][2] = (S)r(1,2);

      rm[2][0] = (S)r(2,0);
      rm[2][1] = (S)r(2,1);
      rm[2][2] = (S)r(2,2);
      shot.Extrinsics.Rot() = rm;
      vcg::Point3<S> tv;
      jblas::vec3 t = extrinsics->getT();
      tv[0] = (S)t[0];
      tv[1] = (S)t[1]; 
      tv[2] = (S)t[2];
      shot.Extrinsics.Tra() = tv;
    }

  }
}

#endif