TrifocalTensorBase.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 "TrifocalTensorBase.hh"

using namespace BIAS;
using namespace std;

TrifocalTensorBase::TrifocalTensorBase()
  : Tensor3D3x3x3<double>()
{}

TrifocalTensorBase::TrifocalTensorBase(const TrifocalTensorBase& t)
  : Tensor3D3x3x3<double>(t)
{}

TrifocalTensorBase::~TrifocalTensorBase()
{}


void TrifocalTensorBase::Dump()
{
  Matrix3x3<TRIFOCALTENSOR_TYPE> T1,T2,T3;
  GetMatrices(T1,T2,T3);
  cout << "TRIFOCAL TENSOR [T1,T2,T3]: " << endl;
  cout << " T1 = " << T1 << endl;
  cout << " T2 = " << T2 << endl;
  cout << " T3 = " << T3 << endl;
}


void TrifocalTensorBase::Compose(RMatrixBase& R1, RMatrixBase& R2, 
                                 RMatrixBase& R3,
                                 Vector3<double>& C1, Vector3<double>& C2, 
                                 Vector3<double>& C3)
{
  RMatrixBase R2n, R3n, R1i;
  Vector3<double> C2n, C3n;
  R1i=R1.Transpose();
  R1i.Mult(R2, R2n); // R2n = R1i * R2
  R1i.Mult(R3, R3n); // R3n = R1i * R3
  R1i.Mult((C2 - C1), C2n); // C2n = R1i * (C2 - C1)
  R1i.Mult((C3 - C1), C3n); // C3n = R1i * (C3 - C1)
  Compose(R2n, R3n, C2n, C3n);
}

void 
TrifocalTensorBase::Compose(RMatrixBase& R1, RMatrixBase& R2, RMatrixBase& R3,
                            HomgPoint3D& C1, HomgPoint3D& C2, HomgPoint3D& C3)
{
  Vector3<double> c1, c2, c3;
  C1.GetEuclidean(c1);
  C2.GetEuclidean(c2);
  C3.GetEuclidean(c3);
  Compose(R1, R2, R3, c1, c2, c3);
}

void TrifocalTensorBase::Compose(RMatrixBase& R2, RMatrixBase& R3,
                                 Vector3<double>& C2, Vector3<double>& C3)
{
  // see Hartley Zissermann, second edition p 367, (eq 15.1)

  //cerr << "composition from : "<<R2<<"\n"<<R3<<"\n"<<C2<<"\n"<<C3<<endl;
  Matrix3x3<double> T1, T2, T3, M;
  Vector3<double> a1, a2, a3, b1, b2, b3, RC2, RC3;
  R2.GetRow(0, a1); // a1=R2.GetRow(0);
  R2.GetRow(1, a2); // a2=R2.GetRow(1);
  R2.GetRow(2, a3); // a3=R2.GetRow(2);
  R3.GetRow(0, b1); // b1=R3.GetRow(0);
  R3.GetRow(1, b2); // b2=R3.GetRow(1);
  R3.GetRow(2, b3); // b3=R3.GetRow(2);

  R2.Transpose().Mult(C2, RC2); //RC2=R2.Transpose()*C2;
  R3.Transpose().Mult(C3, RC3); //RC3=R3.Transpose()*C3;
  //T1=a1.OuterProduct(RC3) - RC2.OuterProduct(b1);
  a1.OuterProduct(RC3, T1);
  RC2.OuterProduct(b1, M);
  T1-=M;
  //T2=a2.OuterProduct(RC3) - RC2.OuterProduct(b2);
  a2.OuterProduct(RC3, T2);
  RC2.OuterProduct(b2, M);
  T2-=M;
  //T3=a3.OuterProduct(RC3) - RC2.OuterProduct(b3);
  a3.OuterProduct(RC3, T3);
  RC2.OuterProduct(b3, M);
  T3-=M;
  
  SetFromMatrices(T1, T2, T3);
}

void 
TrifocalTensorBase::Compose(RMatrixBase& R2, RMatrixBase& R3,
                            HomgPoint3D& C2, HomgPoint3D& C3)
{
  Vector3<double> c2, c3;
  C2.GetEuclidean(c2);
  C3.GetEuclidean(c3);
  Compose(R2, R3, c2, c3);
}


void TrifocalTensorBase::CheckPointCorr(HomgPoint2D& p1, HomgPoint2D& p2, 
                                        HomgPoint2D& p3, 
                                        Matrix3x3<double>& res)
{
  //  aequivalent to:
  //   Matrix3x3<double> p2x, p3x, T1, T2, T3; 
  //   GetMatrices(T1, T2, T3);
  //   p2x.SetAsCrossProductMatrix(p2);
  //   p3x.SetAsCrossProductMatrix(p3);
  //   res = p2x * ((p1[0] * T1)+(p1[1] * T2)+(p1[2] * T3)) * p3x;
  HomgPoint2D x1 = p1;
  HomgPoint2D x2 = p2;
  HomgPoint2D x3 = p3;

  x1.Normalize();
  x2.Normalize();
  x3.Normalize();

  Matrix3x3<double> p2x, p3x, cont, tmp;
  p2x.SetAsCrossProductMatrix(x2);
  p3x.SetAsCrossProductMatrix(x3);
  LeftContravariantContraction1(x1, cont);
  p2x.Mult(cont, tmp);
  tmp.Mult(p3x, res);
}

TrifocalTensorBase& TrifocalTensorBase::operator=(const TrifocalTensorBase& t)
{
  Tensor3D3x3x3<double>::operator=(t);
  return *this;
}