GenSynthMatches.hh

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef __GenSynthMatches_hh__
#define __GenSynthMatches_hh__

#include <bias_config.h>

#include <Base/Common/BIASpragmaStart.hh>

#include <iostream>
#include <string>
#include <vector>

#include <Base/Image/Image.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Base/Geometry/FMatrixBase.hh>
#include <Base/Geometry/EuclideanTransf3D.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/PMatrixBase.hh>
#include <Utils/Param.hh>
#include <Base/Math/Random.hh>
#include "MatchDataBase.hh"


namespace BIAS{

  class PMatrix;
  class HomgPoint3D;

#define GSM_CAM_MOTION      0x00010000
#define GSM_VIEWINGVOL      0x00020000
#define GSM_3D_POINTS       0x00040000
#define GSM_2D_MATCHES      0x00080000
#define GSM_MOVIN_TRANSF    0x00100000
#define GSM_MOVIN_3D_POINTS 0x00200000
#define GSM_FMATRIX         0x00400000
#define GSM_NOISE           0x00800000

#define GSM_RETURN_OK                           0
#define GSM_RETURN_NO_STATIC_3D_POINTS         -1
#define GSM_RETURN_NO_MOVING_3D_POINTS         -2
#define GSM_RETURN_STATIC_OBJECT_NOT_VISIBLE   -3
#define GSM_RETURN_MOVING_OBJECT_NOT_VISIBLE   -4
#define GSM_RETURN_ERROR_HANDLING_STATIC_POINT -5
#define GSM_RETURN_ERROR_HANDLING_MOVING_POINT -6

  /** @class GenSynthMatches
      @ingroup g_utils
      @brief class for generating synthetic matches
      @author woelk 2004 */
  class BIASUtils_EXPORT GenSynthMatches : public MatchDataBase
  {
  public:
    GenSynthMatches(Param &para);
    virtual ~GenSynthMatches();

    // clears all internal vectors and resets K and Ki
    // also resets _rand
    void Reset();

    /** @brief Create static and moving 3D points
        see _CreateCamMovement for explanation of parameter */
    int Create3DPoints(bool fixed_errors = false);
    ///@brief Replace the static 3D point at index by a newly generated one
    int CreateStatic3DPoint(int index);
    ///@brief Replace the moving 3D point at index by a newly generated one
    int CreateMoving3DPoint(int index);
    /** returns negative value, if error occurs, 0 on success 
        see _CreateCamMovement for explanation of parameter "fixed_errors" */
    int CreateMatches(bool points3D_set_by_user = false,
                      bool fixed_errors = false);
    ///@brief create matches for existing 3D points
    int CreateMatches(int &index);

    /** see _CreateCamMovement for docu */
    inline void CreateCamMovement(const bool fixed_error = false) 
    {_CreateCamMovement(fixed_error);} ;
    
    /// fills _MovingTransform
    inline void CreateMovingTransforms() {_CreateMovingTransforms();};

    ///@brief set the static 3D points used for computation
    inline int SetStatic3DPoints(const std::vector<BIAS::HomgPoint3D> &points){
      _StaticPoints = points;
      return _StaticPoints.size();
    }
    ///@brief read the static 3D points used for computation
    inline int GetStatic3DPoints(std::vector<BIAS::HomgPoint3D> &points){
      points = _StaticPoints;
      return _StaticPoints.size();
    };
    ///@brief set the moving 3D points used for computation
    inline int SetMoving3DPoints(const std::vector<BIAS::HomgPoint3D> &points){
      _MovingPoints = points;
      return _MovingPoints.size();
    };
    ///@brief read the moving 3D points used for computaion
    inline int GetMoving3DPoints(std::vector<BIAS::HomgPoint3D> &points){
      points = _MovingPoints;
      return _MovingPoints.size();
    };

    inline int GetMovingTransforms(std::vector<BIAS::EuclideanTransf3D> &res) {
      res = _MovingTransform;
      return _MovingTransform.size();
    }
    inline int GetNumMovingObjects() {
      return *_NumMovingObjects;
    }

