FMatrixBase.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_FMatrixBase_hh_
#define _BIAS_FMatrixBase_hh_

#include <bias_config.h>

#include <math.h>

#include <Base/Debug/Error.hh>
#include <Base/Debug/Debug.hh>

#include <Base/Math/Matrix3x3.hh>
#include <Base/Math/Vector3.hh>

#include "RMatrixBase.hh"
#include "KMatrix.hh"
#include "HomgPoint2D.hh"
#include "HomgLine2D.hh"


#define FMATRIX_TYPE double
namespace BIAS {

  /** @class FMatrixBase 
      @ingroup g_geometry
      @brief describes the epipolar relationship between a point and his 
      mapping on a corresponding epipolar line.
    
      The 3x3 matrix F is the fundamental matrix of an image pair (b1, b2). 
      In particular, with p' = transpose(p) this results in the epipolar 
      constraint: 
      p1' F p2 = 0 for every pair (p1, p2) of corresponding image points in 
      b1 and b2 respectively.

      Only implementation very specific to FMatrixBase (and not valid for a 
      general 3x3 matrix) should be implemented here.
      @author Jan Woetzel
      @status untested (04/18/2002) now derived from Matrix3x3 and not 
      directly from Matrix<double>
      status alpha (02/26/2002)
    **/

  class BIASGeometryBase_EXPORT FMatrixBase : public Matrix3x3<FMATRIX_TYPE> {

  public:

    /** destructor
        @status untested (04/18/2002) **/
    ~FMatrixBase();

    /** default constructor
        @status untested (04/18/2002)**/
    FMatrixBase() : Matrix3x3<FMATRIX_TYPE>() { _isDecomposed=false; };

    /** constructor setting identity or zero
        @brief author koeser **/
    explicit FMatrixBase(const MatrixInitType& i) : 
      Matrix3x3<FMATRIX_TYPE>(i) {_isDecomposed=false; };

    // DEPRECATED due to instantiation problem JW
    //explicit FMatrixBase(const char *s) : Matrix3x3<FMATRIX_TYPE>(s) { _isDecomposed=false; };

    /** @author Jan Woetzel
        @status untested (04/18/2002) **/
    FMatrixBase(const Matrix3x3<FMATRIX_TYPE>& A) 
      : Matrix3x3<FMATRIX_TYPE>(A) 
    { _isDecomposed=false; };

    /** constructor
        for convenience to build form a c-style aaray amtrix 
        @author Jan Woetzel (07/15/2002)  **/
     explicit FMatrixBase(const FMATRIX_TYPE *d) {
      // get F from the c-style roww-wise data array d
      for (int i=0; i<9;i++) {
        (this->GetData())[i] = d[i];
      };
      _isDecomposed=false; 
    };

    /// see Set(Vector3<FMATRIX_TYPE>& epipole, RMatixBase& R);
    inline FMatrixBase(const Vector3<FMATRIX_TYPE>& epipole, 
                       const RMatrixBase& R)
      : Matrix3x3<FMATRIX_TYPE>() 
    { Set(epipole, R); }

    /// see Set(Vector3<FMATRIX_TYPE>& epipole, 
    ///         Vector3<FMATRIX_TYPE>& EulerAngXYZ);
    inline FMatrixBase(const Vector3<FMATRIX_TYPE>& epipole, 
                       const Vector3<FMATRIX_TYPE>& EulerAngXYZ)
      : Matrix3x3<FMATRIX_TYPE>() 
    { Set(epipole, EulerAngXYZ); };

    /** assignment operators
        calling corresponding operator from base class "TNT::Matrix" 
        if appropriate
        @author Jan Woetzel
        @status untested (04/18/2002) 
        status alpha (02/25/2002) **/
    inline FMatrixBase &operator=(const Matrix3x3<FMATRIX_TYPE> &mat) {
      Matrix3x3<FMATRIX_TYPE>::operator=(mat); 
      _isDecomposed=false;
      return *this;
    };

