Image3DTemplate.hpp

#ifndef IMAGE_3D_TEMPLATE_HPP
#define IMAGE_3D_TEMPLATE_HPP


#include <GL/gl.h>
// for definition of HAVE_OPENGL
#include "jafarConfig.h"

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "model3d/point3D.hpp"
#include "model3d/point3DVar.hpp"
#include <iostream>
#include <fstream>
#include <vector>
#include <cfloat>
#include <stdlib.h>
#include <fstream>


namespace jafar {
  namespace model3d {

#ifndef MAX
#define MAX(a,b) ((a) > (b) ? (a) : (b))
#endif
    
#ifndef MIN
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#endif


    // pre-declaration to avoid compiler error 
    // for template friend function
    template <class PointType> class Image3DTemplate;
    template <class PointType>  
    std::ostream& operator<< (std::ostream& os,
            const Image3DTemplate<PointType>& im3d);
    template <class PointType>  
    std::istream& operator>> (std::istream& is,
            Image3DTemplate<PointType>& im3d);


    template <class PointType>
    class Image3DTemplate
    {
    public:

      enum ROBOT_TYPE { NIL_ROBOT,
      ROBOT_ADAM,
      ROBOT_HILARE,
      ROBOT_HILARE2, 
      ROBOT_LAMA,
      ROBOT_EVE,
      ROBOT_DALA,
      ROBOT_DILIGENT,
      ROBOT_INCONNU};

      enum CAPTEUR_TYPE { NIL_CAPTEUR, 
        CAPTEUR_LASER1, 
        CAPTEUR_LASER2, 
        CAPTEUR_STEREO,
        CAPTEUR_STEREO_GEROMS94, 
        CAPTEUR_LASER_SIMULATION,
        CAPTEUR_STEREO_SIMULATION};

      enum REPERE_TYPE { NIL_REPERE,
       REPERE_CAPTEUR,
       REPERE_AUTRE};
      
      enum COORDINATES_TYPE { NIL_COORDONNEES, 
            COORDONNEES_CARTESIENNES,
            COORDONNEES_CYLINDRIQUES, 
            COORDONNEES_SPHERIQUES };  
      
      // --------------
      // constructor and destructor
      Image3DTemplate();
      ~Image3DTemplate();

      // --------------
      // methodes

      inline PointType* operator () (int i, int j)
      {
  return &m_vPoints[i*m_nWidth+j];
      }

      void initialize(int nWidth_, int nHeight_);

      void clear();

      void info();


#ifdef HAVE_OPENGL

      void display(unsigned char r=0, 
       unsigned char g=0,
       unsigned char b=0);
      
#endif // HAVE_OPENGL

      // temprary function to load data directly from
      // three files have the data , one for X, Y and Z
      void readBrutData(int nWidth_, char* xFile, char* yFiel, char* zFile);


      void minMax();

      void minMaxFilteredPoints();

      void lowPassfilter(double maxX_);

      // mark the points out of the cube given by these 
      // limits as not_good points
      void dropOuterPoints(double x_min, double x_max, 
                           double y_min, double y_max,
                           double z_min, double z_max);

      // mark the points out of the cube given by these 
      // limits as not_good points
      void dropOuterPoints(double rho_min, double rho_max);

      void saveToFile(char* fileName);
      void readFromFile(char* fileName);

      // --------------
      // data member

      std::vector<PointType> m_vPoints;

      std::vector<PointType> m_vFilteredPoints;

      int m_nNumberOfGoodPoints;
 
      int m_nWidth;
      int m_nHeight;

      double minX;
      double minY;
      double minZ;
      double maxX;
      double maxY;
      double maxZ;


      double fminX;
      double fminY;
      double fminZ;
      double fmaxX;
      double fmaxY;
      double fmaxZ;


      ROBOT_TYPE       robot_type;
      CAPTEUR_TYPE     capteur_type;
      REPERE_TYPE      repere_type;
      COORDINATES_TYPE coordinates_type;

      // --------------
      // io steraming
      // Attention the empty <> is necessary to have a template function
      // --------------
      friend std::ostream& operator<< (std::ostream& os,
               const Image3DTemplate<PointType>& im3d);

      //      friend std::ostream& operator<< <> (::std::ostream& os,
      //        const Image3DTemplate<PointType>& im3d);
      friend std::istream& operator>> (std::istream& is,
               Image3DTemplate<PointType>& im3d);
    };

