FMatrix.cpp

/*
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
*/


#include "FMatrix.hh"
#include "Base/Geometry/KMatrix.hh"
#include <MathAlgo/SVD.hh>
#include <Base/Math/Vector3.hh>

using namespace BIAS;
using namespace std;


#define F_SVD_THRESH 0.1


void FMatrix::ComputeFromPMatrices(BIAS::PMatrix &P1,
                                   BIAS::PMatrix &P2) {

  // invert P1 to get backprojected rays of coordinates
  BIAS::Matrix<double> P1Inv;
  P1.GetPseudoInverse(P1Inv);

  // get center1, which is the null vector of P1
  HomgPoint3D C1;
  P1.GetNullVector(C1);

  // get epipol in image 2
  BIAS::HomgPoint2D e(P2 * C1);

  // get cross product matrix of epipol
  BIAS::Matrix3x3<double> e_x(e.GetSkewSymmetricMatrix());

  // compose all
  (*this) = Matrix3x3<double>(e_x * P2 * P1Inv);

  double maxentry = 0;
  for (unsigned int i=0; i<3; i++) {
    for (unsigned int j=0; j<3; j++) {
      if (fabs((*this)[i][j])>maxentry) maxentry = fabs((*this)[i][j]);
    }
  }
  if (maxentry>0.0) (*this) *= 1.0 / maxentry;
}



int FMatrix::GetEpipoles(HomgPoint2D &Epipole1, HomgPoint2D &Epipole2) const
{
  SVD svdF ( (Matrix<FMATRIX_TYPE>)(*this), F_SVD_THRESH );
#ifdef BIAS_DEBUG
  if ((GetDebugLevel() & D_FMATRIX_GETEPIPOLES)!=0){
    BIAS::FMatrix FCopy = (*this);
    cerr << "F: " << FCopy;
    Matrix<double> S(3,3);
    S.SetIdentity();
    for (int i=0; i<3; i++)
      S[i][i]=svdF.GetS()[i];

    cerr << "U:" << svdF.GetU() << "\tS: " << svdF.GetS() << "\tV: "
         << svdF.GetVT() << "\n U S Vt:"
         << svdF.GetU() * S * svdF.GetVT() << "\nF:" << FCopy << endl;
    cerr << "NullspaceDim(): " << svdF.NullspaceDim() << endl;
  }
#endif
  Vector<double> Ep1, Ep2;
  if (!svdF.GetNullvector(Ep1)) {
    Epipole1.Set(0,0,0);
    return -1;
  }
  Epipole1 = Ep1;
  Epipole1.Normalize();
  if (!svdF.GetLeftNullvector(Ep2)) {
    Epipole2.Set(0,0,0);
    return -2;
  }
  Epipole2 = Ep2;
  Epipole2.Normalize();
  return 0;
}

/*-----------------------------------------------------------------
  decompose a f matrix to (skew)(rank3).
  ----------------------------------------*/
void FMatrix::DecomposetoSR(BIAS::Matrix3x3<double> &skew_matrix,
                            BIAS::Matrix3x3<double> &rank3_matrix)

{
  HomgPoint2D Epipole1, Epipole2;
  GetEpipoles(Epipole1, Epipole2);

  //ma2_static_trivec_to_crossmatrix (&epipole2_trivec, cross_matrix);
  skew_matrix.SetAsCrossProductMatrix(Epipole2);

  //ma2_static_multiply_3x3_3x3 (cross_matrix, f_matrix, rank3_matrix);
  rank3_matrix=skew_matrix*(*this);
}

FMatrix& FMatrix::operator=(const FMatrix& f)
{
  FMatrixBase::operator=(f);
  return *this;
}


bool FMatrix::IsValid()
{
  /* result is true by default.
     The wrong cases should be intercepted by if-clauses.
     The first one that matches permanently sets the result to false.
     mdunda 08 2003 */
  bool result=true;

  SVD svdF ( Matrix<FMATRIX_TYPE>(*this) );

  // The nullspace dimension should be 1. In that case the rank is 2.
  int dimN=svdF.NullspaceDim();
  if ( dimN != 1 )
    {
      BIASERR("F-Matrix invalid due to wrong nullspace dimension."
          << "dim(N)=" << dimN);
      result=false;
    }
  return result;
}

double FMatrix::GetResidualError(const std::vector<BIAS::HomgPoint2D> &p1,
                                 const std::vector<BIAS::HomgPoint2D> &p2){


  int numPoints = std::min(p1.size(), p2.size());
  double res = 0;
  HomgPoint2D temp2D_1, temp2D_2;
//  double temp;
  for(int i =0; i < numPoints; i++){
    temp2D_1 = p1[i];
    temp2D_2 = p2[i];
//    temp = GetEpipolarError(temp2D_1, temp2D_2);
    res += GetEpipolarError(temp2D_1, temp2D_2);

  }
  res = res / numPoints;
  return res;
}