    /** set it from epipole and rotation matrix
        in order to generate essential matrix from image 1 to image 2
        (i.e. generating the epipolar line in image 2, gfiven a point in 
        image 1),  use epipole in second image and rotation matrix from image 1
        to image 2 
        @author woelk 07/2004 */
    inline void Set(const Vector3<FMATRIX_TYPE>& epipole, 
                    const RMatrixBase& R);

    /** see constructor from epipole and rotation matrix
        builds rotation matrix in x, y, z order with moving axis
        see RMatrixBase
        @author woelk 07/2004 */
    inline void Set(const Vector3<FMATRIX_TYPE>& epipole, 
                    const Vector3<FMATRIX_TYPE>& EulerAngXYZ);

    /** set function, it is undocumented if this really works with the
        camer center C, I doubt it, woelk 12/2004 */
    inline void Set(RMatrixBase &R, Vector3<FMATRIX_TYPE> &C);


    /** @brief computes error in epipolar geometry for the given 
        correspondence P1;P2
      
        this error is the sum of squared distances to epipolar lines in both
        images, for inliers it should typically be of one or two pixels
        (if not-normalized pixel coordinates are used)
        @attention P1 and P2 HAVE TO BE homogenized
        @param P1 homogenized first point of correspondence to check
        @param P2 homogenized second point of correspondence to check
        @status tested, optimized
        @author koeser 09/2003 */
     inline double GetEpipolarErrorHomogenized(const BIAS::HomgPoint2D& P1,
                                              const BIAS::HomgPoint2D& P2) const;

    /** @brief computes error in epipolar geometry for the given 
        correspondence P1,P2 using above function after homogenizing the points
        @author koeser 09/2003 */
    inline double GetEpipolarError(BIAS::HomgPoint2D& P1,
                                   BIAS::HomgPoint2D& P2) 
    { P1.Homogenize(); P2.Homogenize(); 
    return GetEpipolarErrorHomogenized(P1,P2);}

    /** returns the cosine of the angle bewteen epipolar line 
        and match line */
    inline double GetCosAngleErrorHomogenized(const BIAS::HomgPoint2D& p1,
                                              const BIAS::HomgPoint2D& p2);

    /** Returns epipolar line in image 1 for point in image 2. */
    HomgLine2D GetEpipolarLineImage1(const HomgPoint2D& point) const ;

    /** Returns epipolar line in image 1 for point in image 2. */
    void GetEpipolarLineImage1(const HomgPoint2D& point,
                               HomgLine2D& homline) const;

    /** Returns epipolar line in image 2 for point in image 1. */
    HomgLine2D GetEpipolarLineImage2(const HomgPoint2D& point) const;

    /** Returns epipolar line in image 2 for point in image 1. */
    void GetEpipolarLineImage2(const HomgPoint2D& point,
                               HomgLine2D& homline) const;

    /** constructs the FMatrix from image 1 to image 2 as K1 [e]_x R K2
        @author woelk 08/2004 */
    void Compose(const KMatrix& K1, const RMatrixBase& R1, 
                 const Vector3<double>& C1, const KMatrix& K2, 
                 const RMatrixBase& R2, const Vector3<double>& C2);
  
    /** constructs the FMatrix from image 1 to image 2 as 
        [e]_x R, assumes K1=K2=Identity()
        @author woelk 08/2004 */
    void Compose(const RMatrixBase& R1, const Vector3<double>& C1, 
                 const RMatrixBase& R2, const Vector3<double>& C2);
  protected:
    // cache for e and r
    bool _isDecomposed;
    HomgPoint2D _epipole;
    RMatrixBase _R;

  }; // class FMatrixBase

  
  ///////////////////////////////////////////////////////////////////////////
  ///              implementation
  ///////////////////////////////////////////////////////////////////////////

  void FMatrixBase::Set(const Vector3<FMATRIX_TYPE>& epipole, 
                        const RMatrixBase& R)
  {
    Matrix3x3<FMATRIX_TYPE> e;
    HomgPoint2D he=epipole; he.Homogenize(); 
    e.SetAsCrossProductMatrix(he);
    e.Mult(R, *this);
    _isDecomposed=true;
    _epipole=he;
    _R=R;
  }
  
