TrifocalTensor.cpp

#include "TrifocalTensor.hh"
#include <Base/Debug/DebugSimple.hh>
#include <MathAlgo/SVD.hh>

using namespace std;
using namespace BIAS;


TrifocalTensor::TrifocalTensor()
{
  Init();
}


TrifocalTensor::~TrifocalTensor()
{
}


TrifocalTensor::TrifocalTensor(BIAS::PMatrix &P1, BIAS::PMatrix &P2, 
                               BIAS::PMatrix &P3)
{
  Init();
  Compose( P1, P2, P3 );
}


TrifocalTensor::TrifocalTensor(BIAS::PMatrix &P2, BIAS::PMatrix &P3)
{
  Init();
  Compose( P2, P3 );
}


void TrifocalTensor::Init()
{    
  Epipole21_ = NULL;
  Epipole31_ = NULL;
}


void TrifocalTensor::Compose(BIAS::PMatrix &P1, BIAS::PMatrix &P2, BIAS::PMatrix &P3)
{
  if ( P1.IsIdentity() ) Compose( P2, P3 ); // P1 is canonical ( P1=[I|0] )
  P1.GetCanonicalH(CanH_);
  PMatrix P2c(P2 * CanH_);
  PMatrix P3c(P3 * CanH_);
//   COUT("new P2 is " << P2c << endl);
//   COUT("new P3 is " << P3c << endl);
  Compose( P2c, P3c );
}


void TrifocalTensor::Compose(BIAS::PMatrix &P2, BIAS::PMatrix &P3)
{
  Matrix3x3<TRIFOCALTENSOR_TYPE> T[3];
  Vector3<double> a,b,c,d;
  for(int i=0;i<3;i++)
    {
      a = P2.GetCol(i);
      b = P3.GetCol(3);
      c = P2.GetCol(3);
      d = P3.GetCol(i);
      T[i] = a.OuterProduct(b) - c.OuterProduct(d);
    }
  SetFromMatrices( T[0], T[1], T[2] );
}


void TrifocalTensor::ComputeEpipoles()
{
  if ( Epipole21_!=NULL ) delete Epipole21_;
  if ( Epipole31_!=NULL ) delete Epipole31_;
  Matrix3x3<TRIFOCALTENSOR_TYPE> T1,T2,T3;
  Vector<TRIFOCALTENSOR_TYPE> u1(3),u2(3),u3(3);
  Vector<TRIFOCALTENSOR_TYPE> v1(3),v2(3),v3(3);

  GetMatrices(T1, T2, T3);

  SVD svd1((Matrix<TRIFOCALTENSOR_TYPE>)(T1));
  SVD svd2((Matrix<TRIFOCALTENSOR_TYPE>)(T2));
  SVD svd3((Matrix<TRIFOCALTENSOR_TYPE>)(T3));

  // compute u_i^\top T_i = 0^\top
  u1 = svd1.GetLeftNullvector();
  u2 = svd2.GetLeftNullvector();
  u3 = svd3.GetLeftNullvector();

  // compute T_i v_i = 0
  v1 = svd1.GetNullvector();
  v2 = svd2.GetNullvector();
  v3 = svd3.GetNullvector();

  // compose [u1,u2,u3]
  Matrix3x3<TRIFOCALTENSOR_TYPE> U;
  U[0][0] = u1[0]; U[0][1] = u2[0]; U[0][2] = u3[0];
  U[1][0] = u1[1]; U[1][1] = u2[1]; U[1][2] = u3[1];
  U[2][0] = u1[2]; U[2][1] = u2[2]; U[2][2] = u3[2];

  // compose [v1,v2,v3]
  Matrix3x3<TRIFOCALTENSOR_TYPE> V;
  V[0][0] = v1[0]; V[0][1] = v2[0]; V[0][2] = v3[0];
  V[1][0] = v1[1]; V[1][1] = v2[1]; V[1][2] = v3[1];
  V[2][0] = v1[2]; V[2][1] = v2[2]; V[2][2] = v3[2];

  SVD svdu((Matrix<TRIFOCALTENSOR_TYPE>)(U));
  SVD svdv((Matrix<TRIFOCALTENSOR_TYPE>)(V));

  // e'^\top [u1,u2,u3] = 0 
  Epipole21_ = new HomgPoint2D(Vector3<double>(svdu.GetLeftNullvector()));
  // e''^\top [v1,v2,v3] = 0
  Epipole31_ = new HomgPoint2D(Vector3<double>(svdv.GetLeftNullvector()));
}

