CamCalibration.h

Go to the documentation of this file.

#ifndef _CAMCALIBRATION_H
#define _CAMCALIBRATION_H

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

#include <vector>
#include <math.h>
#include <time.h>
#include <iostream>
#include <stdio.h>
#include <cv.h>
#include <geometry/homography.h>

struct ProjObs;

class CamCalibration {

public:
  CamCalibration();

  ~CamCalibration();

  void AddCamera( int width, int height );

  bool AddHomography( int c );

  struct s_struct_points {
    double u,v,x,y;
    s_struct_points(){}
    s_struct_points( double u, double v, double x, double y ) : u(u), v(v), x(x), y(y) {}
  };

  bool AddHomography( int c, std::vector<CamCalibration::s_struct_points> p, CvMat* ready );

  bool StoreHomographiesToFile( char* file_name );

  bool LoadHomographiesFromFile( char* file_name );

  bool Calibrate( int p_HomographyNum, int p_PreFilter, int p_Solutions, double p_PreFilter_a, double p_PreFilter_b, double p_PreFilter_c,
    double p_InitialGuess_a, double p_InitialGuess_b, double p_InitialGuess_c,
    int p_Iterations, double p_Epsilon, double p_PostFilter );

  void PrintOptimizedResultsToFile1();

  void PrintOptimizedResultsToFile2(  char* file_descriptor, bool create_png = false, char* sequence_descriptor = "", 
    char* png_descriptor = "", int c_start = 0, int h_start = 0 );

  // Functions below this line are used by CamCalibration and CamAugmentation only ! //

  static void ExtractRotationTranslationFrom3x4Matrix( CvMat* RotTrans , CvMat* Rot, CvMat*Trans );

  static void ComposeRotationTranslationTo3x4Matrix( CvMat* RotTrans , CvMat* Rot, CvMat*Trans );

  static void Mat3x4Mul( CvMat* m_A, CvMat* m_B, CvMat* m_C );

  static void Mat3x4Inverse( CvMat* m_A, CvMat* m_B );

  static void HomogenousNormalizeVector( CvMat* mat );

  void PlotPointsToImagesAfterOptimization();

  void ClearAll();

private:

  struct s_struct_plane {
    int                 p;
    double             *px;
    double             *py;
    std::vector<CvMat*> v_m_wp;
    std::vector<CvMat*> v_m_pp;
    double              rx,ry,rz;
    double              tx,ty,tz;

    s_struct_plane() {
      px     = NULL;
      py     = NULL;
    }

    ~s_struct_plane() {
      delete px;
      delete py;
      for( int i = 0; i < (int)v_m_wp.size(); i++ ) cvReleaseMat(&v_m_wp[i]);
      v_m_wp.clear();
      for( int i = 0; i < (int)v_m_pp.size(); i++ ) cvReleaseMat(&v_m_pp[i]);
      v_m_pp.clear();
    }
  };

  struct s_struct_homography {
    s_struct_plane*    s_plane_object;
    homography*     m_homography;
    CvMat*      m_estim_r_t_matrix;
    CvMat*      m_jacobian_matrix;
    bool      b_discard;

    s_struct_homography(){
      s_plane_object     = new s_struct_plane();
      b_discard     = true; // specifies discarding candidates
      m_homography       = NULL; // NULL  = not valid!
      m_estim_r_t_matrix = cvCreateMat( 3, 4, CV_64FC1 );
      m_jacobian_matrix  = cvCreateMat( 3, 9, CV_64FC1 );
    }
    ~s_struct_homography(){
      if( s_plane_object ) delete s_plane_object;
      if( m_homography   ) delete m_homography;
      cvReleaseMat(&m_estim_r_t_matrix);
      cvReleaseMat(&m_jacobian_matrix );
    }
  };

  struct s_struct_intrinsic {
    double focal;
    double aspect;
    double u0,v0;
  };

  struct s_struct_camera {
    int         w,h;
    s_struct_intrinsic     s_intrinsic;
    s_struct_intrinsic     s_estim_int;
    std::vector<s_struct_homography*> v_homography;
    CvMat*        m_calibration_matrix;
    CvMat*        m_rot_trans_matrix;
    CvMat*        m_estim_calib_matrix;
    CvMat*        m_estinvcalib_matrix;  

    s_struct_camera( int width, int height ){
      w = width;
      h = height;
      m_calibration_matrix = cvCreateMat( 3, 3, CV_64FC1 );
      m_rot_trans_matrix   = cvCreateMat( 3, 4, CV_64FC1 );
      m_estim_calib_matrix = cvCreateMat( 3, 3, CV_64FC1 );
      m_estinvcalib_matrix = cvCreateMat( 3, 3, CV_64FC1 );
    }

    ~s_struct_camera(){
      for( int i = 0; i < (int)v_homography.size(); i++ ) delete v_homography[i];
      v_homography.clear();
      cvReleaseMat(&m_calibration_matrix);
      cvReleaseMat(&m_rot_trans_matrix  );
      cvReleaseMat(&m_estim_calib_matrix);
      cvReleaseMat(&m_estinvcalib_matrix);
    }
  };

  std::vector<s_struct_camera*> v_camera;

  struct s_struct_solution {
    int c,h;
    s_struct_solution( int c, int h ) : c(c), h(h) {}
  };

