grid3D.hpp

#ifndef GRID_3D_HPP
#define GRID_3D_HPP

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

namespace jafar {
  namespace model3d {

    class Grid3D
    {
    public:
      Grid3D();
      ~Grid3D();

      void build(Image3D& im3d, double dbStep);

      void initialize(int nX, int nY, int nZ, 
          double minX_, double minY_, double minZ_,
          double stepX_, double stepY_, double stepZ_);

      void fillGrid(::std::vector<Point3D>* ppoints);

#ifdef HAVE_OPENGL

      void display(int nOnlyRectangle=0);
#endif // HAVE_OPENGL

#ifdef HAVE_OPENGL
      void displayCloud3D();
      void displayGrayCloud3D();
      void TestOpenGL();
#endif // HAVE_OPENGL

      void colorizeImage3D(char* fileName, char* outputIndeces);
      //      void colorizeImage3D(unsigned char* pImage, int nWidth, int nHeight);

      std::vector<float> m_vColor;
      unsigned char* pImage_BRUT;
      int m_nWidth_BRUT, m_nHeight_BRUT;
      void loadBrutImage(char* fileName);

      void extractFloor(double delta, bool removeInnerPoints);
      Plane3D m_plane3dFloor; 
#ifdef HAVE_OPENGL
      void displayFloorPlane();
#endif // HAVE_OPENGL


      int*** m_pAccumulator;
      int m_nRho, m_nPhi, m_nTheta, m_nMaxVote;
      void prepareHoughAccumulator(float fRhoMax,
                                   float rhoStep,
                                   float angleStep,
                                   bool withSave = false,
                                   char * fileName = NULL);

#ifdef HAVE_OPENGL

      void displayHoughAccumulator();
#endif // HAVE_OPENGL

      void extractBigerHoughPlane3D(int maxRho=25,
                                    float rhoStep=0.02, 
                                    float angleStep=5);

      Plane3D m_BiggerPlane3D;// the major plane
#ifdef HAVE_OPENGL
      void displayWithBiggerPlane();
#endif // HAVE_OPENGL

      //=============test only

      void extractHoughPlanes3D(int   maxRho=25, 
                                float rhoStep=0.02, 
                                float angleStep=5, 
                                float fThresholdOfMaxVote=0.5,
                                float fMinDistanceOfTowPlane=0.10,
                                float fMinCosDihedralAngle=0.975, //Theta = 13 deg
                                int   nMaxPlaneNumber=30);

      void extractProgProbHoughPlanes3D(int   maxRho=25, 
                                        float rhoStep=0.02, 
                                        float angleStep=5, 
                                        float fThresholdOfMaxVote=0.5,
                                        float fMinDistanceOfTowPlane=0.10,
                                        float fMinCosDihedralAngle=0.025,
                                        int   nMaxPlaneNumber=30,
                                        int minPointNumberThreshold=50,
                                        int minFinalVoteThreshold=100,
                                        char* planeFileName="plane.txt");


      void savePlanesCornersToFile(char* fileName);
      void savePlanesEquations(std::vector<Plane3D>& vPlane3D,
             int nMaxPlaneNumber,
             char* fileName);


      std::vector<Point3D> vP1;
      std::vector<Point3D> vP2;
      std::vector<Point3D> vP3;
      std::vector<Point3D> vP4;
#ifdef HAVE_OPENGL
      void displayPlanes();
#endif // HAVE_OPENGL

      inline Grid3DCell* operator()(int i, int j, int k)
      {
        return &m_vCells[(i*m_nNY*m_nNZ)+(j*m_nNZ)+k];
      }


      void savePlanesToFile(char* fileName);

      // reduce the number of final planes found in the cells
      // by testing the simularity
      void reducePlanesNumber(double distThreshold, 
                              double cosDihedralThreshold,
                              int nPower);

      void extractPlaneInCellsMLSE();

      void extractPlaneInCellsEKFI();

      void bestPlaneMinLeastSquares(std::vector<int>& pointIndex,
            Plane3D& plane3D);


      void bestPlaneEKFI(std::vector<int>& pointIndex,
       Plane3D& plane3D);

      void planeEKFI(std::vector<int>& pointIndex,
         double abcE[5],
         int nPlaneType);

      void planeMinLeastSquares(std::vector<int>& pointIndex,
        double abcE[5],
        int type);

      void info();

      RansacPlane* getRansac() 
      { return &m_RansacPlane;}

      // data members

      ::std::vector<Grid3DCell> m_vCells;      

      ::std::vector<Point3D>* m_pPoints;

      RansacPlane m_RansacPlane;

      int m_nNX;
      int m_nNY;
      int m_nNZ;

      double m_dbStepX;
      double m_dbStepY;
      double m_dbStepZ;

      double m_dbMinX;
      double m_dbMinY;
      double m_dbMinZ;
    };

   
  }// namespace model3d
}// namespace jafar

#endif // GRID_3D_HPP