PMatrix.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 __BIAS_PMatrix_hh__
#define __BIAS_PMatrix_hh__
#include "bias_config.h"

#include <Base/Common/BIASpragmaStart.hh>


#include <Base/Debug/Debug.hh>
#ifdef BIAS_HAVE_XML2
#  include <Base/Common/XMLBase.hh>
#endif
#include <Base/Geometry/PMatrixBase.hh>

#include <Base/Geometry/HomgLine2D.hh>
#include <Base/Geometry/HomgPlane3D.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <MathAlgo/SVD.hh>
#include <Base/Geometry/KMatrix.hh>
#include <fstream>
#include <string>

namespace BIAS {

  /** @class PMatrix
      @ingroup g_geometry
      @brief describes a projective 3D -> 2D mapping in homogenous coordinates

      The P Matrix is a 3x4 matrix (3 rows {Zeilen}, 4 columns{Spalten})
      describing a mapping between a 3D projective point X in space and the
      corresponding projective 2D point x (in an image plane).

      x = P * X

      where X is of type HomgPoint3D and x of type HomgPoint2D.
      After homogenizing, x[0] and x[1] contain the image coordinates of the
      projection of the 3D point.

      If P is a metric camera, it can be composed/decomposed into external
      parameters (orientation R and center C) and internal parameters (such
      as the focal length in the calibration matrix K):

      With R' = transpose of R,  K = Camera calibration matrix and
      C = Camera center is:
      P = (K R' | -K R' C)

      \ref geometry "See also the more general geometry section."

      Only implementation very specific to PMatrix
      (and not valid for general matrices, e.g. like decomposition)
      should be implemented here.


      @attention For performance reasons, the PMatrix decomposition/svd is
      only done once, subsequent calls to GetC(),... only return copies of the
      precomputed values. If the fields of the PMatrix have changed, the
      user is responsible for calling InvalidateDecomposition() to force a
      re-decomposition at the next GetC(),... call.
      If one does not call InvalidateDecomposition() after changing a PMatrix,
      subsequent calls of GetC() may return outdated values !

      @author Jan Woetzel **/

