FMatrixEstimation.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 "FMatrixEstimation.hh"
#include "MathAlgo/SVD.hh"

#include "MathAlgo/Minpack.hh"
#include "Base/Geometry/KMatrix.hh"
#include "PMatrixEstimation.hh"

#include <Base/Common/CompareFloatingPoint.hh>

#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;

int
FErrorFunction_(void *p,int m, int n, const double *x, double *fvec, int iflag) {
    FMatrixEstimation *pCurrentObject = (FMatrixEstimation*)p;
    Parametrization *Para = pCurrentObject->currentPara_;

    vector<double> params;
  params.reserve(7);
  for (unsigned int i=0; i<7; i++){
    params.push_back(x[i]);
//    cout<<"Param:"<<x[i]<<endl;
  }
  //construct FMatrix from parameters
  BIAS::FMatrix F;
  Para->ParamsToFMatrix(F, params);
//  cout<<"F:"<<F<<endl;
  F = FMatrix(pCurrentObject->NormF2_.Transpose() * F * pCurrentObject->NormF1_);

  const unsigned int uiDataSize = pCurrentObject->p1_->size();
  const vector<BIAS::HomgPoint2D> &p1 = *pCurrentObject->p1_;
  const vector<BIAS::HomgPoint2D> &p2 = *pCurrentObject->p2_;


  HomgPoint2D epi1, epi2;
  F.GetEpipolesHomogenized(epi1, epi2);
  F.Normalize();

  double error = 0.0;

  for (unsigned int j=0; j<uiDataSize; j++) {
    fvec[j] = F.GetEpipolarErrorHomogenized(p1[j], p2[j]);
    error += fvec[j];
  }

//   cout << "f error " << error << endl;
  return 0;
}

int
FGoldStandardErrorFunction_(void *p,int m, int n, const double *x, double *fvec, int iflag) {

  // increase iteration counter
    FMatrixEstimation* pCurrentObject = (FMatrixEstimation*)p;

  PMatrix P1(MatrixIdentity);

  // rebuild P2
  PMatrix P2(MatrixIdentity);
  // write params from x into P2
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 4; j++) {
      P2[i][j] = x[i*4 + j];
    }
  }

  // get correspondences
  const unsigned int uiDataSize = pCurrentObject->p1_->size();
  const std::vector<BIAS::HomgPoint2D> &p1 = *pCurrentObject->p1_;
  const std::vector<BIAS::HomgPoint2D> &p2 = *pCurrentObject->p2_;

  // rebuild 3D points
  vector<HomgPoint3D> points3D;
  for (unsigned int i = 0; i < uiDataSize; i++) {
    points3D.push_back(HomgPoint3D(x[12 + i*3], x[12 + i*3 + 1], x[12 + i*3 + 2], 1.0));
  }

  // compute cost function
  HomgPoint2D p1Est;
  HomgPoint2D p2Est;
  double error1x, error2x, error1y, error2y;
  HomgPoint2D p1Temp, p2Temp;

  for (unsigned int i = 0; i < uiDataSize; i++) {
    // project points
    p1Est = P1 * points3D[i];
    p1Est.Homogenize();
    p2Est = P2 * points3D[i];
    p2Est.Homogenize();

    error1x = (p1Est[0] - p1[i][0]) * (p1Est[0] - p1[i][0]);
    error1y = (p1Est[1] - p1[i][1]) * (p1Est[1] - p1[i][1]);
    error2x = (p2Est[0] - p2[i][0]) * (p2Est[0] - p2[i][0]);
    error2y = (p2Est[1] - p2[i][1]) * (p2Est[1] - p2[i][1]);

    fvec[i*4 ] = error1x;
    fvec[i*4+1] = error1y;
    fvec[i*4+2] = error2x;
    fvec[i*4+3] = error2y;
  }
  return 0;
}

