PMatrixBase.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 "PMatrixBase.hh"
#include "RMatrixBase.hh"
#include <Base/Debug/Exception.hh>
#include <Base/Geometry/Quaternion.hh>
#include <Base/Math/BogIO.hh>

using namespace BIAS;
using namespace std;

void PMatrixBase::Compose(const Matrix3x3<PMATRIX_TYPE> &K, 
                          const Matrix3x3<PMATRIX_TYPE> &R,
                          const Vector3<PMATRIX_TYPE> &C)
{
  Matrix3x3<double> Rt, KRt;
  Vector3<double> LastCol;
  
  Rt = R;
  Rt.TransposeIP();
  KRt=K*Rt;
  // last column of P is:  -R'C  (docu; JW 11/2002)
  KRt.Mult(C, LastCol);
  LastCol.MultIP(-1.0);
  
  // fill the Matrix RtRtC = [R' | -R'C]  (docu; JW 11/2002)
  for (int row=0; row<3; row++) {
    (*this)[row][0] = KRt[row][0];
    (*this)[row][1] = KRt[row][1];
    (*this)[row][2] = KRt[row][2];
    (*this)[row][3] = LastCol[row]; 
  }
}

void PMatrixBase::Compose(const Matrix3x3<PMATRIX_TYPE> &K, 
                          const Matrix3x3<PMATRIX_TYPE> &R,
                          const HomgPoint3D &C)
{
  Vector3<PMATRIX_TYPE> cnew;
  if (!C.IsAtInfinity()){
    cnew.Set(C[0]/C[3], C[1]/C[3], C[2]/C[3]);
    Compose(K, R, cnew);
  } else {
    BIASERR("cannot compose camera at infinity");
  }
}

void PMatrixBase::Compose(const Matrix3x3<PMATRIX_TYPE> &R,
                          const Vector3<PMATRIX_TYPE> &C)
{
  Matrix3x3<PMATRIX_TYPE> K;
  K.SetIdentity();
  Compose(K, R, C);
}

void PMatrixBase::Compose(const Matrix3x3<PMATRIX_TYPE> &R,
                          const HomgPoint3D &C)
{
  Matrix3x3<PMATRIX_TYPE> K;
  K.SetIdentity();
  Compose(K, R, C);
}

void PMatrixBase::Compose(const Matrix3x3<PMATRIX_TYPE> &K, 
                          const Vector<double>& src,
                          const enum E_ParametrizationType param_type)
{
  RMatrixBase R = Parametrization2R_(src, param_type);
  
  Vector3<double> C(src[0], src[1], src[2]);
  Compose(K, R, C);
}

RMatrixBase PMatrixBase::
Parametrization2R_(const Vector<double>& src,
                   const enum E_ParametrizationType param_type) const
{
  RMatrixBase R;
  switch (param_type){
  case AXIS_TIMES_ANGLE:
    {
      if (src.Size()!=6){
        BEXCEPTION("invalid src vector size "<<src.Size());
      }
      Vector3<double> axis_angle;
      axis_angle.Set(src[3], src[4], src[5]);
      R.SetFromAxisAngle(axis_angle);
    }
    break;
  case AXIS_AND_ANGLE:
    {
      if (src.Size()!=7){
        BEXCEPTION("invalid src vector size "<<src.Size());
      }
      Vector3<double> axis;
      axis.Set(src[3], src[4], src[5]);
      R.Set(axis, src[6]);
    }
    break;
  case EULER:
    if (src.Size()!=6){
      BEXCEPTION("invalid src vector size "<<src.Size());
    }
    R.SetXYZ(src[3], src[4], src[5]);
    break;
  case QUATERNION:
    {
      if (src.Size()!=7){
        BEXCEPTION("invalid src vector size "<<src.Size());
      }
      Quaternion<double> q(src[3], src[4], src[5], src[6]);
      R.SetFromQuaternion(q);
    }
    break;
  default:
    BIASERR("unknown enum type "<<param_type);
    BEXCEPTION("unkown enum type "<<param_type);
    break;
  }
  return R; 
}

PMatrixBase & 
PMatrixBase::newsize(int rows, int cols) 
{
  BIASERR("The size of a P-Matrix has to be 3x4. newsize can't create a " 
         << rows<<" x "<< cols << " PMatrixBase. aborting.");
  exit(-1);
  //return *this;
}

/** Sets P-Matrix from a vector which contains the elements of P 
    row wise.*/
void PMatrixBase::SetFromVector(const Vector<PMATRIX_TYPE>& vals)
{  
  PMATRIX_TYPE *data = vals.GetData();
  for (unsigned int i = 0 ; i <  P_MATRIX_ROWS; i++)
    for (unsigned int j = 0 ; j < P_MATRIX_COLS; j++)
      (*this)[i][j]=*(data++);
}

HomgPoint3D PMatrixBase::Backproject(const HomgPoint2D& x, double mu)
{
  // prepare data
  HomgPoint2D xx=x;
  Matrix3x3<double> M;
  HomgPoint2D p4;

  for (int i=0;i<3;i++){
    p4[i]=(*this)[i][3];
    for (int j=0;j<3;j++)
      M[i][j]=(*this)[i][j];
  }

  // invert M
  if (M.InvertIP() != 0) {
    BIASERR("error: unable to invert M "<<M<<endl)
    BIASABORT
  }

  // perform transformation
  xx*=mu;
  xx-=p4;
  xx = M*xx; 
  
  // return result
  return HomgPoint3D(xx[0], xx[1], xx[2], 1.0);
}

int PMatrixBase::
LoadBOG(const std::string & filename)
{
  Matrix3x3<PMATRIX_TYPE> K;
  Matrix3x3<PMATRIX_TYPE> R;
  Vector3<PMATRIX_TYPE> C;
  int result=0;
  if (0!=LoadBogK(filename, K)) result += -1;
  if (0!=LoadBogRC(filename, R)) result += -2;
  if (0!=C.LoadBogTC(filename)) result += -4;
  Compose(K, R, C);
  return result;
}