    // -----------------------------------------------------
    // Implementation
    // -----------------------------------------------------

    template <class PointType>
    Image3DTemplate<PointType>::Image3DTemplate()
    {
      m_nNumberOfGoodPoints = 0;
    }

    template <class PointType>
    Image3DTemplate<PointType>::~Image3DTemplate()
    {
      m_vPoints.clear();
    }

    template <class PointType>
    void Image3DTemplate<PointType>::initialize(int nWidth_, int nHeight_)
    {
      JFR_PRECOND((nWidth_ > 0)&&(nHeight_>0), 
      "invalide negative width or height");
      m_nWidth = nWidth_;
      m_nHeight = nHeight_;
      m_vPoints.resize(m_nWidth*m_nHeight);
    }

    template <class PointType>
    void Image3DTemplate<PointType>::clear()
    {
      m_nWidth = 0;
      m_nHeight = 0;
      m_vPoints.clear();
      m_nNumberOfGoodPoints = 0;
    }


    template <class PointType>
    void Image3DTemplate<PointType>::info()
    {
      std::cout << "Image3D" << std::endl
    << "\tWidth= " << m_nWidth
    << "\tHeight= "<< m_nHeight<< std::endl
    << "\tNumber of Points = "<< m_vPoints.size() << std::endl
    << "\tNumber of Good points " << m_nNumberOfGoodPoints 
    << std::endl
    << "\t " <<  minX << " < X < " << maxX << std::endl
    << "\t " <<  minY << " < Y < " << maxY << std::endl
    << "\t " <<  minZ << " < Z < " << maxZ << std::endl;
    }


#ifdef HAVE_OPENGL

    template <class PointType>
    void Image3DTemplate<PointType>::display(unsigned char uc_r,
               unsigned char uc_g,
               unsigned char uc_b)
    {
  
      typename std::vector<PointType>::iterator i;
  
      // push the current colors
      glPushAttrib(GL_CURRENT_BIT);
      glDisable(GL_CULL_FACE);
      glPushMatrix();
  
      glBegin(GL_POINTS);
          glColor3ub(uc_r,uc_g,uc_b);// black points
    for (i= m_vPoints.begin(); i!= m_vPoints.end(); i++) 
      if (i->isGoodPoint())
        glVertex3f(i->x(),i->y(),i->z());  
      glEnd();

      glPopMatrix();
      glPopAttrib();
    }
    
#endif // HAVE_OPENGL


    template <class PointType>
    void Image3DTemplate<PointType>::minMax()
    {
      m_nNumberOfGoodPoints = 0;
 
      minX = DBL_MAX;
      minY = DBL_MAX;
      minZ = DBL_MAX;
      
      maxX = -DBL_MAX;
      maxY = -DBL_MAX;
      maxZ = -DBL_MAX;

      typename std::vector<PointType>::iterator i;
      for (i=m_vPoints.begin(); i != m_vPoints.end(); i++) {
  if (i->isGoodPoint()) { 
    minX = MIN(minX, i->x());
    minY = MIN(minY, i->y());
    minZ = MIN(minZ, i->z());
    
    maxX = MAX(maxX, i->x());
    maxY = MAX(maxY, i->y());
    maxZ = MAX(maxZ, i->z());
    m_nNumberOfGoodPoints++;
  }
      }
      if (m_nNumberOfGoodPoints == 0) {
  minX = 0;
  minY = 0;
  minZ = 0;
  maxX = 0;
  maxY = 0;
  maxZ = 0;

      }
      std::cout << m_nNumberOfGoodPoints << " good points !! " << std::endl;
    }


    template <class PointType>
    void Image3DTemplate<PointType>::minMaxFilteredPoints()
    {
      fminX = DBL_MAX;
      fminY = DBL_MAX;
      fminZ = DBL_MAX;
      
      fmaxX = -DBL_MAX;
      fmaxY = -DBL_MAX;
      fmaxZ = -DBL_MAX;


      if (m_vFilteredPoints.size() == 0) {
  fminX = 0;
  fminY = 0;
  fminZ = 0;  
  fmaxX = 0;
  fmaxY = 0;
  fmaxZ = 0;
      }

      typename std::vector<PointType>::iterator i;
      for (i=m_vFilteredPoints.begin(); i != m_vFilteredPoints.end(); i++) {
  fminX = MIN(fminX, i->x());
  fminY = MIN(fminY, i->y());
  fminZ = MIN(fminZ, i->z());
  
  fmaxX = MAX(fmaxX, i->x());
  fmaxY = MAX(fmaxY, i->y());
  fmaxZ = MAX(fmaxZ, i->z());
      }

      std::cout << "filtered points : " << std::endl
    << "\tMin X Y Z are : "  << fminX << "  " 
    << fminY << " " << fminZ << std::endl
    << "\tMax X Y Z are : " << fmaxX
    << " " << fmaxY << " " << fmaxZ
    << std::endl;
      
    }