HomgPoint2D TrifocalTensor::GetEpipole21( bool ForceReCalc )
{
  if ( Epipole21_==NULL || ForceReCalc ) ComputeEpipoles();
  return *Epipole21_;
}


HomgPoint2D TrifocalTensor::GetEpipole31( bool ForceReCalc )
{
  if ( Epipole31_==NULL || ForceReCalc ) ComputeEpipoles();
  return *Epipole31_;
}

FMatrix TrifocalTensor::GetFMatrix21( bool ForceReCalc )
{
  FMatrix F;
  Vector3<TRIFOCALTENSOR_TYPE> f1,f2,f3,tmp;
  HomgPoint2D e2 = GetEpipole21(ForceReCalc);
  HomgPoint2D e3 = GetEpipole31();
  Vector3<TRIFOCALTENSOR_TYPE> e21(e2[0],e2[1],e2[2]);
  Vector3<TRIFOCALTENSOR_TYPE> e31(e3[0],e3[1],e3[2]);

  Matrix3x3<TRIFOCALTENSOR_TYPE> T1,T2,T3;
  GetMatrices(T1, T2, T3);

  T1.Mult(e31,tmp);
  f1 = e21.CrossProduct(tmp);
  T2.Mult(e31,tmp);
  f2 = e21.CrossProduct(tmp);
  T3.Mult(e31,tmp);
  f3 = e21.CrossProduct(tmp);

  F[0][0] = f1[0]; F[0][1] = f2[0]; F[0][2] = f3[0];
  F[1][0] = f1[1]; F[1][1] = f2[1]; F[1][2] = f3[1];
  F[2][0] = f1[2]; F[2][1] = f2[2]; F[2][2] = f3[2];

  return F;
}


FMatrix TrifocalTensor::GetFMatrix31( bool ForceReCalc )
{
  FMatrix F;
  Vector3<TRIFOCALTENSOR_TYPE> f1,f2,f3,tmp;
  HomgPoint2D e2 = GetEpipole21(ForceReCalc);
  HomgPoint2D e3 = GetEpipole31();
  Vector3<TRIFOCALTENSOR_TYPE> e21(e2[0],e2[1],e2[2]);
  Vector3<TRIFOCALTENSOR_TYPE> e31(e3[0],e3[1],e3[2]);

  Matrix3x3<TRIFOCALTENSOR_TYPE> T1,T2,T3;
  GetMatrices(T1, T2, T3);

  T1.Mult(e21,tmp);
  f1 = e31.CrossProduct(tmp);
  T2.Mult(e21,tmp);
  f2 = e31.CrossProduct(tmp);
  T3.Mult(e21,tmp);
  f3 = e31.CrossProduct(tmp);

  F[0][0] = f1[0]; F[0][1] = f2[0]; F[0][2] = f3[0];
  F[1][0] = f1[1]; F[1][1] = f2[1]; F[1][2] = f3[1];
  F[2][0] = f1[2]; F[2][1] = f2[2]; F[2][2] = f3[2];

  return F;
}


PMatrix TrifocalTensor::GetPMatrix2( bool ForceReCalc )
{
  PMatrix P;
  HomgPoint2D e2 = GetEpipole21(ForceReCalc);
  HomgPoint2D e3 = GetEpipole31();
  e2.Normalize();
  e3.Normalize();
  Vector3<TRIFOCALTENSOR_TYPE> e31(e3[0],e3[1],e3[2]);
  Vector3<TRIFOCALTENSOR_TYPE> p1,p2,p3;

  Matrix3x3<TRIFOCALTENSOR_TYPE> T1,T2,T3;
  GetMatrices(T1, T2, T3);

  T1.Mult(e31,p1);
  T2.Mult(e31,p2);
  T3.Mult(e31,p3);

  P[0][0] = p1[0]; P[0][1] = p2[0]; P[0][2] = p3[0]; P[0][3] = e2[0];
  P[1][0] = p1[1]; P[1][1] = p2[1]; P[1][2] = p3[1]; P[1][3] = e2[1];
  P[2][0] = p1[2]; P[2][1] = p2[2]; P[2][2] = p3[2]; P[2][3] = e2[2];

  return P;
}