int FMatrixEstimation::SevenPoint(std::vector<BIAS::FMatrix>& vecF,
                                  const std::vector<BIAS::HomgPoint2D> &p1,
                                  const std::vector<BIAS::HomgPoint2D> &p2) {
#ifdef BIAS_DEBUG
  if ((p1.size()!=7) || (p2.size()!=7)) {
    BIASERR("Must provide 7 point correspondences for 7-Point algorithm"
        <<"p1.size="<<p1.size()<<"p2.size="<<p2.size());
    BIASABORT;
  }
#endif

  const std::vector<BIAS::HomgPoint2D> *pp1 = &p1;
  const std::vector<BIAS::HomgPoint2D> *pp2 = &p2;
  std::vector<BIAS::HomgPoint2D> p1norm;
  std::vector<BIAS::HomgPoint2D> p2norm;
  KMatrix K1, K2;
  if (NormalizeHartley_) {
    // for (unsigned int i=0; i<7; i++) cout <<" " <<p1[i];
    N_.Hartley(p1, p1norm, K1);
    // for (unsigned int i=0; i<7; i++) cout <<" " <<p2[i];
    N_.Hartley(p2, p2norm, K2);
    pp1 = &p1norm;
    pp2 = &p2norm;
  }

  // determine rank of equation system
  BIAS::Matrix<double> A(7, 9);

  for (unsigned int i=0; i<7; i++) {
    // run over all rows of A (and all point correspondences)
    A[i][0] = (*pp1)[i][0] * (*pp2)[i][0];
    A[i][1] = (*pp1)[i][1] * (*pp2)[i][0];
    A[i][2] = (*pp2)[i][0];
    A[i][3] = (*pp1)[i][0] * (*pp2)[i][1];
    A[i][4] = (*pp1)[i][1] * (*pp2)[i][1];
    A[i][5] = (*pp2)[i][1];
    A[i][6] = (*pp1)[i][0];
    A[i][7] = (*pp1)[i][1];
    A[i][8] = 1;
  }

  BIAS::SVD A_SVD(A);
  Vector<double> f1(9), f2(9);
  if (!A_SVD.GetNullvector(f1, 0)) {
    BIASERR("Error retrieving first F !");
  }
  if (!A_SVD.GetNullvector(f2, 1)) {
    BIASERR("Error retrieving second F !");
  }

  // f1 and f2 span a 2dimensional null space of A
  // find matrix with rank<3 in this subspace:
  // impose constraint that det(alpha*f1+(1-alpha)*f2)=0)
  std::vector<POLYNOMIALSOLVE_TYPE> AlphaPolynomial, Alphas;
  GetDetPolynomial(f1, f2, AlphaPolynomial);

  // solve for alpha
  PolynomialSolve Solver;
  Solver.CheckCoefficients(AlphaPolynomial);
  const unsigned int uiNumberSolutions = Solver.Analytic(AlphaPolynomial, Alphas);

  vecF.resize(uiNumberSolutions);
  if (uiNumberSolutions==0)
    return -1;

  // compute (1 or 3) solutions from weighting with alphas
  for (unsigned int i = 0; i<uiNumberSolutions; i++) {
    vecF[i].SetFromVector(f1*Alphas[i] + f2*(1.0-Alphas[i]));
    if (NormalizeHartley_)
      vecF[i] = K2.Transpose() * vecF[i] * K1;
  }

  return 0;
}