    template <class PointType>
    void Image3DTemplate<PointType>::lowPassfilter(double maxX_)
    {
      double dbMeanX = 0;
      double dbMeanY = 0;
      double dbMeanZ = 0;

      double dbSigma2X = 0; // sigma2 of X
      double dbSigma2Y = 0;
      double dbSigma2Z = 0;

      if (m_nNumberOfGoodPoints == 0)
        return;

      typename std::vector<PointType>::iterator i;

      for (i=m_vPoints.begin(); i != m_vPoints.end(); i++) {
        if (i->isGoodPoint()) { 
          dbMeanX += i->x();
          dbMeanY += i->y();
          dbMeanZ += i->z();
        }
      }

      dbMeanX /= m_nNumberOfGoodPoints;
      dbMeanY /= m_nNumberOfGoodPoints;
      dbMeanZ /= m_nNumberOfGoodPoints;

      for (i=m_vPoints.begin(); i != m_vPoints.end(); i++) {
        if (i->isGoodPoint()) { 
          dbSigma2X += (i->x()-dbMeanX)*(i->x()-dbMeanX);
          dbSigma2Y += (i->y()-dbMeanY)*(i->y()-dbMeanY);
          dbSigma2Z += (i->z()-dbMeanZ)*(i->z()-dbMeanZ);
        }
      }

      dbSigma2X /= m_nNumberOfGoodPoints;
      dbSigma2Y /= m_nNumberOfGoodPoints;
      dbSigma2Z /= m_nNumberOfGoodPoints;

      int nNumberOfGoodFilteredPoints = 0;
      double dbSigmaX = sqrt(dbSigma2X);
      double dbSigmaY = sqrt(dbSigma2Y);
      double dbSigmaZ = sqrt(dbSigma2Z);

      for (i=m_vPoints.begin(); i != m_vPoints.end(); i++) {
        if (i->isGoodPoint()) {
          if ( (fabs(i->x()-dbMeanX) < 2*dbSigmaX) &&
               (i->x()<maxX_) &&
               (fabs(i->y()-dbMeanY) < 2*dbSigmaY) &&
               (fabs(i->z()-dbMeanZ) < 2*dbSigmaZ) )
            nNumberOfGoodFilteredPoints++;
        }
      }
      
      std::cout << "Mean X    Y     Z  are " << std::endl
                << dbMeanX <<" " << dbMeanY << " "
                << dbMeanZ << std::endl;
      std::cout << "Sigma  X    Y     Z  are " << std::endl
                << dbSigmaX <<" " << dbSigmaY << " "
                << dbSigmaZ << std::endl;
      std::cout << "number of filtered points " 
                << nNumberOfGoodFilteredPoints
                << std::endl;
      
      if (nNumberOfGoodFilteredPoints == 0) 
        return;
      
      m_vFilteredPoints.resize(nNumberOfGoodFilteredPoints);
      
      int j = 0;
      for (i=m_vPoints.begin(); i != m_vPoints.end(); i++) {
        if (i->isGoodPoint()) {
          if ( (fabs(i->x()-dbMeanX) < 2*dbSigmaX) &&
               (i->x()<maxX_) &&
               (fabs(i->y()-dbMeanY) < 2*dbSigmaY) &&
               (fabs(i->z()-dbMeanZ) < 2*dbSigmaZ) ) {
            
            m_vFilteredPoints[j] = *i;
            j++;
          }
        }
      }
      
      std::cout << "number of filtered points " 
                << nNumberOfGoodFilteredPoints
                << std::endl;
      
    }
    


    // mark the points out of the cube given by these limits as not_good points
    template <class PointType>
    void Image3DTemplate<PointType>::dropOuterPoints(double x_min, double x_max, 
                                                     double y_min, double y_max,
                                                     double z_min, double z_max)
    {
      
      typename std::vector<PointType>::iterator i;
      
      for (i=m_vPoints.begin(); i != m_vPoints.end(); i++) {
        if (i->isGoodPoint()) { 
          if ( (i->x()<x_min) || (i->x()>x_max) ||
               (i->y()<y_min) || (i->y()>y_max) ||
               (i->z()<z_min) || (i->z()>z_max) )
            i->state = Point3D::BAD_POINT;
        }
      }
    }