  class BIASGeometry_EXPORT PMatrix : public PMatrixBase
#ifdef BIAS_HAVE_XML2
      , public XMLBase
#endif
  {
  public:

    inline PMatrix() : PMatrixBase()
    {  Svd_ = NULL; InvalidateDecomposition(); };

    explicit PMatrix(const MatrixInitType& i)
      : PMatrixBase(i) { Svd_ = NULL; InvalidateDecomposition(); };

    // DEPRECATED due to instantiation problem JW
    //explicit inline PMatrix(const char *s) : PMatrixBase(s)  { Svd_ = NULL; InvalidateDecomposition(); };
    /// replacement for above JW
    explicit PMatrix(const std::string & s);

    /** @author grest **/
    inline PMatrix(const PMatrix & A)
    { Svd_=NULL; operator=(A); }

    /** @author Jan Woetzel
        @status beta (04/17/2002) **/
    inline PMatrix(const PMatrixBase & Amat_ )
      : PMatrixBase(Amat_)
    { Svd_ = NULL; InvalidateDecomposition(); };

    /** @author Jan Woetzel
        not fine, should no be here, better cast stepwise between direct
        father and child
        @status untested (04/17/2002) **/
    PMatrix(const Matrix<PMATRIX_TYPE>& Amat);

    /** Take R, K, C and compose PMatrix
        @author Jan-Friso Evers-Senne
        @date 2002-08-20 */
    inline PMatrix(const Matrix3x3<PMATRIX_TYPE> &K,
                   const Matrix3x3<PMATRIX_TYPE> &R,
                   const Vector3<PMATRIX_TYPE> &C) {
      Svd_ = NULL; Compose(K, R, C); }

    /** destructor
        @status untested (04/17/2002) **/
    virtual ~PMatrix();

    inline bool IsMetric() { return IsMetric_;}

    /** setting a new size different from 3x4 is not allowed for the fixed
        size P-Matrix overloads the general newsize from basec class
        @author Jan Woetzel **/
    PMatrix& newsize(int rows, int cols);

    /** @brief computes translation vector origin world coo -> origin
        camera coo (center), uses decomposition, which is cached */
    int GetC(Vector3<double>& C);

   /** @brief computes translation vector origin world coo -> origin
        camera coo (center), uses decomposition, which is cached */
    BIAS::Vector3<double> GetC();

    /** @brief computes translation vector origin world coo -> origin
        camera coo (center), uses decomposition, which is cached */
    int GetC(HomgPoint3D& C);

    /** @brief this is another way of getting C,
        especially interesting for non-metric cameras
        @author koeser 07/2004 */
    int GetNullVector(HomgPoint3D& C);

    /** @returns rotation matrix camera -> world coo
        (which contains the column-vectors H0, V0, A (from left to right) ) */
    int GetR(Matrix3x3<double>& R);
    inline Matrix3x3<double> GetR()
    { if (!IsDecomposed_ ) if (Decompose_() != 0)
      BIASERR("error decomposing"); return R_; };

    /// calibration matrix
    int GetK(KMatrix& K);

    inline KMatrix GetK()
    { if (!IsDecomposed_  ) if (Decompose_() != 0)
      BIASERR("error decomposing"); return K_; };

    /**
        @brief returns the unit vector H0, while H0 and V0 span
        the camera coordinate system in world coordinates

        returns the unit vector H0 which is not the cam.
        standard CAHV because image size is unknown (docu: JW 11/2002)
        represents the world coordinates of the vector (1,0,0) in ics
        A,H,V are orthonormal */
    int GetH(Vector3<double>& H);
    BIAS::Vector3<double> GetH();

    /** @return the unit vector V0 which is not the cam.#
        standard CAHV because image size is unknown (docu: JW 11/2002)
        A,H,V are orthonormal*/
    int GetV(Vector3<double>& V);
    BIAS::Vector3<double> GetV();

    /* @return up-vector of OpenGL camera
       convenience call for OpenGL JW 09/2003 */
    BIAS::Vector3<double> GetUp_gl(){
      return GetV().GetNormalized() * (double)-1;
    };

    /** @brief checks whether X is in optical A-dir from C
        (only for metric cameras)
        @author koeser 01/05 */
    inline bool IsInFrontOfCamera(const Vector3<double>& X) {
      return ((X-GetC()).ScalarProduct(GetA())>0.0);
    }


    /** @brief returns the unit vector A which is the normal vector to the
        image plane in world coordinates.

        or in other words: the camera orientation <br>
        A,H,V are orthonormal*/
    int GetA(Vector3<double>& A);

    /** @brief returns the unit vector A which is the normal vector to the
        image plane in world coordinates.

        or in other words: the camera orientation <br>
        A,H,V are orthonormal*/
    BIAS::Vector3<double> GetA();

    /// Returns SVD of this.
    void GetSVD(Matrix<double>& U, Vector<double>& S,
                Matrix<double>& VT);

    /** @brief returns 4x3 pseudo inverse */
    int GetPseudoInverse(BIAS::Matrix<double>& Pinv);


    int GetHinf(BIAS::Matrix3x3<double> &Hinf);

    /** returns image plane in world coordinates */
    void GetImagePlane(HomgPlane3D& plane);

    /** calls the above */
    HomgPlane3D GetImagePlane();

    /** modify C and re-compose P
        not fine to use, better use:  Compose() */
    void SetC(const Vector3<double>& newC);

    /** to re-Decompose_() after filling with data use this.
        @author Jan-Friso Evers-Senne */
    inline void InvalidateDecomposition();

    /** composes this from K, R and C
        using P = [ K R' | -K R' C ]
        with R' = transpose(R)
        @author Felix Woelk */
    void Compose(const Matrix3x3<double>& K, const Matrix3x3<double>& R,
                 const Vector3<double>& C);

    void Compose(const Matrix3x3<double>& K, const Matrix3x3<double>& R,
                 const HomgPoint3D& C);

    /** @author woelk 02/2006 */
    void Compose(const Matrix3x3<double>& K,
                 const Vector<double>& parametrization,
                 enum E_ParametrizationType param_type);

    /** return line resulting of intersection of the two image planes
        in pixel coordinates of this camera
        return -1 if camera planes are parallel */
    int
    GetIntersectionOfImagePlanes(PMatrix& P, HomgLine2D& intersection);

    /** calls the above
        returns line at infinity if planes are parallel */
    inline HomgLine2D GetIntersectionOfImagePlanes(PMatrix& P);

    /** Returns a homogenized 3D point X on the viewray of x,
        which always lies in front of the camera(this is checked).
        @author Daniel Grest, last edit Jan 2003 */
    HomgPoint3D BackprojectPseudoinverse (const HomgPoint2D& x);

    /** returns vector from C to point, w of point determines ray length
        analytical inverse of K is used */
    Vector3<double> GetRayWorldCoo(const HomgPoint2D& point);
    /** returns a 3D point res which is located
        depth away from center of projection in direction given by point */
    void BackprojectWorldCoo(const HomgPoint2D& point, double depth,
                             HomgPoint3D& res);

    /** Inverts the PMatrix application to a 3D point whose z-component
     * in <b>cameracoordinates</b> and its imagepoint (x,y) is known.
     */
    void BackprojectByZDepth(const double& x,
                 const double& y,
                 const double& z_depth,
                 HomgPoint3D& res);


    /** returns normed vector from C to point
        analytical inverse of K is used */
    Vector3<double> GetNormRayWorldCoo(const HomgPoint2D& point);

    /** write the CAHV decomposition of this P matrix
        in CAHV 1,0 format to stream os
        (see RK's/TNT CAHV1.0 fromat in SequenceReconstruction/
        Sequenetracking/KochSI.C)
        cols, rows is the size of the image this P matrix corresponds to.
        image size is neccessary because of shift of the axis origin
        cenetered/in left corner
        This is the 'Cunningham' format (see diss. Reinhard Koch, p.14)
        @author Jan Woetzel 11/2002
        @argument cols number of pixels
        @argument rows number of pixels */
    int WriteCAHV_10(int cols, int rows, std::ostream &os=std::cout);

    /** assignment operator
        @author Jan Woetzel
        @status alpha (02/28/2002)**/
    PMatrix& operator=(const PMatrix &mat);

    /** @return field of view angle in rad.
        for max: angle in rad between upper left and lower right
        opposing image corners.
        for min: min. angle of (horiz. vert) angle.
        Max. field of view (not horizontal or verttical fov.)
        @param optmin true if the min angle should be used,
        else the max angle(=quer)
        @param dimX horiz. image dimension in pixel (=columns)
        @param dimY vert.  image dimension in pixel (=rows)
        unfortunately not const because missing consts in decompose etc..
        we assume rectangular pixels for min(x,y) selection.
        @author JW 06/2003 */
    float GetFieldOfView(const unsigned int dimX,
             const unsigned int dimY,
             const bool optmin=true);

    /* similar to the above, but
       @return fov in Y-direction in rad
       The angle (should be) measured as total vertical/horizontal, indepedant
       of proj. center
       TODO/FIXME/HACK! (currently assumes optical center is in image center
       JW 09/2003 */
    float GetFieldOfViewY(const unsigned int dimX, const unsigned int dimY,
                          const bool & compY=true);

    // interface for the above
    float GetFieldOfViewX(const unsigned int dimX, const unsigned int dimY)
    {return GetFieldOfViewY(dimX, dimY, false);}

    ///// convenience interface for the above
    //float GetFieldOfViewX(const Vector2<unsigned int> & imgDim)
    //{return GetFieldOfViewX(imgDim[0], imgDim[1]);}
    //
    ///// convenience interface for the above
    //float GetFieldOfViewY(const Vector2<unsigned int> & imgDim)
    //{return GetFieldOfViewY(imgDim[0], imgDim[1]);}

    /** overload Load because it has to invalidate decomposition! JW 09/2003 */
    bool Load(const std::string & filename){
      bool result = BIAS::PMatrixBase::Load(filename);
      InvalidateDecomposition();
      return result;
    };


    bool  Save(const std::string & filename);

    /** decompose the P-matrix to [ A | a ] where A is a 3x3-matrix
        and a is a 3-vector.
        @author mdunda 01 2004 */
    void DecomposeAa(BIAS::Matrix3x3<PMATRIX_TYPE> &A,
                     BIAS::Vector3<PMATRIX_TYPE> &a) const;

    /** Get the projective homography matrix so that P * H = [ I | 0 ]
        @author mdunda 01 2004 */
    Matrix4x4<PMATRIX_TYPE> GetCanonicalH() const;
    void GetCanonicalH(Matrix4x4<PMATRIX_TYPE> &ProjH) const;

    /** @brief scale P such that optical axis (first three entries of last row)
        has unit length
        @author koeser 03/2004 */
    inline void Normalize();


    /** @brief Load calibration data file from BBC-Free-D system

        opens a free-d file and reads first (or only) piece of free-d data
        @param AddCenterPointShiftX add this to the principle point (in meter)
        @param AddCenterPointShiftY add this to the principle point (in meter)
        @author evers, koeser */
    int LoadBBC(const std::string &filename,
                const double& AddCenterPointShiftX = 0.0,
                const double& AddCenterPointShiftY = 0.0);

    /** @brief Load next piece of calibration data from an open BBC-Free-D file
        @param AddCenterPointShiftX add this to the principle point (in meter)
        @param AddCenterPointShiftY add this to the principle point (in meter)
        @author evers, koeser */
    int BBCIn(std::ifstream &ifs,
              const double& AddCenterPointShiftX = 0.0,
              const double& AddCenterPointShiftY = 0.0);

#ifdef BIAS_HAVE_XML2
    /** @brief specialization of XML block name function */
    virtual int XMLGetClassName(std::string& TopLevelTag,
                                double& Version) const;

    /** @brief specialization of XML write function */
    virtual int XMLOut(const xmlNodePtr Node, XMLIO& XMLObject) const;

    /** @brief specialization of XML read function */
    virtual int XMLIn(const xmlNodePtr Node, XMLIO& XMLObject);
#endif


    int SetCovariance(const BIAS::Matrix<double> &cov) {
      Covariance_ = cov; return 0;}

    int GetCovariance( BIAS::Matrix<double> &cov) {
      cov = Covariance_ ; return 0;}

    /** @brief returns average squared projection Error */
    double GetProjectionError(const std::vector<BIAS::HomgPoint2D>& points2D, const std::vector<BIAS::HomgPoint3D>& points3D);

  protected:
    /** @brief Computes K,R,C, assuming a metric PMatrix P = K[R^T | -R^T * C]
        with K[0][1]=0 (zero skew) and K[2][2]=1

        Does a LU-Decomposition of the first 3x3 part. Finally C is computed
        from the last column of P
        @attention this is NOT the cam.### 'standard' CAHV !
        and pmatrix must be metric, i.e. there must not be a _projective_ skew
        due to distorted 3d space coordinates:
        .Projection equation:  x =  P * H^-1  *  H * X
        .                        = (P * H^-1) * (H * X)
        .                        =    P'      *    X'
        If such a P' (with a projective distortion H) is estimated for some
        reason, the lu factorization makes no sense and the Get-Functions will
        return bad values, e.g. R is no rotation ...
        To get the center in the H-distorted space in that case, you should
        call PMatrix::GetNullVector instead.
        If you get a k[0][1]==skew!=0.0 the decomposition failed.
        (comments added by koeser)
        @author woelk */
    int Decompose_();

    void Compose_();

    //* @brief computes a new svd for pseudoinverse */
    inline int MakeSVD_();

    /** @brief check if SVD is still valid */
    inline void CheckSVD_();

    /** @brief check if Decomposition is still valid */
    inline void CheckDecomposition_();

    /// camera center in wcs (extrinsic parameter)
    Vector3<double> C_;

    /// unit vector in wcs assigning the direction of
    /// the optical axis
    Vector3<double> A_;

    /// unit vector in wcs assigning the direction
    /// of the horizontal axis of the image plane
    Vector3<double> H0_;

    /// unit vector in wcs assigning the direction
    /// of the vertical axis of the image plane
    Vector3<double> V0_;

    /// Hinfinity
    Matrix3x3<double> Hinf_;

    Matrix3x3<double> R_; ///< rotation matrix (intrinsic parameter)
    KMatrix K_; ///< camera calibration matrix (intrinsic parameter)
    SVD  *Svd_; ///< (for decomposition, docu: JW 11/2002)

#ifdef BIAS_DEBUG
    /// backup of P at time of last decomposition to check whether
    /// decomposition is still valid
    Matrix3x4<double> PConsistencyBackup_;
    Matrix3x4<double> PConsistencyBackupSVD_;

#endif
    Matrix<double> Covariance_;

    /// tells us whether we have a chached decomposition with C,A,...
    bool IsDecomposed_;

    /// tells us whether we have an arbitrary 3x4 matrix or
    /// a P which is exactly a composition of P = K * R [ I | -C ]
    /// this is called a metric pmatrix
    bool IsMetric_;
  }; // class PMatrix
  // ====================================




