EMatrix.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 "EMatrix.hh"
#include <Base/Geometry/Quaternion.hh>
#include <limits.h>

using namespace std;
using namespace BIAS;

int EMatrix::
InitFromF(const FMatrixBase &F,  const KMatrix &K1, const KMatrix &K2) 
{
  FMatrix Fnorm;
  
  // normalize 
  double normalization = F.NormFrobenius();
  if (fabs(normalization)>DBL_EPSILON)
    Fnorm = (F / normalization );
  else return -1;
  
  Matrix<double> tmp(K2.Transpose()*Fnorm*K1);
  
  normalization = tmp.NormFrobenius();
  if (fabs(normalization)>DBL_EPSILON)
    Fnorm = tmp / normalization;
  else return -1;

  // compose nearest essential matrix
  SVD Fnorm_svd((Matrix<FMATRIX_TYPE>)Fnorm);
  Vector<double> S(Fnorm_svd.GetS());
  tmp.SetZero();
  tmp[1][1] = tmp[0][0] = 0.5*(S[0]+S[1]);
  tmp[2][2] = 0.0;
  (*this) = Matrix3x3<double>( Fnorm_svd.GetU()*tmp*Fnorm_svd.GetVT() );
  return 0;
}



int EMatrix::
GetRotationTranslation(RMatrix &R, Vector3<double> &C,
                       const std::vector<HomgPoint2D> &inlier1,
                       const std::vector<HomgPoint2D> &inlier2)
{
  //AddDebugLevel(D_EMATRIX_ROT_TRANS);
  BIASCDOUT(D_EMATRIX_ROT_TRANS, "EMatrix::GetRotationTranslation()\n");
  // at first compute possible candidates for base sign and rotation 
  // @see for details see Hartley Zisserman pp. 239
  SVD svd;
  if (svd.Compute((Matrix<FMATRIX_TYPE>)(*this))!=0) return -10;
  Matrix<double> U = svd.GetU();
  Vector3<double> e(U.GetCol(2));
  BIASCDOUT(D_EMATRIX_ROT_TRANS,"epipol =  "<<e<<endl);
#ifdef BIAS_DEBUG
  HomgPoint2D te1, te2;
  this->GetEpipolesHomogenized(te1, te2);
  BIASCDOUT(D_EMATRIX_ROT_TRANS, "epipoles homogenized: te1 = "<<te1
            <<"\tte2 = "<<te2<<endl);
#endif
  Matrix<double> W(3,3); W.SetZero();
  W[0][1] = -1.0; W[1][0] = 1.0; W[2][2] = 1.0;
  
  std::vector<RMatrix> RotCand;
  BIASCDOUT(D_EMATRIX_ROT_TRANS, "- Compute rotation candidates : \n");

  // first candidate
  R = svd.GetU()*W*svd.GetVT();
  R.TransposeIP();  
  RotCand.push_back(R.GetDeterminant()*R);
  BIASCDOUT(D_EMATRIX_ROT_TRANS, "  R[0] = " << RotCand[0]);
  
  // second candidate
  R = svd.GetU()*W.Transpose()*svd.GetVT(); 
  R = R.Transpose();
  RotCand.push_back(R.GetDeterminant()*R);
  BIASCDOUT(D_EMATRIX_ROT_TRANS, "  R[1] = " << RotCand[1]);
#ifdef BIAS_DEBUG
  {
    BIAS::Quaternion<double> tmpQ[2];
    RotCand[0].GetQuaternion(tmpQ[0]);
    RotCand[1].GetQuaternion(tmpQ[1]);
    BIASCDOUT(D_EMATRIX_ROT_TRANS, "  Q[0] = " << tmpQ[0]
              << " (angle = " << tmpQ[0].GetRotationAngle()*180.0/M_PI
              << ", axis = " << tmpQ[0].GetRotationAxis() << endl);
    BIASCDOUT(D_EMATRIX_ROT_TRANS, "  Q[1] = " << tmpQ[1]
              << " (angle = " << tmpQ[1].GetRotationAngle()*180.0/M_PI
              << ", axis = " << tmpQ[1].GetRotationAxis() << endl);
  }
#endif

  // now evaluate the candidates to find the best one
  unsigned int inlier_count, best_inlier_count = 0;
  Triangulation triangulator;
  HomgPoint3D p3d;
  const Vector3<double> C1(0.0, 0.0, 0.0);
  Vector3<double> CCurrent;
  const Quaternion<double> Q1(0.0, 0.0, 0.0, 1.0); //identity
  Quaternion<double> Q2;
  
  for (unsigned int i = 0; i < RotCand.size(); i++)
  {
    for (double base_sign = -1.0; base_sign <= 2.0 ; base_sign += 2.0)
    {
      // since we want to set +/-e as the last column of P (which is -R^T*C)
      // we have to compute a current C that produces this row,
      // therefore we undo R^T by multiplying by R
      CCurrent = Vector3<double>(base_sign*RotCand[i]*e);
      RotCand[i].GetQuaternion(Q2);
      // evaluate inliers
      inlier_count = 0;
      BIAS::Vector3<double> eucl3d;
      for (unsigned int n = 0; n < inlier1.size(); n++) {
        // now run triangulation
        int tri_res =
          triangulator.Triangulate(C1, Q1, CCurrent, Q2,
                                   inlier1[n], inlier2[n], eucl3d);
        p3d.Set(eucl3d);
        if (tri_res == 0) {
          inlier_count++;   
        }
      }
      if (inlier_count > best_inlier_count){
        BIASCDOUT(D_EMATRIX_ROT_TRANS, "- set best inlier count " 
                  << inlier_count << " for R = " << RotCand[i] 
                  << "C = " << CCurrent  << endl);
        best_inlier_count = inlier_count;
        // save the same R and C we used for the test
        R = RotCand[i];
        C = CCurrent;
      } else {
        BIASCDOUT(D_EMATRIX_ROT_TRANS, "- inlier count " << inlier_count
                  << " for R = " << RotCand[i] << "C = " << CCurrent << endl);
      }
    } // loop over base signs
  } // loop over rotation candidates
  
  if (best_inlier_count == 0) {
    BIASERR("No inlier found during triangulation for essential candidates!");
    return -1;
  }
#ifdef BIAS_DEBUG
  else {
    BIAS::Quaternion<double> tmpQ;
    R.GetQuaternion(tmpQ);
    BIASCDOUT(D_EMATRIX_ROT_TRANS, "-> solution is R = " << R
              << "   Q = " << tmpQ
              << " (angle = " << tmpQ.GetRotationAngle()*180.0/M_PI
              << ", axis = " << tmpQ.GetRotationAxis() << endl
              << "   C = " << C << endl);
  }
#endif

  return 0;
}

int EMatrix::
GetRotationTranslation(Quaternion<double> &Q, Vector3<double> &C,
                       const vector<HomgPoint2D> &inlier1,
                       const vector<HomgPoint2D> &inlier2)
{
  RMatrix R;
  int res = GetRotationTranslation(R, C, inlier1, inlier2);
  if (res==0){
    if (R.GetQuaternion(Q)!=0){
      return -20;
    }
  }
  return res;
}