int FMatrixEstimation::Linear(BIAS::FMatrix& F, const std::vector<BIAS::HomgPoint2D> &p1,
                              const std::vector<BIAS::HomgPoint2D> &p2) {

  const std::vector<BIAS::HomgPoint2D> *pp1 = &p1;
  const std::vector<BIAS::HomgPoint2D> *pp2 = &p2;
  std::vector<BIAS::HomgPoint2D> p1norm;
  std::vector<BIAS::HomgPoint2D> p2norm;
  KMatrix K1, K2;
  if (NormalizeHartley_) {
    N_.Hartley(p1, p1norm, K1);
    N_.Hartley(p2, p2norm, K2);
    pp1 = &p1norm;
    pp2 = &p2norm;
  }

  // determine rank of equation system
  BIAS::Matrix<double> A(p1.size(), 9);

  for (unsigned int i=0; i<p1.size(); i++) {
    // run over all rows of A (and all point correspondences)
    A[i][0] = (*pp1)[i][0] * (*pp2)[i][0];
    A[i][1] = (*pp1)[i][1] * (*pp2)[i][0];
    A[i][2] = (*pp2)[i][0];
    A[i][3] = (*pp1)[i][0] * (*pp2)[i][1];
    A[i][4] = (*pp1)[i][1] * (*pp2)[i][1];
    A[i][5] = (*pp2)[i][1];
    A[i][6] = (*pp1)[i][0];
    A[i][7] = (*pp1)[i][1];
    A[i][8] = 1;
    //  cout<<"P1:"<<p1[i]<<p2[i]<<endl;
  }

  BIAS::SVD A_SVD(A);
  //  cout<<"Linear FEst S:"<<A_SVD.GetS()<<endl;

  // get decomposition of F matrix
  BIAS::Matrix<double> U, VT;
  BIAS::Matrix<double> S(3, 3, 0.0);
  BIAS::Vector<double> s;

  // solve equation system and compute least squares F usually with rank 3
  VT = A_SVD.GetVT();

  for (unsigned int i=0; i<9; i++)
    F[i/3][i%3] = VT[8][i];

  // re-add hartley normalization, so that f is valid for original coordinates
  if (NormalizeHartley_)
    F = K2.Transpose() * F * K1;

  // now F is a matrix probably of rank 3, limit to rank 2:
  EnforceMaxRank2(F);

  return 0;
}

int FMatrixEstimation::Optimize(BIAS::FMatrix& F, const std::vector<BIAS::HomgPoint2D> &p1,
                                const std::vector<BIAS::HomgPoint2D> &p2) {
#ifdef BIAS_DEBUG
  if (p1.size()!=p2.size()) {
    BIASERR("Vectors of corresponding points have different sizes !!!");
    BIASABORT;
  }
  if (p1.size()<7) {
    BIASERR("Too few points to optimize: "<<p1.size());
    return -1;
  }
#endif

  // we need to call an external function from levenberg-marquardt,
  // prepare data structs
  // warning: this section is not thread safe !
  p1_ = &p1;
  p2_ = &p2;

  HomgPoint2D epi1, epi2;
  F.Normalize();
  F.GetEpipolesHomogenized(epi1, epi2);

//  cout << "initial F " << F << endl;
//  cout << "optimize Initial F " << epi1 << " " << epi2 << " " << F.GetResidualError(p1, p2) << endl;

  FMatrix F_normed;
  ComputeNormFParam(F, F_normed);

    vector<double> vecFParamsStart, vecFParamsResult;
    Vector<double> FParamsStart(7), FParamsResult(7);
    currentPara_ = new Parametrization;
    currentPara_->FMatrixToParams(F_normed, vecFParamsStart);
    for (unsigned int i=0; i<7; i++)
      FParamsStart[i] = vecFParamsStart[i];
    int res;
    if ((res = LevenbergMarquardt(&FErrorFunction_, this,
                                  p1.size(), 7, FParamsStart, FParamsResult,
                                  MINPACK_DEF_TOLERANCE))!=0) {
      // BIASERR("Error in Levenberg-Marquardt-Optimization of F: "<<res);
      return -1;
    }
    vecFParamsResult.resize(7);
    for (unsigned int i=0; i<7; i++)
      vecFParamsResult[i] = FParamsResult[i];
    currentPara_->ParamsToFMatrix(F, vecFParamsResult);
    delete currentPara_;
    currentPara_ = NULL;
#ifdef BIAS_DEBUG
    p1_ = NULL;
    p2_ = NULL;
#endif

   F = FMatrix(NormF2_.Transpose() * F * NormF1_);

  return 0;
}