  inline HomgLine2D
  PMatrix::GetIntersectionOfImagePlanes(PMatrix& P)
  {
    HomgLine2D intersection(0.0, 0.0, 0.0);
    GetIntersectionOfImagePlanes(P, intersection);
    return intersection;
  }

  inline void PMatrix::InvalidateDecomposition() {
    if (Svd_!=NULL) delete Svd_;
    Svd_ = NULL;
#ifdef BIAS_DEBUG
    PConsistencyBackup_.Fill(-1.0);
    PConsistencyBackupSVD_.Fill(-1.0);
#endif
    IsDecomposed_ = false;
    IsMetric_ = false; }

  void PMatrix::Normalize() {
#ifdef BIAS_DEBUG
    if (IsDecomposed_) CheckDecomposition_();
    if (Svd_ != NULL) CheckSVD_();
#endif

    // make determinant of front 3x3 matrix > 0 by multiplying with -1 one if necessary
    Matrix3x3<double> M;
    for(unsigned int i=0; i < 3; i++){
      for(unsigned int j=0; j < 3; j++){
        M[i][j] = (*this)[i][j];
      }
    }
    bool changedSign = false;
    if(M.GetDeterminant() < 0){
      this->MultiplyIP(-1);
      changedSign = true;
    }

    // normalize last rot to unit length
    const PMATRIX_TYPE* pLastRow = row_[2];
    (*this) /= sqrt(pLastRow[0]*pLastRow[0] +
                    pLastRow[1]*pLastRow[1] + pLastRow[2]*pLastRow[2]);

    // multiplication with -1 touches rotation, therefore decomposition is invalid
    if(changedSign)
      IsDecomposed_ = false;

#ifdef BIAS_DEBUG
    if (IsDecomposed_) {

      PConsistencyBackup_ = (*this);
    }
    if (Svd_ != NULL) {
      PConsistencyBackupSVD_ = (*this);
    }
#endif
  }