    void DrawTrue(Image<unsigned char>& im, int minindex=0, 
                  int maxindex=1) const;
    void DrawNoisy(Image<unsigned char>& im, int minindex=0, 
                   int maxindex=1) const;

    /// write in ascii file format
    int WriteData(std::string file) const;
    /// read in ascii file format
    int ReadData(std::string file);

    /// binary read 
    int Read(const std::string& fname);
    /// binary read 
    virtual int Read(std::istream& is);

    /// binary write 
    int Write(const std::string& fname) const;
    /// binary write 
    virtual int Write(std::ostream& os) const;

    /// R = _GT_R[imNo]; 
    void GetGTR(int imNo, RMatrixBase& R) const;
    void GetGTR(std::vector<RMatrixBase>& R) const;

    /// C = _GT_C[imNo]; 
    void GetGTC(int imNo, HomgPoint3D& C) const;
    void GetGTC(std::vector<HomgPoint3D>& C) const;

    /// P = _GT_P[imNo]; 
    void GetGTP(int imNo, PMatrixBase& P) const;
    void GetGTP(std::vector<PMatrixBase>& P) const;

    /// p = _GT_Points[imNo]; 
    void GetGTPoints(int imNo, std::vector<HomgPoint2D>& p) const;
    void GetGTPoints(std::vector<std::vector<HomgPoint2D> >& p) const;

    /// p = _GT_NormalizedPoints[imNo]; 
    void GetGTNormalizedPoints(int imNo, std::vector<HomgPoint2D>& p) const;
    void GetGTNormalizedPoints(std::vector<std::vector<HomgPoint2D> >& p)const;

    /// p = _StaticPoints;
    void Get3DPoints(std::vector<HomgPoint3D>& p) const;

    /// p = _MovingTransform[frame] * _MovingPoints;
    void Get3DMovingPoints(std::vector<HomgPoint3D>& p,const int frame=0)const;

    /// inl[i] indicates if Get(GT)Points()[i] is outlier
    void GetInliers(std::vector<bool>& inl) const;

    // returns the fundamental matrices for each indep. motion
    void GetGTNormalizedF(std::vector<std::vector<FMatrixBase> >& F) const;

    GenSynthMatches &operator=(const MatchDataBase& mdb);

    friend BIASUtils_EXPORT std::ostream& 
    operator<<(std::ostream& os,const GenSynthMatches& m);

    friend BIASUtils_EXPORT std::istream& 
    operator>>(std::istream& os, GenSynthMatches& m);

    inline void GetImageSize(int & width, int & height) const
    { width = *_ImWidth; height = *_ImHeight; }

    inline double GetSigmaImagePoints() const
    { return *_SigmaImagePoints; }

    /** when getting point list for an image,
        inidices 
          0           - max_inlier-1    are true matches generated by static 
                                              object
          max_inlier  - max_outlier-1   are uniform distributed random outliers
                                           
          max_outlier - max_mov_obj-1   are the points generated by the moving 
          objects */
    void GetPointSourceBorders(int &max_inlier, int &max_outlier, 
                               int &max_mov_obj) const
    { max_inlier = *_NumStaticPoints; 
      max_outlier = *_NumStaticPoints + *_NumOutliers; 
      max_mov_obj = *_NumMovingPoints * *_NumMovingObjects + 
        *_NumStaticPoints + *_NumOutliers; }

    int CreateGTVRML(const std::string& file_name) const;

  protected:
    BIAS::Random _rand;
    BIAS::Param *_para; // just needed in WriteData