int FMatrixEstimation::GoldStandard(BIAS::FMatrix& F, const std::vector<BIAS::HomgPoint2D> &p1,
                                    const std::vector<BIAS::HomgPoint2D> &p2) {

  NormalizeHartley_ = true;

  int p1Size = p1.size();
  BIASASSERT(p1Size == (int) p2.size());
  BIASASSERT(p1Size >= 8);

  //  std::vector<BIAS::HomgPoint2D> p1_15;
  //  std::vector<BIAS::HomgPoint2D> p2_15;
  std::vector<BIAS::HomgPoint2D> p1_rest;
  std::vector<BIAS::HomgPoint2D> p2_rest;

  for (int i = 0; i < p1Size; i++) {
    if (i < 15) {
      //      p1_15.push_back(p1[i]);
      //      p2_15.push_back(p2[i]);
      p1_rest.push_back(p1[i]);
      p2_rest.push_back(p2[i]);
    } else {
      p1_rest.push_back(p1[i]);
      p2_rest.push_back(p2[i]);
    }
  }

  // initialize F
  Linear(F, p1, p2);
  //  std::cout << "Estimated F matrix " << 1/F[2][2] * F << std::endl;

  // initialize projection matrices with P1 = [I|0] and P2 = [[e']x F| e']
  PMatrix P1, P2;
  P1.SetIdentity();
  P2.SetIdentity();

  HomgPoint2D epipole1, epipole2;
  F.GetEpipoles(epipole1, epipole2);
  if (epipole1[1]!=0.0)
    epipole1.Normalize();
  if (epipole2[1]!=0.0)
    epipole2.Normalize();
  //  cout << "est. epipoles are "<<  epipole1 <<" and "<<epipole2 <<endl;

  Matrix3x3<double> mMatrix(MatrixIdentity);
  mMatrix = (Matrix3x3<double>)epipole2.GetSkewSymmetricMatrix() * F;
  for (int i=0; i<3; i++) {
    for (int j=0; j<3; j++) {
      P2[i][j] = mMatrix[i][j];
    }
  }

  P2.SetCol(3, epipole2);

  // use correspondences p1 and p2 and F to triangulate 3D points
  vector<HomgPoint3D> points3D;
  Triangulation triangulator;
  HomgPoint3D tempPoint3D;
  for (int i = 0; i < p1Size; i++) {
    triangulator.Optimal(P1, P2, p1_rest[i], p2_rest[i], tempPoint3D);
    tempPoint3D.Homogenize();
    points3D.push_back(tempPoint3D);
  }

  // optimize 3D points and all 12 entries of P2

  // we need to call an external function from levenberg-marquardt,
  // prepare data structs
  // warning: this section is not thread safe !
  p1_ = &p1;
  p2_ = &p2;

  // build vector containing parameters to be optimized
  // the first 12 positions contain the matrix P2, the others contain the 3D points
  // [i+1], [i+2], [i+3]
  Vector<double> FParamsStart(12 + 3*p1Size), FParamsResult(12 + 3*p1Size);

  // write P2 into FParamsStart
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 4; j++) {
      FParamsStart[i*4 + j] = P2[i][j];
    }
  }

  for (int i = 0; i < p1Size; i++) {
    if (!points3D[i].IsHomogenized())
      points3D[i].Homogenize();
    FParamsStart[12 + i*3 ] = (points3D[i])[0];
    FParamsStart[12 + i*3 + 1] = (points3D[i])[1];
    FParamsStart[12 + i*3 + 2] = (points3D[i])[2];
  }

  int res;
  itersLM_ = 0;
  if ((res = LevenbergMarquardt(&FGoldStandardErrorFunction_,
                                this, 4*p1Size, 12 + 3*p1Size, FParamsStart,
                                FParamsResult, MINPACK_DEF_TOLERANCE))!=0) {
    BIASERR("Error in Levenberg-Marquardt-Optimization: "<<res);
    return res;
  }

  // rebuilding P2
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 4; j++) {
      P2[i][j]= FParamsResult[i*4 + j];
    }
  }

  P1.SetIdentity();

  // rebuilding F
  epipole2[0] = FParamsResult[3];
  epipole2[1] = FParamsResult[7];
  epipole2[2] = FParamsResult[11];

  for (int i=0; i<3; i++) {
    for (int j=0; j<3; j++) {
      mMatrix[i][j] = FParamsResult[i*4 + j];
    }
  }

  F = (Matrix3x3<double>)epipole2.GetSkewSymmetricMatrix() * mMatrix;

  return 0;
}