PMatrix TrifocalTensor::GetPMatrix3( bool ForceReCalc )
{
  PMatrix P;
  HomgPoint2D e2 = GetEpipole21(ForceReCalc);
  HomgPoint2D e3 = GetEpipole31();
  e2.Normalize();
  e3.Normalize();
  Vector3<TRIFOCALTENSOR_TYPE> e21(e2[0],e2[1],e2[2]);
  Vector3<TRIFOCALTENSOR_TYPE> p1,p2,p3,tmp;

  Matrix3x3<TRIFOCALTENSOR_TYPE> T1,T2,T3;
  GetMatrices(T1, T2, T3);
  T1 = T1.Transpose();
  T2 = T2.Transpose();
  T3 = T3.Transpose();

  Matrix3x3<TRIFOCALTENSOR_TYPE> M;
  // compute [e''e''^\top-I]
  M[0][0] = e3[0]*e3[0]-1; M[0][1] = e3[1]*e3[0];   M[0][2] = e3[2]*e3[0];
  M[1][0] = e3[0]*e3[1];   M[1][1] = e3[1]*e3[1]-1; M[1][2] = e3[2]*e3[1];
  M[2][0] = e3[0]*e3[2];   M[2][1] = e3[1]*e3[2];   M[2][2] = e3[2]*e3[2]-1;

  T1.Mult(e21,tmp);
  M.Mult(tmp,p1);
  T2.Mult(e21,tmp);
  M.Mult(tmp,p2);
  T3.Mult(e21,tmp);
  M.Mult(tmp,p3);

  P[0][0] = p1[0]; P[0][1] = p2[0]; P[0][2] = p3[0]; P[0][3] = e3[0];
  P[1][0] = p1[1]; P[1][1] = p2[1]; P[1][2] = p3[1]; P[1][3] = e3[1];
  P[2][0] = p1[2]; P[2][1] = p2[2]; P[2][2] = p3[2]; P[2][3] = e3[2];

  return P;
}


void
TrifocalTensor::GetEpipolarLines(Vector3<TRIFOCALTENSOR_TYPE> &Point1,
                                 HomgLine2D &Line2,
                                 HomgLine2D &Line3)
{
  Matrix3x3<TRIFOCALTENSOR_TYPE> M = LeftContravariantContraction1(Point1);
        
  SVD svd((Matrix<TRIFOCALTENSOR_TYPE>)(M));

  Line2 = HomgLine2D(svd.GetLeftNullvector());
  Line3 = HomgLine2D(svd.GetNullvector());
}


HomgPoint2D TrifocalTensor::TransferView3(Vector3<TRIFOCALTENSOR_TYPE> &x1,
                                          Vector3<TRIFOCALTENSOR_TYPE> &x2)
{
  HomgPoint2D p1(x1);
  HomgPoint2D p2(x2);
  p1.Normalize();
  p2.Normalize();

  // compute epipolar line of x1 in view 2
  HomgLine2D le(GetFMatrix21() * p1);
  //cout << "le = " << le << endl;

  /* make line perpendicular and passing thru x2, as the epipolar line is 
     not good for transfer */
  HomgLine2D l2;
  le.GetPerpendicularLine(p2,l2);

  //cout << "l2 = " << l2 << endl;

  //cout << "l2*x2=" << l2.ScalarProduct(p2) << endl;;

  Vector3<TRIFOCALTENSOR_TYPE> x3;
  LeftContravariantContraction2(l2).TransposedMult(x1,x3);

  return HomgPoint2D(x3);
}

TrifocalTensor& TrifocalTensor::operator=(const TrifocalTensor& t)
{
  TrifocalTensorBase::operator=(t);
  return *this;
}