PMatrixBase.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/Debug/Debug.hh>
#include <Base/Math/Matrix3x4.hh>
#include <Base/Math/Vector4.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Vector.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Math/Matrix4x4.hh>
#include "HomgLine2D.hh"
#include "HomgPoint3D.hh"
  //#include "HomgLine3D.hh"

// dimension of P matrix:
#define P_MATRIX_ROWS 3
#define P_MATRIX_COLS 4

// element type:
#define PMATRIX_TYPE double

#define PMATRIX_EPSILON 1e-14
namespace BIAS {

  class BIASGeometryBase_EXPORT RMatrixBase;

/** @class PMatrixBase
    @ingroup g_geometry
    @brief describes a projective 3D -> 2D mapping in homogenous coordinates 
    
    Matrix P is a 3x4 matrix (3 rows {Zeilen}, 4 columns{Spalten}) 
    describing a mapping between a 3D projective point in space and the 
    corresponding projective 2D point (in an image plane).
    
    With R' = transpose of R,  K = Kamera calibration matrix and 
    C = Kamera center is:
    P = (K R' | -K R' C)
    This means that R is the rotation from camera to world coo, and
    R' = R.Transpose() the rotation from world to camera coo.

    Only implementation very specific to PMatrix 
    (and not valid for general matrices, e.g. like decomposition) 
    should be implemented here. 
    @author Jan Woetzel
    @status alpha (02/26/2002) **/

  class BIASGeometryBase_EXPORT PMatrixBase : public Matrix3x4<PMATRIX_TYPE> {

  public:

    /** default constructor **/
    inline PMatrixBase() : Matrix3x4<PMATRIX_TYPE>()
    {}

    /** constructor setting identity or zero
        @brief author koeser **/
    explicit inline PMatrixBase(const MatrixInitType& i)
      : Matrix3x4<PMATRIX_TYPE>(i)
    {}

    explicit inline PMatrixBase(const std::string & s)
      : Matrix3x4<PMATRIX_TYPE>(s)
    {}

    /** @author Jan Woetzel **/
    inline PMatrixBase(const PMatrixBase & A) : Matrix3x4<PMATRIX_TYPE>(A) 
    {}

    /** @author Jan Woetzel **/
    inline PMatrixBase(const Matrix3x4<PMATRIX_TYPE> & Amat_ ) 
      : Matrix3x4<PMATRIX_TYPE>(Amat_) 
    {}

    /** @author Jan Woetzel
        not fine, should no be here, better cast stepwise between direct 
        father and child **/
    explicit inline PMatrixBase(const Matrix<PMATRIX_TYPE>& Amat);
    
    /** Take R, K, C and compose PMatrixBase
        @author Jan-Friso Evers-Senne
        @date 2002-08-20 */
    inline
    PMatrixBase(const Matrix3x3<PMATRIX_TYPE> &K, 
                const Matrix3x3<PMATRIX_TYPE> &R,
                const Vector3<PMATRIX_TYPE> &C);
    
    /** assume K=Identity and compose P
        @author woelk 08/2004 */
    inline PMatrixBase(const Matrix3x3<PMATRIX_TYPE> &R,
                       const Vector3<PMATRIX_TYPE> &C);
    
    /** destructor **/
    virtual inline ~PMatrixBase()
    {}

    /** 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
        @status alpha (02/25/2002)  **/
    PMatrixBase &newsize(int rows, int cols);

    inline PMatrixBase& operator=(const PMatrixBase &mat);

    /** Convert the PMatrixBase to 'identity' matrix, overriding the Matrix3x4 
        method:
        1 0 0 0
        0 1 0 0
        0 0 1 0
        @author Ingo Thomsen
        @date 04/11/2002
        @status tested **/
    inline void set_identity();

    /** Sets P-Matrix from a vector which contains the elements of P 
        row wise.*/
    void SetFromVector(const Vector<PMATRIX_TYPE>& vals);

    /** 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<PMATRIX_TYPE> &K, 
                 const Matrix3x3<PMATRIX_TYPE> &R,
                 const Vector3<PMATRIX_TYPE> &C);
    
    void Compose(const Matrix3x3<PMATRIX_TYPE> &K, 
                 const Matrix3x3<PMATRIX_TYPE> &R,
                 const HomgPoint3D &C);

    /// calibrated case, uses K=Identity
    void Compose(const Matrix3x3<PMATRIX_TYPE> &R,
                 const Vector3<PMATRIX_TYPE> &C);

    /// calibrated case, uses K=Identity
    void Compose(const Matrix3x3<PMATRIX_TYPE> &R,
                 const HomgPoint3D &C);
    
    /// description of supported parametrization types
    enum E_ParametrizationType { AXIS_TIMES_ANGLE, AXIS_AND_ANGLE,
                                 EULER, QUATERNION };
      
    /** Compose a projection matrix using a parametrization. The supported 
        parametrization vector always starts with the camera center followed
        by the camera orientation. Currently Euler angles, 
        axis (norm 1) and angle, axis times angle and quaternion are supported
        as orientation parametrization.
        @author woelk 02/2006 */
    void Compose(const Matrix3x3<PMATRIX_TYPE> &K, 
                 const Vector<double>& parametrization,
                 const enum E_ParametrizationType param_type);

    /** Back projects image point x to 3D space and returns ray on which the 3D
        space point lies.
        @note Inverted matrix is computed on each call.
        @author jmf */
    HomgPoint3D Backproject(const HomgPoint2D& x, double mu=1);

    /** Load a Daimler .bog fiel and compose a P matrix from it.
        @author Jan Woetzel 03/2005 */
    int LoadBOG(const std::string & filename);

  protected:
    /// @author woelk 03/2006
    RMatrixBase Parametrization2R_(const Vector<double>& parametrization,
        const enum E_ParametrizationType param_type)const;

 }; // class PMatrixBase
  
  ////////////////////////////////////////////////////////////////////////
  //  implementation
  ////////////////////////////////////////////////////////////////////////

  inline
  PMatrixBase::PMatrixBase(const Matrix3x3<PMATRIX_TYPE> &K,
                           const Matrix3x3<PMATRIX_TYPE> &R,
                           const Vector3<PMATRIX_TYPE> &C)
  {
    Compose(K, R, C);
  }

  inline PMatrixBase::PMatrixBase(const Matrix3x3<PMATRIX_TYPE> &R,
                                  const Vector3<PMATRIX_TYPE> &C)
  { Compose(R, C); }

  inline void PMatrixBase::set_identity(){
    register PMATRIX_TYPE* d = GetData();
    d[0] = d[5] = d[10] = 1;
    d[1] = d[2] = d[3] = d[4] = d[6] = d[7] = d[8] = d[9] = d[11] = 0;
  }

  inline PMatrixBase::PMatrixBase(const Matrix<PMATRIX_TYPE>& Amat)  
  {
    if ((Amat.num_rows()==3) && (Amat.num_cols()==4)) {
      //(is slow, better use memcopy or even better 'grandpa'-constructor.
      for (int index=0; index<12; index++ ) {
        this->GetData()[index] = Amat.GetData()[index]; 
      };
    } else {
      BIASERR(" wrong size in constructor! Amat is a "<<Amat.num_rows()
             <<" x "<<Amat.num_cols()<<" matrix but must be 3x4."
             <<std::endl );
      exit(1);
    };
  }

  inline  PMatrixBase& PMatrixBase::operator=(const PMatrixBase &mat) {
    BIAS::Matrix<PMATRIX_TYPE>::operator=(mat);
    return *this;
  }

} // end namespace 

#endif // _BIAS_PMatrixBase_hh_