    // mark the points out of the cube given by these limits as not_good points
    template <class PointType>
    void Image3DTemplate<PointType>::dropOuterPoints(double rho_min, double rho_max)
    {
      typename std::vector<PointType>::iterator i;
      double r;
      for (i=m_vPoints.begin(); i != m_vPoints.end(); i++) {
        if (i->isGoodPoint()) { 
          r = sqrt(i->x()*i->x() + i->y()*i->y() + i->z()*i->z());
          if ( (r < rho_min) || (r > rho_max) )
            i->state = Point3D::BAD_POINT;
        }
      }
    }
    

    // temprary function to load data directly from
    // three files have the data , one for X, Y and Z
    template <class PointType>
    void Image3DTemplate<PointType>::readBrutData(int nWidth_,
                                                  char* xFile, 
                                                  char* yFile, 
                                                  char* zFile)
    {
      initialize(nWidth_, nWidth_);
      std::ifstream xstr;
      std::ifstream ystr;
      std::ifstream zstr;
      xstr.open(xFile);
      ystr.open(yFile);
      zstr.open(zFile);
      double x,y,z;
      int nPointCount = nWidth_*nWidth_;
      for (int i=0;i< nPointCount; i++) {
        xstr >> x;
        ystr >> y;
        zstr >> z;
        m_vPoints[i].setCoord(x,y,z);
      }
      
      minMax();
      lowPassfilter(10);
      minMaxFilteredPoints();
      
      xstr.close();
      ystr.close();
      zstr.close();      
    }



    template <class PointType>
    void Image3DTemplate<PointType>::saveToFile(char* fileName)
    {
      std::fstream file;
      file.open(fileName, std::fstream::out | std::fstream::trunc);

      if (! file.is_open()) {
  std::cout << "can't open file " << fileName << std::endl;
  return;
      }
      /*
      file << m_vPoints.size() << std::endl;

      file << m_nWidth  << std::endl;
      file << m_nHeight << std::endl;


      typename std::vector<PointType>::const_iterator i;
      for(i=m_vPoints.begin(); i!= m_vPoints.end(); i++)
  file << *i << std::endl;
      file << std::endl;
      */

      typename std::vector<PointType>::const_iterator i;
      for(i=m_vFilteredPoints.begin(); i!= m_vFilteredPoints.end(); i++)
  if (i->x() !=0 && i->y()!=0) 
    file << i->x()-3.0 << "  " << i->y()
         << "  " << i->z()+0.63 << std::endl;
      file << std::endl;
    }
    

    template <class PointType>
    void Image3DTemplate<PointType>::readFromFile(char* fileName)
    {
      std::fstream file;
      file.open(fileName, std::fstream::in);

      if (! file.is_open()) {
  std::cout << "can't open file " << fileName << std::endl;
  return;
      }
      m_vPoints.clear();
      int nSize;
      file >> nSize;


      if (nSize>0) {
  file >> m_nWidth;
  file >> m_nHeight;
  m_vPoints.resize(nSize);
  for (int i=0; i< nSize; i++) 
    file >> m_vPoints[i];
      }
    }



    //--------------------------------------------------------------------
    // friend functions for serialization
    //--------------------------------------------------------------------
    template <class PointType>
    std::ostream& operator << (std::ostream& os,
             const Image3DTemplate<PointType>& im3d)
    {
      os << im3d.m_vPoints.size() << std::endl;;
      typename std::vector<PointType>::const_iterator i;
      for(i=im3d.m_vPoints.begin(); i!= im3d.m_vPoints.end(); i++)
  os << *i << std::endl;
      os << std::endl;
      return os;
    }


    template <class PointType>
    std::istream& operator >> (std::istream& is,
             Image3DTemplate<PointType>& im3d)
    {
      im3d.m_vPoints.clear();
      int nSize;
      is >> nSize;
      if (nSize>0) {
  im3d.m_vPoints.resize(nSize);
  for (int i=0; i< nSize; i++) 
    is >> im3d.m_vPoints[i];
      }
      return is;
    }
   
  }// namespace model3d
}// namespace jafar

#endif // IMAGE_3D_TEMPLATE_HPP