  inline int PMatrix::MakeSVD_(){
    int retvalue = 0;
    if (Svd_!=NULL) {
#ifdef BIAS_DEBUG
      CheckSVD_();
#endif
    } else {
      Svd_ = new SVD;
      retvalue = Svd_->compute(*this);
#ifdef BIAS_DEBUG
      PConsistencyBackupSVD_ = (*this);
#endif
    }
    return retvalue;
  }

  inline void PMatrix::CheckSVD_() {
#ifdef BIAS_DEBUG
    // check if PMatrix is _REALLY_ still valid
    if (memcmp((void*)PConsistencyBackupSVD_.GetData(),
               (void*)GetData(),12*sizeof(double))) {
      BIASERR("Tried to access cached decomp.(outdated) for changed PMatrix! "
              <<"You have to invalidate after changing PMatrix! Aborting.");
      BIASABORT;
    }
#endif
  }

  inline void PMatrix::CheckDecomposition_() {
#ifdef BIAS_DEBUG
    // check if PMatrix is _REALLY_ still valid
    if (memcmp((void*)PConsistencyBackup_.GetData(),
               (void*)GetData(),12*sizeof(double))) {
      BIASERR("Tried to access cached(outdated) svd for changed PMatrix ! "
              <<"You have to invalidate after changing PMatrix! Aborting.");
      BIASABORT;
    }
#endif
  }


} // end namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif