FMatrixBase.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 "FMatrixBase.hh"
#include <Base/Common/W32Compat.hh>
#include "HomgPoint3D.hh"
#include "PMatrixBase.hh"

#include <Base/Common/BIASpragma.hh>


using namespace BIAS;
using namespace std;

FMatrixBase::~FMatrixBase() 
{}


/** Returns epipolar line in image 1 for point in image 2. */
HomgLine2D FMatrixBase::GetEpipolarLineImage1(const HomgPoint2D& point) const
{
   HomgLine2D homline;
   GetEpipolarLineImage1(point,homline);
   return homline;
}

void FMatrixBase::GetEpipolarLineImage1(const HomgPoint2D& point,
                                        HomgLine2D& homline) const {
  
  const register FMATRIX_TYPE *matP = this->GetData();  
  homline[0] = point[0] * matP[0]
      + point[1] * matP[3]
      + point[2] * matP[6];
  homline[1] = point[0] * matP[1]
    + point[1] * matP[4]
    + point[2] * matP[7];
  homline[2] = point[0] * matP[2]
    + point[1] * matP[5]
    + point[2] * matP[8];    
}
  
/** Returns epipolar line in image 2 for point in image 1. */
HomgLine2D FMatrixBase::GetEpipolarLineImage2(const HomgPoint2D& point) const
{
  HomgLine2D homline((*this)*point);
  return homline;    
}
  
void FMatrixBase::GetEpipolarLineImage2(const HomgPoint2D& point,
                                        HomgLine2D& homline) const
{
  homline=HomgLine2D((*this)*point);
}

void FMatrixBase::Compose(const RMatrixBase& R1, const Vector3<double>& C1,
                          const RMatrixBase& R2, const Vector3<double>& C2)
{
  RMatrixBase R;
  (R2.Transpose()).Mult(R1, R);
  HomgPoint2D e;
  HomgPoint3D C1h(C1);
  PMatrixBase P(R2, C2);
  e=P*C1h;
  e.Homogenize();
  Matrix3x3<double> ec;
  ec.SetAsCrossProductMatrix(e);
  ec.Mult(R, *this);
}

void FMatrixBase::
Compose(const KMatrix& K1, const RMatrixBase& R1, const Vector3<double>& C1,
        const KMatrix& K2, const RMatrixBase& R2, const Vector3<double>& C2)
{
  // Compose as Essential matrix E first
  Compose(R1, C1, R2, C2);

  // Now set Fundamental matrix as F = K2^-T E K1^-1
  BIAS::Matrix3x3<double> K1Inv, K2Inv, tmp;
  if (K1.GetInverse(K1Inv) != 0) {
    BIASERR("Failed to invert K1!");
    BIASABORT;
  }
  if (K2.GetInverse(K2Inv) != 0) {
    BIASERR("Failed to invert K2!");
    BIASABORT;
  }
  K2.Transpose().Mult(*this, tmp);
  tmp.Mult(K1Inv, *this);
}