void FMatrixEstimation::GetDetPolynomial(const BIAS::Vector<double> &F1,
                                         const BIAS::Vector<double> &F2,
                                         std::vector<POLYNOMIALSOLVE_TYPE> &v) {
#ifdef BIAS_DEBUG
  if (F1.size()!=9) {
    BIASERR("Vector has wrong size "<<F1.size());
    BIASABORT;
  }
  if (F2.size()!=9) {
    BIASERR("Vector has wrong size "<<F2.size());
    BIASABORT;
  }
  if (v.size()!=0) {
    BIASERR("Vector v is not empty: "<<v.size());
    BIASABORT;
  }
#endif

  // We have two FMatrices in vector form F1, F2.
  // We want to find a linear combination of these two which has rank 2

  // set up determinant equation using sarrus' rule for 3x3 matrices
  // where our combined matrix F = alpha F1 + (1-alpha) F2
  // we are looking for alpha, such that det(F)=0

  // first we need the differences
  const double d[9] = { (F1[0]-F2[0]), (F1[1]-F2[1]), (F1[2]-F2[2]), (F1[3]-F2[3]), (F1[4]-F2[4]),
      (F1[5]-F2[5]), (F1[6]-F2[6]), (F1[7]-F2[7]), (F1[8]-F2[8]) };

  // init coeffs with zero
  v.resize(4, 0);

  // accessing matrix according to sarrus' rule
  // better compute the determinant using short code from MAPLE or something
  // like that, as it is done in v_x_l::FMatrixCompute7Point
  // this here works definitely, but is quite slow
  unsigned int x, y, z;
  for (unsigned int i=0; i<3; i++) {
    // + diagonal from top to bottom
    x = i;
    y = 3 + ((i+1)%3);
    z = 6 + ((i+2)%3);
    v[3] += d[x] * d[y] * d[z];
    v[2] += d[x] * d[y] * F2[z] + d[x] * F2[y] * d[z] + F2[x] * d[y] * d[z];
    v[1] += d[x] * F2[y] * F2[z] + F2[x] * d[y] * F2[z] + F2[x] * F2[y] * d[z];
    v[0] += F2[x] * F2[y] * F2[z];

    // - diagonal from bottom to top
    x = 6+i;
    // y is the same
    z = (i+2)%3;
    v[3] -= d[x] * d[y] * d[z];
    v[2] -= d[x] * d[y] * F2[z] + d[x] * F2[y] * d[z] + F2[x] * d[y] * d[z];
    v[1] -= d[x] * F2[y] * F2[z] + F2[x] * d[y] * F2[z] + F2[x] * F2[y] * d[z];
    v[0] -= F2[x] * F2[y] * F2[z];
  }
}

