PMatrix.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 <Base/Common/BIASpragma.hh>

#include "PMatrix.hh"

#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Geometry/AffineTransf.hh>
#include <MathAlgo/Lapack.hh>
#include <fstream>
#include <sstream>
#include <MathAlgo/SVD.hh>

using namespace BIAS;
using namespace std;


// precision for output using streams, counted in decimal digits
#define STREAM_PRECISION 12

namespace BIAS {

PMatrix::~PMatrix() {
  if (Svd_ != NULL) delete Svd_;
#ifdef BIAS_DEBUG
  Svd_ = NULL;
#endif
}

PMatrix::PMatrix(const std::string & s) 
: PMatrixBase(s)  
{ 
  Svd_ = NULL;
  InvalidateDecomposition();
}


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

PMatrix::PMatrix(const Matrix<PMATRIX_TYPE>& Amat)  
  :  PMatrixBase(Amat)
{
  if ((Amat.num_rows()==3) && (Amat.num_cols()==4)) {
    Svd_=NULL;
    InvalidateDecomposition();
    IsMetric_ = false;
  } else {
    BIASERR(" wrong size in constructor! Amat is a "<<Amat.num_rows()
            <<" x "<<Amat.num_cols()<<" matrix but must be 3x4."
            <<std::endl );
    BIASASSERT((Amat.num_rows()==3) && (Amat.num_cols()==4));
  };
}

PMatrix& PMatrix::operator=(const PMatrix &mat) 
{
  if (&mat != this){
    BIAS::PMatrixBase::operator=(mat);
    if (Svd_!=NULL) { delete Svd_; Svd_=NULL; }
    if (mat.Svd_!=NULL) Svd_ = new SVD(*(mat.Svd_));
    C_=mat.C_;
    A_=mat.A_;
    H0_=mat.H0_;
    V0_=mat.V0_;
    R_=mat.R_;
    K_=mat.K_;
    Covariance_ = mat.Covariance_;
    Hinf_=mat.Hinf_;
#ifdef BIAS_DEBUG
    PConsistencyBackup_ = mat.PConsistencyBackup_;
    PConsistencyBackupSVD_ = mat.PConsistencyBackupSVD_;
#endif
    IsDecomposed_ = mat.IsDecomposed_;
    IsMetric_ = mat.IsMetric_;
  }
  return *this;
}

/** \todo 5 different  Back-Project-Funktions, "Please cleanup" 
    (evers 2004-11-29) 
*/


int PMatrix::GetPseudoInverse(BIAS::Matrix<double>& Pinv) {
  if (Svd_ == NULL) {
    MakeSVD_();
  } else {
#ifdef BIAS_DEBUG
    CheckSVD_();
#endif
  }

  Matrix<double> V(Svd_->GetV()); // 3x4 matrix
  Matrix<double> SinvUT(4,3); 
  SinvUT.SetZero();
  Matrix<double> U(Svd_->GetU());  // 4x4 matrix
  Vector3<double> S(Svd_->GetS()); 
  for (int i=0;i<U.num_rows();i++)
    for (int j=0;j<U.num_cols();j++) {
      SinvUT[i][j] = 1.0/S[i] * U[j][i];
    }         
  Pinv = V * SinvUT; 

  return 0;
}


int PMatrix::GetNullVector(HomgPoint3D& C) {
  MakeSVD_();  
  Vector<double> Nullvec(Svd_->GetNullvector());
  if (fabs(Nullvec[3]) > DBL_EPSILON) {
    // homogenize vector
    C.Set(Nullvec[0]/Nullvec[3], Nullvec[1]/Nullvec[3], Nullvec[2]/Nullvec[3],
          1.0);
  } else {
    // return "as is"
    C.Set(Nullvec[0],Nullvec[1],Nullvec[2],Nullvec[3]);
  }
  return 0;
}

int PMatrix::GetHinf(BIAS::Matrix3x3<double> &Hinf) {
  if (!IsDecomposed_) {
    if (Decompose_()!=0) return -1;
  } else {
    // flag says: "matrix is decomposed!"
#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }
  Hinf = Hinf_;
  return 0;
}


int PMatrix::GetC(Vector3<double>& C) 
{
  if (!IsDecomposed_) {
    if (Decompose_()!=0)
      return -1;
  } else {
    // flag says: "matrix is decomposed!"
#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }

  C=C_;
  return 0;
}

// JW
BIAS::Vector3<double> PMatrix::GetC() {
  BIAS::Vector3<double>  C;
  GetC(C);
  return C;
}

int PMatrix::GetC(HomgPoint3D& C) 
{
  if (!IsDecomposed_) {
    if (Decompose_()!=0)
      return -1;
  } else {
    // flag says: "matrix is decomposed!"
#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }

  C.Set(C_[0], C_[1], C_[2]);
  return 0;
}

int PMatrix::GetR(Matrix3x3<double>&  R) 
{
  if (!IsDecomposed_) {
    if ( Decompose_() != 0)
      return -1;
  } else {
    // flag says: "matrix is decomposed!"
#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }

  R=R_;
  return 0;
}

int  PMatrix::GetK(KMatrix& K) 
{
  if (!IsDecomposed_) {
    if (Decompose_() != 0)
      return -1;
  } else {
    // flag says: "matrix is decomposed!"
#ifdef BIAS_DEBUG
    CheckDecomposition_();   
#endif
  }
  K=K_;
  return 0;
}

/// This is not the cam.# standard CAHV, but the unit vector H0 
// (jw, 11/2002) (because image size is unknown)
int PMatrix::GetH(Vector3<double>& H) 
{
  if (!IsDecomposed_) {
    if(Decompose_()!=0)
      return -1;
  } else {

    // flag says: "matrix is decomposed!"

#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }

  H=H0_;
  return 0;
}

// JW
BIAS::Vector3<double> PMatrix::GetH(){
  BIAS::Vector3<double> h;
  if (0!=GetH(h)) BIASERR("could not access H.");
  return h;
}
 
/// This is not the cam.# standard CAHV, but the unit vector H0 
//(jw, 11/2002) (because image size is unknown)
int PMatrix::GetV(Vector3<double>&  V)
{
  if (!IsDecomposed_) {
    if (Decompose_()!=0)
      return -1;
  } else {
    // flag says: "matrix is decomposed!"

#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }
  V=V0_;
  return 0;
}

// JW
BIAS::Vector3<double> PMatrix::GetV(){
  BIAS::Vector3<double> v;
  if (0!=GetV(v)) BIASERR("could not access V.");
  return v;
}

int PMatrix::GetA(Vector3<double>& A) 
{
  if (!IsDecomposed_) {
    if (Decompose_() != 0)
      return -1;
  } else {
    // flag says: "matrix is decomposed!"

#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }
  A=A_;
  return 0;
}

// JW
BIAS::Vector3<double> PMatrix::GetA(){
  BIAS::Vector3<double> a;
  if (0!=GetA(a)) BIASERR("could not access A.");
  return a;
}

void PMatrix::GetSVD(Matrix<double>& U, Vector<double>& S, 
                     Matrix<double>& VT) {
  MakeSVD_();
  U  = Svd_->GetU();
  S  = Svd_->GetS();
  VT = Svd_->GetVT();
}

int PMatrix::Decompose_() {
  int decomposeresult = 0;
  // see diploma thesis gonzales p. 94 (docu; JW 11/2002)
  double rho = 0.0;
  PMatrix Ptmp;
  Ptmp= *this;
  Matrix3x3<double> matA;
  Vector<double> vecB(3);
  
  // sum(P_[2][i]^2) must be 1
  for (int i=0; i<3; i++)
    rho += (*this)[2][i] * (*this)[2][i];
  rho = sqrt(rho);
  // norm temporary P
  if (rho==0) { // JW 05/2003 
    BIASERR("degenerate case detected in decomposition. P="<<(*this)<<endl);
    BIASABORT;
  } else {
    Ptmp /= rho;
  };

  // take only left part of P (as K*R')
  // P = [ matA | -vecB]  (docu; JW 11/2002)
  // M^T = P[0..2][0..2]
  for (int i=0; i<3; i++) {
    for (int j=0; j<3; j++)
      matA[i][j] = Ptmp[i][j];
    
    vecB[i] = -Ptmp[i][3];
  }
  
  // solve linear equations using LU factorization     
  // solve for C:  "matA * C = vecB" (docu; JW 11/2002)
  // Vector<double> tmpC=Lapack_LU_linear_solve(matA, vecB);

  int hres = matA.GetInverse(Hinf_);
  if (hres!=0) {
    BIASERR("error unable to compute Hinf_.");
    Hinf_.SetZero();
    decomposeresult = -1;
    // pick the smallest base vector from the nullspace and assume center
    HomgPoint3D HC;
    this->GetNullVector(HC);
    HC.GetEuclidean(C_);
  } else {
    // even here it seems to me more accurate and numerically stable to compute
    // the center via the nullvector svd, especially when we dont exactly 
    // have (-K R^T C) as the last column of P 
    // However, we keep it the old way since it seemed to work. (koeser)
    //C_ = Hinf_ * vecB;
    Hinf_.Mult(vecB, C_);
  }

  // take rows of Ptmp in: zeile 1...3 = z1..z3
  Vector3<double> z1, z2, z3,  tmp;
  K_.SetZero();
  K_[2][2] = 1.0;
  
  // for all cols: fill rows z: 
  for (int col=0; col<3; col++) {
    z1[col] = Ptmp[0][col]; // 1st row
    z2[col] = Ptmp[1][col]; // 2nd row
    // R = (H V A) , KR^T = Ptmp -> A = z3
    R_[col][2] = Ptmp[2][col];  
    // 3rd row, is unit length 1 because or previous normalization by 
    // rho (docu: JW 11/2002)
    A_[col] = Ptmp[2][col];  // 3rd row
  }

  // cy = z2^T * z3 (=optical center hx docu; is OK because A is orthogonal 
  // to V (see diploma thesis Gonzalez, pp. 94 JW 11/2002)
  K_[1][2] = z2.ScalarProduct(A_);
  // cx = z1^T * z3 (=optical center hx docu; JW 11/2002)
  K_[0][2] = z1.ScalarProduct(A_);

  // fy = || z2^T - cy * z3^T ||
  V0_ = (z2 - (A_ * K_[1][2]) );
  K_[1][1] = V0_.NormL2();

  // V = 1/fy (z2^T - cx * z3^T)
  V0_ /=  K_[1][1];

  // kk: how can we assume skew=0, compute R exploiting that 
  // and then compute the skew ???
  // 
  // skew: S= H * V.NORM
  K_[0][1]=z1.ScalarProduct(V0_); 

  // H = 1/fx (z1^T - cy * z3^T)
  // fx = || z1^T - cx * z3^T ||
  H0_ = (z1  - V0_*K_[0][1]-(A_ * K_[0][2]) );
  K_[0][0] = H0_.NormL2();

  H0_ /= K_[0][0];

  // docu: Here h, v are 'still' unity vectors, so fill them into R 
  // (docu; JW 11/2002)
  for (int i=0; i<3; i++) {
    R_[i][0] = H0_[i];
    R_[i][1] = V0_[i];
  }

#ifdef BIAS_DEBUG

  Vector3<double> sum(0.0, 0.0, 0.0), col1, col2, col3;
  for (int i=0; i<3; i++){ 
    sum[i] += R_[i][0] * R_[i][0] + R_[i][1] * R_[i][1] + R_[i][2] * R_[i][2]; 
    col1[i] = R_[i][0];
    col2[i] = R_[i][1];
    col3[i] = R_[i][2];
  }
  if ( fabs(col1.ScalarProduct(col2))>1e-10 ||
       fabs(col3.ScalarProduct(col2))>1e-10 ||
       fabs(col1.ScalarProduct(col3))>1e-10 ){
    BIASERR("warning, numerical instability in PMatrix::Decompose_(): "
            << "R_ is nor orthogonal (not a rotation matrix), scalar "
            << "products are: " << col1.ScalarProduct(col2) << "," <<
            col3.ScalarProduct(col2) << "," << col1.ScalarProduct(col3));
  }
  if ( fabs(sum[0]-1.0)>1e-10 || fabs(sum[1]-1.0)>1e-10 ||  
       fabs(sum[2]-1.0)>1e-10 ){
    BIASERR("warning, numerical instability in PMatrix::Decompose_(): "
            << "R_ is not a orthonormal (not a rotation matrix) " 
            << "Vector lengthes are"<<sum[0] << "," << sum[1] << "," 
            << sum[2]);
  } 


  // make backup of pmatrix fields for debug purposes
  PConsistencyBackup_ = (*this);
#endif
  IsDecomposed_ = true;
  return decomposeresult;
}

void PMatrix::
BackprojectByZDepth(const double& x, 
            const double& y, 
            const double& z_depth,
            HomgPoint3D& res)
{
  HomgPoint3D C;
  GetC(C);

  HomgPoint2D p(x*z_depth, y*z_depth, z_depth);
  p = GetK().Invert()*p;
  p = GetR()*p;
  res.Set(p[0]+C[0], p[1]+C[1], p[2]+C[2], C[3]);
}

void PMatrix::BackprojectWorldCoo(const HomgPoint2D& point, double depth,
                                  HomgPoint3D& res)
{
  Vector3<double> ray;
  HomgPoint3D C, hray;
  if (GetC(C)!=0){
    BIASERR("error getting C");
    //BIASASSERT(false);
    BIASABORT;
  }
  ray = GetNormRayWorldCoo(point);
  hray.Set(depth*ray[0], depth*ray[1], depth*ray[2], 0.0);
  res = C+hray;
}

HomgPoint3D PMatrix::BackprojectPseudoinverse (const HomgPoint2D& x) {
  Matrix<double> Pinv;
  GetPseudoInverse(Pinv);

  HomgPoint3D newPoint(Pinv * x);
  newPoint.Homogenize();
 
  //// check if direction is the same as camera orientation
  Vector3<double> viewray, center;
  GetC(center);
  viewray[0] = newPoint[0] - center[0];   
  viewray[1] = newPoint[1] - center[1]; 
  viewray[2] = newPoint[2] - center[2];  
  // scalar product viewray with camera orientation
  double dS = viewray[0] * (*this)[2][0] +  viewray[1] * (*this)[2][1] +  
    viewray[2] * (*this)[2][2];
  if (dS<0) {
    // backprojected point is behind camera
    newPoint[0] = center[0] - viewray[0];
    newPoint[1] = center[1] - viewray[1];
    newPoint[2] = center[2] - viewray[2];
  }

  newPoint[3]=1; // there is the case that w=0, though other values are valid

  return newPoint;
}



void PMatrix::GetImagePlane(HomgPlane3D& plane)
{
  if (!IsDecomposed_) {
    Decompose_();
  } else {
    // flag says: "matrix is decomposed!"
#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }
  plane[0] = A_[0];
  plane[1] = A_[1];
  plane[2] = A_[2];
  Vector3<double> PlaneVec(0.0, 0.0, 1.0);
  Vector3<double> WorldPlaneVec;
  R_.Mult(PlaneVec, WorldPlaneVec);
  WorldPlaneVec += C_;
  plane[3] = WorldPlaneVec.NormL2(); 
}

HomgPlane3D PMatrix::GetImagePlane() {
  HomgPlane3D plane;
  GetImagePlane(plane);
  return plane;
}

int PMatrix::GetIntersectionOfImagePlanes(PMatrix& P, 
                      HomgLine2D& intersection) 
{
  // this is just the way, to do: fast
  // NormalVector = P.GetA()
  // Transform Normal Vector to CameraCoo of this PMatrix
  // R = GetR();
  // NewA = R.TRanspose() * NormalVector
  // intersection[0] = NewA[0];
  // intersection[1] = NewA[1];
  // intersection[2] = NewA[2] + NewA[3];

  int res = 0;
  HomgPlane3D Plane = P.GetImagePlane();
  HomgPlane3D TransformedPlane;
  AffineTransf at;
  Matrix3x3<double> R; 
  if (GetR(R) !=0)
    return -1;
//    cerr << "Plane: " << Plane << endl;
//    cerr << "R: " << R << endl;
  R.TransposeIP();
//    cerr << "R': " << R << endl;
  at.SetAsRotationMatrix(R);
//    cerr << "at: " << at << endl;
  at.Mult(Plane, TransformedPlane);
//    cerr << "TransfPlane: " << TransformedPlane << endl;

  double dot;
  TransformedPlane.ScalarProduct(HomgPlane3D(0.0, 0.0, 1.0, 0.0), dot);
  if ((dot < PMATRIX_EPSILON) && (dot > -PMATRIX_EPSILON)) { 
    res = -1;
  } else {
    intersection[0] = TransformedPlane[0];
    intersection[1] = TransformedPlane[1];
    intersection[2] = TransformedPlane[2] + TransformedPlane[3];
  }

  return res;
}


void PMatrix::Compose(const Matrix3x3<double>& K, 
                      const Matrix3x3<double>& R, 
                      const Vector3<double>& C)
{
  IsMetric_ = true;
  R_ = R;
  C_ = C;
  K_ = K;
  for (register int i=0; i<3; i++){
    H0_[i] = R_[i][0];
    V0_[i] = R_[i][1]; 
    A_[i] = R_[i][2]; 
  }
  PMatrixBase::Compose(K, R, C);
  for (unsigned int j=0; j<3; j++) {
    for (unsigned int i=0; i<3; i++) {
      Hinf_[i][j] = (*this)[i][j];
    }
  }
  int res = Hinf_.InvertIP();
  if (res!=0) { 
    BIASERR("error computing Hinf_."); 
    cout << "K: "<<K<<endl<<"R: "<<R<<endl<<"C: "<<C<<endl;BIASABORT; }

#ifdef BIAS_DEBUG
  // save the PMatrix fields
  PConsistencyBackup_ = (*this);
#endif
  IsDecomposed_=true;

  if (Svd_ != NULL) {
    delete Svd_;
    Svd_ = NULL;
  }
}

void PMatrix::Compose(const Matrix3x3<double>& K, 
                      const Matrix3x3<double>& 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]);
    PMatrix::Compose(K, R, cnew);
    for (unsigned int j=0; j<3; j++) {
      for (unsigned int i=0; i<3; i++) {
        Hinf_[i][j] = (*this)[i][j];
      }
    }
  int hres = Hinf_.InvertIP();
  if (hres!=0) BIASERR("error: unable to compute Hinf_.");
#ifdef BIAS_DEBUG
    // save the PMatrix fileds
    PConsistencyBackup_ = (*this);
#endif
    IsDecomposed_ = true;
  } else {
    BIASERR("cannot compose camera at infinity");
  }
}

void PMatrix::
Compose(const Matrix3x3<double>& K, 
        const Vector<double>& parametrization,
        enum E_ParametrizationType param_type)
{
  Vector3<double> C;
  C.Set(parametrization[0], parametrization[1], parametrization[2]);
  RMatrixBase R = Parametrization2R_(parametrization, param_type);
  
  // compute and set Hinf_, ....
  Compose(K, R, C);
}

Vector3<double> 
PMatrix::GetNormRayWorldCoo(const HomgPoint2D& point)
{
  Vector3<double> result = GetRayWorldCoo(point);
  //cerr << "point: "<<point<<"\tresult: " << result <<endl;
  return result / result.NormL2();
}


Vector3<double> 
PMatrix::GetRayWorldCoo(const HomgPoint2D& point)
{
  KMatrix Kinv;
  Vector3<double> result;

  if (!IsDecomposed_){
    Decompose_();
    
//#ifdef BIAS_DEBUG
//    BIASWARN("DBG decomposing P (for call in "__FILE__<<" : "<<__LINE__<<")" );
//#endif
  
  } else {
    // flag says: "matrix is decomposed!"
#ifdef BIAS_DEBUG
    CheckDecomposition_();
#endif
  }

  Kinv = K_.Invert();
  
  Kinv.Mult(point, result);
  
  return R_ * result;
}



// JW 11/2002
int PMatrix::WriteCAHV_10(int cols, int rows, ostream & os) 
{ 
  if (cols<0 || cols>10000 || rows<0 || rows>10000 ) {
    // Error: impossible image size (in pixel) given
    BIASERR( " image cols x rows = "<<cols<<" x "<< rows
             <<" is to small or to big!"); 
  };
   
  Vector3<double> A0; // unit vector, direction of optical axis
  Vector3<double> H0; // unit vector, spans horizontal pixel-axis 
  Vector3<double> V0; // unit vector, spans vertical pixe-axixs 
  //(iff there is skew then H0,V0 are NOT orthogonal!)

  Vector3<double> H; // Cunningham CAHV 1.0 format H vector 
  Vector3<double> V; // Cunningham CAHV 1.0 format V vector 
  
  // direction of the optical axis: 
  // get the 3rd row of KR' which is P[2][0..2] and divide it by 
  // its norm to get a unit vector. 
  A0[0] = (*this)[2][0]; 
  A0[1] = (*this)[2][1];
  A0[2] = (*this)[2][2];
  double Alength = sqrt ( A0.ScalarProduct(A0) ); // norm of A
  A0 /= Alength; // normalize A to unit vector
  
  H0[0] = (*this)[0][0]; // get 1st row of KR' which is P[0][0..2]
  H0[1] = (*this)[0][1];
  H0[2] = (*this)[0][2];
  H0 /= Alength; // scale H (A should be unit vector)
  
  V0[0] = (*this)[1][0]; // get the 2nd row of KR' which is P[1][0..2]
  V0[1] = (*this)[1][1];
  V0[2] = (*this)[1][2];
  V0 /= Alength; // scale V (A should be unit vector)

  Vector3<double> TTh;
  Vector3<double> TTv;
  TTh =  A0 * ((cols-1)/2.0);
  TTv =  A0 * ((rows-1)/2.0);
  H = H0 - TTh;
  V = V0 - TTv;
  
  
  // get intrinsic parameters: 
  double imgCenterX = cols/2.0; // center of the image (not cols-1, see 
  // RK code (and example)
  double imgCenterY = rows/2.0;
  KMatrix K;
  if (GetK(K) != 0)
    return -1;
  double opticalCenterX = K[0][2]; // optical center in pixel (cx=hx)
  double opticalCenterY = K[1][2]; // (cy=hy)
  double bxh = opticalCenterX - imgCenterX; // length (in pixel) from image
  //  center to optical center
  double byh = opticalCenterY - imgCenterY;
  double fx = K[0][0]; // focal length
  double fy = K[1][1]; 
  double skew = K[0][1]; // are the x/y axes skewed (=not orthogonal)
  double sx = 1.0;
  double sy = (fx/fy)*sx;  // ratio of pixel size sx/sy
    
  // write CAHV decomposed P to file in special 'Cunningham CAHV 1.0 format: 
  Vector3<double> C;
  if (GetC(C) !=0)
    return -1;
  os<<setprecision(12);
  os <<";CAHV 1.0" << endl;
  os <<";Generated by PMatrix::WriteCAHV_10 (JW 2002)" << endl;
  os <<";     Cx       Cy        Cz"<<endl;
  os <<C[0]<<" "<<C[1]<<" "<<C[2]<<endl;
  os <<";     Ax       Ay        Az"<<endl;
  os <<A0[0]<<" "<<A0[1]<<" "<<A0[2]<<endl;
  os <<";     Hx       Hy        Hz"<<endl;
  os <<H[0]<<" "<<H[1]<<" "<<H[2]<<endl;
  os <<";     Vx       Vy        Vz"<<endl;
  os <<V[0]<<" "<<V[1]<<" "<<V[2]<<endl;
  
  os <<";     Radialdistortion rd "<<endl;
  os <<"0.0"<<endl; // fix to zero because it's not used by following programs 
  os <<"; Pixelsize sx (fixed)      sy (ratio)"<<endl;
  os <<sx<<" "<<sy<<endl;
  os <<"; Image-size: colums      rows"<<endl;
  os <<cols<<" "<<rows<<endl;
  os <<"; Internal parameters:"<<endl;
  os <<";  fx = "<< fx <<" fy = "<< fy <<" skew = "<< skew << endl;
  os <<";  hx = "<< bxh<<" hy = "<< byh << endl;

  return -1;
}




// JW 06/2003
// unfortunately not const because missing consts in decompose etc... 
float PMatrix::GetFieldOfView(const unsigned int dimX,
                              const unsigned int dimY,
                              const bool optmin) 
{
  BIAS::Vector3<double> nw, vec; // two oposing image corners
  BIAS::HomgPoint2D p2d = BIAS::HomgPoint2D( 0.0, 0.0, 1.0); // NW corner
  nw = this->GetNormRayWorldCoo( p2d ); // wcs vector
  
  if (!optmin) {
    // max. angle
    p2d[0]=(double)dimX; // SE corner
    p2d[1]=(double)dimY; //w is still 1 
  } else if (dimX<dimY) {
    // min angle X
    // optmin and dimX is smaller dimY --> SW is min
    p2d[0]=(double)dimX; // y,w are still 0,1
  } else {
    // min angle Y
    // optmin and dimY is smaller
    p2d[1]=dimY; // x,w are still 0,1
  };
  
  // second vector (SE, SW or NE)
  vec = this->GetNormRayWorldCoo( p2d ); // wcs vector
  float cosAlpha = float(nw.ScalarProduct(vec));
  float alpha = acos( cosAlpha );
  // cout<<"DBG fov="<<alpha<<" rad. (= "<<alpha*180.0/M_PI<<" degree)."<<endl;
  return alpha;
}


/** @return field of view in y-direction in rad
    this routine is complexer than expected...
    ... because the principal point may *not* be within camera center.
    HACK/FIXME/TODO:
    \todo not in general correct impl. !!! 
    the principal may converge the view -> what is the field of view?
    (JW 09/2003)
*/
float PMatrix::GetFieldOfViewY(const unsigned int dimX,
                               const unsigned int dimY,
                               const bool & compY) {  
  HomgPoint2D north2d, south2d;
  // set the 2dimage coord used to determine angle:
  if (compY) {
    // vertical fov (north->south)
    north2d = HomgPoint2D(0.5*(double)dimX, 0.0, 1.0); // N
    south2d = HomgPoint2D(0.5*(double)dimX, (double)dimY, 1.0); // S
  } else {
    // horizontal fov  (west -> east)
    north2d = HomgPoint2D(0.0, 0.5*(double)dimY, 1.0); // W
    south2d = HomgPoint2D((double)dimX, 0.5*(double)dimY, 1.0); //E
  };
  // get corresponding direction 
  Vector3<double> north3d = this->GetNormRayWorldCoo( north2d );
  Vector3<double> south3d = this->GetNormRayWorldCoo( south2d );
  north3d.Normalize();
  south3d.Normalize();  
  // get angle between
  float cosAlpha = float(north3d.ScalarProduct(south3d));
  float alpha = acos( cosAlpha );
  //cout<<"DBG fovY="<<alpha<<" rad. (= "<<alpha*180.0/M_PI<<" degree)."<<endl;
  return alpha;
}

void PMatrix::DecomposeAa(Matrix3x3<PMATRIX_TYPE> &A, 
                          Vector3<PMATRIX_TYPE> &a) const {
  for(int i=0; i<3; i++)
    for(int j=0; j<4; j++)
      if ( j==3 )
        a[i] = (*this)[i][j];
      else
        A[i][j] = (*this)[i][j];  
}


Matrix4x4<PMATRIX_TYPE> PMatrix::GetCanonicalH() const
{
  Matrix4x4<PMATRIX_TYPE> ProjH;
  GetCanonicalH(ProjH);
  return ProjH;
}

void PMatrix::GetCanonicalH(Matrix4x4<PMATRIX_TYPE> &ProjH) const {
  Matrix3x3<PMATRIX_TYPE> A;
  Vector3<PMATRIX_TYPE> a;
  DecomposeAa(A,a);  
  SVD svd;//  SVD svd((Matrix<PMATRIX_TYPE>)A);

  //Matrix3x3<PMATRIX_TYPE> PseudoInv(svd.Invert());
  Matrix3x3<PMATRIX_TYPE> PseudoInv(svd.Invert((Matrix<PMATRIX_TYPE>)A));
  Vector<PMATRIX_TYPE> x;/* Vector3<PMATRIX_TYPE> x(svd.solve(a));*/
  Matrix<PMATRIX_TYPE> A2(A);
  Vector<PMATRIX_TYPE> a2(a);
  svd.Solve(A2,a2, x);

  ProjH[0][0] = PseudoInv[0][0];
  ProjH[0][1] = PseudoInv[0][1];
  ProjH[0][2] = PseudoInv[0][2];
  ProjH[1][0] = PseudoInv[1][0];
  ProjH[1][1] = PseudoInv[1][1];
  ProjH[1][2] = PseudoInv[1][2];
  ProjH[2][0] = PseudoInv[2][0];
  ProjH[2][1] = PseudoInv[2][1];
  ProjH[2][2] = PseudoInv[2][2];

  ProjH[3][0] = 0;
  ProjH[3][1] = 0;
  ProjH[3][2] = 0;
  ProjH[3][3] = 1;

  ProjH[0][3] = -x[0];
  ProjH[1][3] = -x[1];
  ProjH[2][3] = -x[2];
}

int PMatrix::LoadBBC(const std::string &filename, const double& addppx,
                     const double& addppy) {
  ifstream ifs(filename.c_str());  
  if (!ifs) {
    perror(filename.c_str());
    return -1;
  }
  int thereturn = BBCIn(ifs, addppx, addppy);
  ifs.close();
  return thereturn;
}

int PMatrix::BBCIn(ifstream &ifs, const double& addppx,
                   const double& addppy) {
  Vector3<double> BBCC(0.0), BBCA(0.0), BBCUp(0.0);
  unsigned int BBCWidth=0, BBCHeight=0;
  // Pixelsize and FocalLength are in [m] (meter)
  double BBCPxSizeX=0, BBCPxSizeY=0, BBCCxShift=0, 
    BBCCyShift=0,BBCFocalLength=0;
  string key;
  istringstream iss;
  char linebuf[200];
  while (ifs.getline(linebuf,200)) {
//     cout <<"read line: "<<linebuf<<endl;
    iss.clear();
    iss.str(linebuf);
    iss >> key;
    if (key == "CVec")              iss >> BBCC[0]>> BBCC[1]>> BBCC[2];
    if (key == "AVec")              iss >> BBCA[0]>> BBCA[1]>> BBCA[2];
    if (key == "UpVec")             iss >> BBCUp[0]>> BBCUp[1]>> BBCUp[2];
    if (key == "Width")             iss >> BBCWidth;
    if (key == "Height")            iss >> BBCHeight;
    if (key == "PixelSizeX")        iss >> BBCPxSizeX;
    if (key == "PixelSizeY")        iss >> BBCPxSizeY;
    if (key == "CenterPointShiftX") iss >> BBCCxShift;
    if (key == "CenterPointShiftY") iss >> BBCCyShift;
    if (key == "FocalLength")       iss >> BBCFocalLength;
    if (key == "}") break; // end of entry
  }

  //cout <<"BBC CVec: "<<BBCC<<endl;
  // cout <<"BBC AVec: "<<BBCA<<" with norm="<< BBCA.NormL2()<<endl;
  // cout <<"BBC UpVec: "<<BBCUp<<" with norm="<< BBCUp.NormL2()<<endl;
  // cout <<"BBC Width: "<<BBCWidth<<endl;
  // cout <<"BBC Height: "<<BBCHeight<<endl;
  // cout <<"BBC PixelSizeX: "<<BBCPxSizeX<<endl;
  // cout <<"BBC PixelSizeY: "<<BBCPxSizeY<<endl;
  // cout <<"BBC CenterPointShiftX: "<<BBCCxShift<<endl;
  // cout <<"BBC CenterPointShiftY: "<<BBCCyShift<<endl;
  // cout <<"BBC FocalLength: "<<BBCFocalLength<<endl;
  
  Matrix3x3<double> K,R;

  K.SetIdentity();
  K[0][0] = BBCFocalLength / BBCPxSizeX; // fx 
  K[1][1] = BBCFocalLength / BBCPxSizeY; // fy

  // compute principle point, addppx and addppy are useful in case
  // the centerpointshift is determined separately
  K[0][2] = double(BBCWidth-1)/2.0  + (BBCCxShift+addppx) / BBCPxSizeX;
  K[1][2] = double(BBCHeight-1)/2.0 + (BBCCyShift+addppy) / BBCPxSizeY;

  Vector3<double> V,C,A,H;
  V = -1.0 * BBCUp;

  C[0] = BBCC[0];
  C[1] = BBCC[1];
  C[2] = BBCC[2];

  A[0] = BBCA[0];
  A[1] = BBCA[1];
  A[2] = BBCA[2];


   H = V.CrossProduct(A);

  for (unsigned int i=0;i<3;i++){
    R[i][0] = H[i];
    R[i][1] = V[i];
    R[i][2] = A[i];
  }

  // cout <<"K: "<<K<<endl;
  //cout <<"R: "<<R<<" with det "<<R.det()<<endl;
  //cout<<"PMatrix compile time is "<<__TIME__<<endl;
  
  Compose(K,R,C);

  return 0;
} 


#ifdef BIAS_HAVE_XML2

int PMatrix::XMLGetClassName(std::string& TopLevelTag,
                                double& Version) const {
  TopLevelTag = "PMatrix";
  Version = 0.1;
  return 0;
}

int PMatrix::XMLOut(const xmlNodePtr Node, XMLIO& XMLObject) const {
  if (IsMetric_) {
#ifdef BIAS_DEBUG
     if (!IsDecomposed_) {
       BIASERR("Undecomposed PMatrix cannot be metric !!!");
       BIASABORT;
     }
#endif
     xmlNodePtr childNode;
     //XMLObject.addAttribute(Node, "Metric", "true");
     XMLObject.addAttribute(Node, "Metric", true);

     // K
     childNode = XMLObject.addChildNode(Node,"Calibration");
     stringstream streamK, streamR;
     streamK << std::setprecision(STREAM_PRECISION);
     for (unsigned int i=0; i<9; i++) {
       streamK << (double) K_.GetData()[i] <<" ";
     }
     XMLObject.addContent(childNode, streamK.str());

     // R
     childNode = XMLObject.addChildNode(Node,"Rotation");
     streamR << std::setprecision(STREAM_PRECISION);
     for (unsigned int i=0; i<9; i++) {
       streamR << (double) R_.GetData()[i] <<" ";
     }
     XMLObject.addContent(childNode, streamR.str());

     // C
     childNode = XMLObject.addChildNode(Node,"Center");
     XMLObject.addAttribute(childNode, "x", C_[0]);
     XMLObject.addAttribute(childNode, "y", C_[1]);
     XMLObject.addAttribute(childNode, "z", C_[2]);
   } else {
     // not metric
     xmlNodePtr theNode;
     //XMLObject.addAttribute(Node, "Metric", "false");
     XMLObject.addAttribute(Node, "Metric", false);
     theNode = XMLObject.addChildNode(Node,"DataFields");
     stringstream datastream; 
     datastream << std::setprecision(STREAM_PRECISION);
     for (unsigned int i=0; i<12; i++) {
       datastream << (double) GetData()[i] <<" ";
     }
     XMLObject.addContent(theNode, datastream.str());
   }
   return 0;
}

int PMatrix::XMLIn(const xmlNodePtr Node, XMLIO& XMLObject) {
  xmlNodePtr childNode;
  string sMetric = XMLObject.getAttributeValueString(Node, "Metric");
  IsMetric_ = sMetric == "true";
  if (IsMetric_) {
    // K
    if ((childNode = XMLObject.getChild(Node, "Calibration"))==NULL) {
      BIASERR("error in xml structure");
      return -1;
    }
    // get data fields 
    stringstream ssCalibration(XMLObject.getNodeContentString(childNode));
    unsigned int CurrentData = 0;
    double* pData = K_.GetData();
    while (ssCalibration.good() && CurrentData<9) {
      ssCalibration >> (pData[CurrentData++]);
    }
    if (CurrentData<9) {
      // reached end of stream before expected end of data
      BIASERR("Error parsing XML code ...");
      return -1;
    }
    //cout <<"K is "<<K_<<endl;

    // R
    if ((childNode = XMLObject.getChild(Node, "Rotation"))==NULL) {
      BIASERR("error in xml structure");
      return -1;
    }
    // get data fields 
    stringstream ssRotation(XMLObject.getNodeContentString(childNode));
    CurrentData = 0;
    pData = R_.GetData();
    while (ssRotation.good() && CurrentData<9) {
      ssRotation >> (pData[CurrentData++]);
    }
    if (CurrentData<9) {
      // reached end of stream before expected end of data
      BIASERR("Error parsing XML code ...");
      return -1;
    }
    //cout <<"R is "<<R_<<endl;

    // C
    if ((childNode = XMLObject.getChild(Node, "Center"))==NULL) {
      // reached end of stream before expected end of data
      BIASERR("Error parsing XML code ...");
      return -1;
    }
    C_[0] = XMLObject.getAttributeValueDouble(childNode, "x");
    C_[1] = XMLObject.getAttributeValueDouble(childNode, "y");
    C_[2] = XMLObject.getAttributeValueDouble(childNode, "z");
    //cout <<"C is "<<C_<<endl;

    // Recompose P
    Compose(K_, R_, C_);
    //cout <<"R after Compose is "<<R_<<endl;
    IsMetric_ = true;
  } else {
    // in the non-metric case we just have 12 values to construct P
    if ((childNode = XMLObject.getChild(Node, "DataFields"))==NULL) {
      BIASERR("error in xml structure");
      return -1;
    }

    // get data fields 
    stringstream ssData(XMLObject.getNodeContentString(childNode));
    unsigned int CurrentData=0;
    double* pData = GetData();
    
    while (ssData.good() && CurrentData<12) {
      ssData >> (pData[CurrentData++]);
    }
    if (CurrentData<12) {
      // reached end of stream before expected end of data
      BIASERR("Error parsing XML code ...");
      return -1;
    }
    InvalidateDecomposition();
    IsMetric_ = false;
  }


  return 0;
}

#endif // BIAS_HAVE_XML2



bool  PMatrix::Save(const std::string & filename)
{
  std::ofstream fs( filename.c_str() );
  if (!fs) {
    return false; // error, could not open file. 
  } else {
    fs<<(*this);
    fs<<"# Covariance [x y z r1 r2 r3] : "<<endl;
    fs<<Covariance_<<endl;
    if (IsMetric_) {
      fs<<"# R: "<<R_<<endl;
      fs<<"# C: "<<C_<<endl;
      fs<<"# K: "<<K_<<endl;
    }
    fs.close();
  };
  return true;
}

double PMatrix::GetProjectionError(const std::vector<BIAS::HomgPoint2D>& points2D, const std::vector<BIAS::HomgPoint3D>& points3D){
  
  if (points2D.size() != points3D.size()) {
     // Error: impossible sizes of correspondence 
     BIASERR( "Vectors have different sizes " << points2D.size() <<"  "<< points3D.size() << endl);
     return -1.0;
   }
  double projectionError = 0.0;
  HomgPoint2D temp, temp2;
  for(unsigned int i = 0; i < points2D.size(); i++){
    temp = HomgPoint2D((*this) * points3D[i]);
    temp.Homogenize();
    temp2 = points2D[i];
    projectionError += temp.DistanceSquared(temp2);
  }
  projectionError /= points2D.size();
 
  return projectionError;
}

} // end namespace BIAS