  struct s_struct_optimal {
    int     c_max, h_max;
    std::vector<CvMat*> v_camera_c;
    std::vector<CvMat*> v_camera_r_t;
    std::vector<CvMat*> v_camera_r_t_jacobian;
    std::vector<CvMat*> v_homography_r_t;
    std::vector<CvMat*> v_homography_r_t_jacobian;
    /*deleteall(){
    for( int i = 0; i < (int)v_camera_c.size(); i++ )
    if( v_camera_c[i] ) cvReleaseMat( &v_camera_c[i] );
    v_camera_c.clear();
    for( int i = 0; i < (int)v_camera_r_t.size(); i++ )
    if( v_camera_r_t[i] ) cvReleaseMat( &v_camera_r_t[i] );
    v_camera_r_t.clear();
    for( int i = 0; i < (int)v_camera_r_t_jacobian.size(); i++ )
    if( v_camera_r_t_jacobian[i] ) cvReleaseMat( &v_camera_r_t_jacobian[i] );
    v_camera_r_t_jacobian.clear();
    for( int i = 0; i < (int)v_homography_r_t.size(); i++ )
    if( v_homography_r_t[i] ) cvReleaseMat( &v_homography_r_t[i] );
    v_homography_r_t.clear();
    for( int i = 0; i < (int)v_homography_r_t_jacobian.size(); i++ )
    if( v_homography_r_t_jacobian[i] ) cvReleaseMat( &v_homography_r_t_jacobian[i] );
    v_homography_r_t_jacobian.clear();
    }*/
    s_struct_optimal(){
    }
    ~s_struct_optimal(){
      //deleteall();
    }
  };

  s_struct_optimal s_optimal;

  std::vector<double> v_opt_param;

  CvMat *m_CH;

  enum matrix_desc { m_calibration,
    m_rot_trans,
    m_estim_calib,
    m_estinvcalib,
    m_estim_r_t,
    m_jacobian };

  int  errorgraphic_counter;

  int  stat_HomographyNum;

  time_t stat_ExpStartTime;

  double  GetRandomValue( double min, double max );

  void    AddGaussianNoise( CvMat* mat, double noise );

  void SetIntrinsicParam( int c, double f, double a, double u, double v );

  void SetPlaneObject( int c, int h, int p, double px[], double py[],
    double rx, double ry, double rz, double tx, double ty, double tz );

  void CreateCalibrationMatrixFromIntrinsicParameters( int c );

  void CreateEstimatedCalibrationMatrixFromEstimatedIntrinsicParameters( int c );

  void CreateEstimatedInverseCalibrationMatrixFromEstimatedIntrinsicParameters( int c );

  void    CreateRotationTranslationMatrixFromInverseCalibrationMatrixAndHomography( int c, int h );

  void    CreateAllRotationTranslationMatrices( int c );

  void    CreateRotationTranslationIdentityMatrix( int c );

  void    CreateRotationTranslationMatrix( int c, double rx, double ry, double rz,
    double tx, double ty, double tz );

  void CreatePlaneObjectWorldPoints( int c, int h );

  void CreatePlaneObjectPlanePoints( int c, int h, double noise );

  void CreateWorldPlaneHomography( int c, int h, CvMat* ready );

  bool    HomographySingular( int c, int h, double p_InitialGuess_a, double p_InitialGuess_b, double p_InitialGuess_c );

  double  VectorAngle( CvMat* v1, CvMat* v2, CvMat* v3, CvMat* v4 );

  double  PointDistance( CvMat* v1, CvMat* v2 );

  void    DeleteWorldPlaneHomography( int c, int h );

  std::vector<s_struct_solution> GetRandomSolution( int num );

  double GetScreenDistance( int c, int h1, int h2 );

  double  GetSolutionQuality( std::vector<s_struct_solution> solution, double p_PreFilter_a, double p_PreFilter_b, double p_PreFilter_c );

  bool    FilterBestHomographiesGreedyMethod( int num );

  bool    FilterBestHomographiesFillingMethod( int num, double p_PreFilter_a );

  bool    FilterBestHomographiesRandomMethod( int num, int p_Solutions, double p_PreFilter_a, double p_PreFilter_b, double p_PreFilter_c );

  bool    ExtractIntrinsicParameters( int c, double p_InitialGuess_a, double p_InitialGuess_b, double p_InitialGuess_c );

  double  GetFrobeniusDistance( int c, int h1, int h2 );

  bool    GetDistalHomographiesForCalibration( int c, int *h1, int *h2 );

  bool FilterHomographiesAfterOptimization( double p_PostFilter );

  bool OptimizeCalibrationByMinimalParameterMethod( int iter, double eps, double p_PostFilter );

  bool    CreateMatrixCH();

  void    GetParameterSetFromOptimalStructure();

  void    SetParameterSetToOptimalStructure();

  void    CreateEstimated3ChainMatrixRT( int h1, int c1, int h2, int c2 );

  void CreateEstimatedOptimalHomographyMatrixRT( int h1, int c1, int h2 );

  void CreateEstimatedOptimalCameraMatrixRT( int c, int h );

  void CreateEstimatedOptimalCameraMatrixC( int c );

  bool CreateOptimalCameraStructure();

  void ReleaseOptimalCameraStructure();

  int     GetParameterNumber();

  int     GetObservationNumber();

  CvMat* GetM( enum matrix_desc desc, int c, int h );

  void Translate3DVector( CvMat* v, double x, double y, double z );

  void Rotate3DVector( CvMat* v, double x, double y, double z );

  static void Mat3x4ToMat4x4( CvMat* m_A, CvMat* m_B );

  static void Mat4x4ToMat3x4( CvMat* m_A, CvMat* m_B );

  static void updateCB(double *params, void **user_data);

  //static void projFunc( double *x, double *params, int na, double *f, double *grad, int *ind, LsqData *Data );

  void PrintOptimizedResultErrors(  double *params );

  void PrintStatisticToFile();

  friend struct ProjObs;
};

#endif