    // internal calibration matrix
    //BIAS::KMatrix _K, _Ki; // _Ki is inverse of _K
    // vector of rotation matrizes of cameras, true and with gaussian noise
    // with distribution para _SigmaCamRot?
    std::vector<BIAS::RMatrixBase> _GT_R;//, _R;
    // vector of camera centers, true and with gaussian noise
    // with distribution para _SigmaCamCenter?
    std::vector<BIAS::HomgPoint3D> _GT_C;//, _C;
    // vector of projection matrizes, true (_K, _GT_R[i], GT_C[i])
    // and composed from _K, _R[i] and _C[i]
    std::vector<BIAS::PMatrixBase> _GT_P;//, _P;
    // vector of transform between moving and static coo
    std::vector<BIAS::EuclideanTransf3D> _MovingTransform;
    // vector of scene points
    std::vector<BIAS::HomgPoint3D> _StaticPoints;
    // vector of points from moving object in local coo of moving obj, 
    // with (0, 0, 0) in center of moving obj
    std::vector<BIAS::HomgPoint3D> _MovingPoints;

    int *_NumStaticPoints, *_NumOutliers; // *_NumImages, *_NumPoints
    int *_ImWidth, *_ImHeight;
    double *_FocalLength, *_PrinciplePointX, *_PrinciplePointY;
    
    // noise for different values
    double *_SigmaImagePoints, *_MaxDispOutliers, *_OutlDirChange;
    double *_OutlLengthChange;
    double *_SigmaCamCenterX, *_SigmaCamCenterY, *_SigmaCamCenterZ;
    double *_SigmaCamRotX, *_SigmaCamRotY, *_SigmaCamRotZ;

    // minimal and maximal distance of 3D points from camera plane
    double *_MinCamDistance, *_MaxCamDistance;

    bool *_UserGivenP;
    // only used from param file if _UserGivenP=true
    BIAS::Vector<double> *_MotionVecX, *_MotionVecY, *_MotionVecZ, *_RotVecX,
      *_RotVecY, *_RotVecZ;

    // parameters for generating moving points
    int *_NumMovingPoints, *_NumMovingObjects;
    // e.g. *_SizeMovingObjX[k] is the x size of the (k)th moving object
    BIAS::Vector<double> *_SizeMovingObjX, *_SizeMovingObjY, *_SizeMovingObjZ;

    // describes the motion of the moving objs
    // e.g. *_MovingObjMotionVecX[k] is the x coo of the center position 
    // of the (k div *_NumMovinObj)th moving object in the 
    // (k div *_NumMovinObj)th image
    // i.e. *_MovingObjMotionVecX="1 2 3   7 8 9" and *_NumMovinObj=2
    // means object number 1 moves from x=1 in image 1 to x=2 in image 2 
    // to x=3 in image 3 while object number 2 moves from x=7 in image 1
    // to x=8 in image 2 to x=9 in image 3
    BIAS::Vector<double> *_MovingObjMotionVecX, *_MovingObjMotionVecY, 
      *_MovingObjMotionVecZ, *_MovingObjRotVecX, *_MovingObjRotVecY, 
      *_MovingObjRotVecZ;

    // the generated match data:  GT means ground truth
    // _GT_Points[k][i] is point number i in image k
    // _Points are generated using noisy data
    // for inliers the folowing is true
    //   _GT_Points[k][i]=_GT_P[k]*_StaticPoints[i]
    //   _Points[k][i]=_P[k]*_StaticPoints[i] + Noise(_SigmaImagePoints)
    std::vector<std::vector<HomgPoint2D> > _GT_Points;
    std::vector<std::vector<HomgPoint2D> > _GT_NormalizedPoints;