void FMatrixEstimation::EnforceMaxRank2(BIAS::FMatrix &F) {
  // get decomposition of F matrix
  BIAS::Matrix<double> U, VT;
  BIAS::Matrix<double> S(3, 3, 0.0);
  BIAS::Vector<double> s;
  // enforce rank 2 of F using SVD
  BIAS::SVD F_SVD((Matrix<FMATRIX_TYPE>)F);

  // get singular values of F
  s = F_SVD.GetS();

  // compose S matrix
  S[0][0] = s[0];
  S[1][1] = s[1];
  // leave smallest singular value at zero

  U = F_SVD.GetU();
  VT = F_SVD.GetVT();

  // recompose F
  F = U * S * VT;
}


/**
 * rank 2 F matrix parametrization as implemented in Parametrization does not work in
 * some cases, eg. when epipoles are at infinity. This normalizatin as described in
 * Estimating the fundamental matrix by transforming image points in projective space
 * by Zhang and Loop
 * Application of this function can be seen in Optimize
 */
void FMatrixEstimation::ComputeNormFParam(const BIAS::FMatrix& initialF, BIAS::FMatrix& normF){

 // cout << "initial F " << initialF << endl;

  // compute epipoles for initial matrix
  HomgPoint2D epi1, epi2;
  initialF.GetEpipoles(epi1, epi2);

//  cout << "epipoles " << epi1 << " " << epi2 << endl;

  // step 1 in paper
  // initialize normalization matrices with identity
  NormF1_.SetIdentity();
  NormF2_.SetIdentity();

  // step 2 in paper
  // find largest element in initial F matrix
  double largest = 0.0;
  int indexRow = 0;
  int indexCol = 0;
  for(int i = 0; i < 3; i++){
    for(int j = 0; j < 3; j++){
      if(initialF[i][j] > largest){
        largest = initialF[i][j];
        indexRow = i;
        indexCol = j;
      }
    }
  }

//  cout << "largest elem " << largest << " " << indexRow << " " << indexCol << endl;


  Vector3<double> row1, row2;
  double ep1, ep2;

  // step 3 in paper - permutation
  if(indexCol != 0){
    row1 = NormF1_.GetRow(0);
    row2 = NormF1_.GetRow(indexCol);
    NormF1_.SetRow(0, row2);
    NormF1_.SetRow(indexCol, row1);
    ep1 = epi1[0];
    ep2 = epi1[indexCol];
    epi1[indexCol] = ep1;
    epi1[0] = ep2;
  }

  // step 4 in paper - permutation
  if(indexRow != 0){
    row1 = NormF2_.GetRow(0);
    row2 = NormF2_.GetRow(indexRow);
    NormF2_.SetRow(0, row2);
    NormF2_.SetRow(indexRow, row1);
    ep1 = epi2[0];
    ep2 = epi2[indexRow];
    epi2[indexRow] = ep1;
    epi2[0] = ep2;
  }

  // step 5 in paper - permutation
  if(fabs((float)epi1[1]) > fabs((float)epi1[2])){
    row1 = NormF1_.GetRow(1);
    row2 = NormF1_.GetRow(2);
    NormF1_.SetRow(1, row2);
    NormF1_.SetRow(2, row1);
    ep1 = epi1[1];
    ep2 = epi1[2];
    epi1[2] = ep1;
    epi1[1] = ep2;
  }

  // step 5 in paper - permutation
  if(fabs((float)epi2[1]) > fabs((float)epi2[2])){
    row1 = NormF2_.GetRow(1);
    row2 = NormF2_.GetRow(2);
    NormF2_.SetRow(1, row2);
    NormF2_.SetRow(2, row1);
    ep1 = epi2[1];
    ep2 = epi2[2];
    epi2[1] = ep1;
    epi2[2] = ep2;
  }

 // cout << "resulting epipoles " << epi1 << " " << epi2 << endl;

//  cout << "normalization matrices " << NormF1_ << " " << NormF2_ << endl;

  normF = FMatrix(NormF2_ * initialF * NormF1_.Transpose());

//  cout << "norm F " << normF << endl;
 normF.GetEpipoles(epi1, epi2);
// cout << "epipoles " << epi1 << " " << epi2 << endl;

}