  void FMatrixBase::Set(const Vector3<FMATRIX_TYPE>& epipole, 
                        const Vector3<FMATRIX_TYPE>& EulerAngXYZ)
  {
    RMatrixBase R;
    R.SetXYZ(EulerAngXYZ[0], EulerAngXYZ[1], EulerAngXYZ[2]);
    Set(epipole, R);
  }

  void FMatrixBase::Set(RMatrixBase &R, Vector3<FMATRIX_TYPE> &C){
    // We have two matrices, P0 = [id | 0] and P1 = [R^T | -R^TC]
    // Now compute essential matrix.
    // C0 = [0 0 0 1] mapped by P1 yields (-R^TC), the epipole in image 1
    // This must be in the left null space of E.
    // Vice versa C is directly the epipole in image 0 and must be 
    // in the right null space of E

    // this sets the left null space to what we want:
    (*this).SetAsCrossProductMatrix(R.Transpose()*C);
    // Unfortunately, the right null space of (*this) is now also R^T*C, 
    // right-multiplying by R^T corrects for that problem:
    (*this) = (*this)*R.Transpose();
  }

  double FMatrixBase::GetEpipolarErrorHomogenized(const BIAS::HomgPoint2D& P1,
                                                  const BIAS::HomgPoint2D& P2) const
  {
    /*
    // old version, quite slow but readable ;-)
    
    // use F Matrix to get epipolar line in other image
    HomgLine2D EpiLineInImage;
    F.GetEpipolarLineImage1(P2,EpiLineInImage);
    
    // compute (perpendicular) distance of point to line and square it
    double dError1 = EpiLineInImage.DistanceSquared(P1);
    
    // do the same for the inverse direction and add error value
    F.GetEpipolarLineImage2(P1,EpiLineInImage);
    
    double dErrorAll = (dError1 + EpiLineInImage.DistanceSquared(P2));
    return dErrorAll;
    
    */
    HomgLine2D EL;
    
    // computation of epipolar line relying on homogenized point
    const register double *matP = GetData();    
    EL[0] = P2[0] * matP[0]
          + P2[1] * matP[3]
          + matP[6];
    EL[1] = P2[0] * matP[1]
          + P2[1] * matP[4]
          + matP[7];
    EL[2] = P2[0] * matP[2]
          + P2[1] * matP[5]
          + matP[8];    
  
    double dist = EL[0] * P1[0] + EL[1] * P1[1] + EL[2];
    // dist is now = x'Fx (zero for perfect correspondence)

    double dError = dist = dist * dist;
    dError /= (EL[0]*EL[0]+EL[1]*EL[1]);
    // dError contains now the perpendicular distance to epiline in image1

    // do the same for the inverse direction and add error value
    // EL = F * P1:
    EL[0] = matP[0]*P1[0]+ matP[1]*P1[1] + matP[2];
    EL[1] = matP[3]*P1[0]+ matP[4]*P1[1] + matP[5];
    // EL[2] is not needed

    return (dError + (dist / (EL[0]*EL[0]+EL[1]*EL[1])));
 
  }

  /// @todo optimze
  double FMatrixBase::GetCosAngleErrorHomogenized(const BIAS::HomgPoint2D& p1,
                                                  const BIAS::HomgPoint2D& p2)
  {
    BIASASSERT(_isDecomposed);
    double result;
    HomgLine2D l1, l2;
    HomgPoint2D rcp2;
    // l1 is the line between the epipole an p1
    l1.Set(_epipole, p1);
    l1.Homogenize();
    // now apply 'homography' to p2
    _R.Mult(p2, rcp2);
    // l2 is the line between p1 and R*p2
    l2.Set(p1, rcp2);
    l2.Homogenize();
    // now calculate the angle between these two lines
    result=l1[0]*l2[0]+l1[1]*l2[1];
    // crop inaccurate results
    if (result>=1.0) { result=1.0; }
    if (result<=-1.0) { result=-1.0; }
    BIASASSERT(result>=-1.0 && result <=1.0);
    return 1.0-result;
  }

} // namespace 
#endif // FMatrixBase