    /** @brief fills _k, _Ki, _GT_R, _R, GT_C, _C, _GT_P and _P

        When "fixed_errors" is set to true, the noisy camera position is set 
        such that:
        - The rotational error is assumed to be given as axis and angle. The 
        axis is choosen randomly and the angle is exactly as indicated by 
        *_SigmaCamRotX (in rad).
        - The translational error is assumed to be given as an angle. It is 
        choosen such the angle between the epipole in the noise free case and 
        the epipole in the noisey case form exactly the angle indicated by 
        *_SigmaCamCenterX (in rad). The translational error thus alway lies in 
        the plane perpendicular to the ground truth epipole. Its direction in 
        the plane is choosen randomly and its magnitude is choosen such that 
        the angles between the epipoles is exactly *_SigmaCamCenterX (in rad)

        When "fixed_errors" is set to false, the normal distributed noise with 
        diagonal covariance matrix whose elements are indicated by parameters
        *_SigmaCamCenter? are added to each camera positions. The camera 
        orientation is computed using the euler angles from *_RotVec?[frame]
        with additional normal disributed noise as indiceated by 
        *_SigmaCamRot?.

        @author woelk 01/2007 */
    void _CreateCamMovement(const bool fixed_error);
    
    /// fills _MovinTransforms
    void _CreateMovingTransforms();

    /// fills _StaticPoints
    /// returns -1 if no common viewing volume of all cameras is present
    int _CreateStatic3DPoints();

     /// returns -1 if no corner of moving object boundary is seen in images
    /// or the center of moving objects is not seen in images
    int _AreMovingObjectsVisible();

    /// fills _MovingPoints
    /// returns -1 if no corner of moving object boundary is seen in images
    /// or the center of moving objects is not seen in images
    int _CreateMoving3DPoints();

    /// fills *_NumStaticPoints first entries of _GT_Points, _Points,
    ///   _GT_NormalizedPoints, _NormalizedPoints
    /// also resizes the above vectors to 
    ///   [*_NumImages][*_NumStaticPoints+*_NumMovingObjects+*_NumOutliers]
    int _CreateStatic2DPoints(int &index);
    /// fills *_NumStaticPoints first entries of _GT_Points and
    ///   _GT_NormalizedPoints
    /// also resizes the above vectors to 
    ///   [*_NumImages][*_NumStaticPoints+*_NumMovingObjects+*_NumOutliers]
    int _CreateStaticGT2DPoints(int &index);

    /// fills _GT_Points, _Points, _GT_NormalizedPoints, _NormalizedPoints
    /// from entries [*_NumStaticPoints] to [*_NumStaticPoints+*_NumOutliers-1]
    void _AddUniformDistributedOutliers();

    /// fills _GT_Points, _Points, _GT_NormalizedPoints, _NormalizedPoints
    /// from entries [*_NumStaticPoints+_*NumOutliers] to 
    /// [*_NumStaticPoints+*_NumOutliers+*_NumMovingPoints-1]
    int _CreateMoving2DPoints(int &index);
    /// fills _GT_Points, _GT_NormalizedPoints
    /// from entries [*_NumStaticPoints+_*NumOutliers] to 
    /// [*_NumStaticPoints+*_NumOutliers+*_NumMovingPoints-1]
    int _CreateMovingGT2DPoints(int &index);
    
    /// fills all protected pointer values
    virtual void _AddParameter(Param &para);

    /// helper function used for visualizing the matches
    void _Draw(Image<unsigned char>& im, int minindex, int maxindex,
               const std::vector<std::vector<HomgPoint2D> >* p) const;

    /// helper function used for visualizing the matches
    void _DrawPoints(Image<unsigned char>& im, int index, 
                     const std::vector<std::vector<HomgPoint2D> >* p) const;

    inline bool _IsSeen(HomgPoint2D &p, double border=0.0){ 
      p.Homogenize(); 
      return (p[0]>=border && p[0]<=(((double)*_ImWidth)-border-1.0) && 
              p[1]>=border && p[1]<=(((double)*_ImHeight)-border-1.0));
    }      

    bool _CheckParams() const;

    int CreateVRML_(const std::string& file_name, 
                    const std::vector<PMatrix> &P,
                    const std::vector<HomgPoint3D>& p) const;

  }; // class

  BIASUtils_EXPORT std::ostream& operator<<(std::ostream& os, const GenSynthMatches& m);
  //BIASUtils_EXPORT std::istream& operator>>(std::istream& os, GenSynthMatches m);

} // namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif