PMatrixLinear.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 "./PMatrixLinear.hh"
#include "./RMatrix.hh"

using namespace BIAS;
using namespace std;


PMatrixLinear::PMatrixLinear()
{
  NewDebugLevel("PMATRIXLINEAR");
  //AddDebugLevel("PMATRIXLINEAR");
}


bool  PMatrixLinear::Compute(const std::vector<HomgPoint3D> &points3D,
                             const std::vector<HomgPoint2D> &points2D,
                             PMatrix& P)
{
  std::vector<HomgPoint3D *> ptrPoints3D;
  std::vector<HomgPoint2D *> ptrPoints2D;
  ptrPoints3D.resize(points3D.size());
  ptrPoints2D.resize(points2D.size());

  for (unsigned int i=0;i<points3D.size();i++) 
    ptrPoints3D[i]= const_cast<HomgPoint3D *>(&(points3D[i]));
  for (unsigned int i=0;i<points2D.size();i++) 
    ptrPoints2D[i]= const_cast<HomgPoint2D *>(&(points2D[i]));
  
  return Compute(ptrPoints3D,ptrPoints2D,P);
}

bool PMatrixLinear::Compute(const std::vector<HomgPoint3D*> &points3D,
                            const std::vector<HomgPoint2D*> &points2D,
                            PMatrix& P){
#ifdef BIAS_DEBUG
  if  (points2D.size()<points3D.size()) {
    BIASERR("Correspondence lists must have same size.");
    BIASABORT;
  }
  if  (points3D.size() < 6)
    BIASERR("For P-Matrix computation at least 6 points are needed. "<<
           "Points3D has only "<< points3D.size() << " points.");
#endif


  // set up "over-determined" equation system
  Matrix<double> A;

  // check whether we can set up an equation system A from the 2d-3d corrs
  if (!GetPEstSystemHom(A, points3D, points2D)) {
    BIASERR("no eq. system!");
    return false;
  }
  BCDOUT(PMATRIXLINEAR,"A = " << A);
  
  // make A well-conditioned
  A.NormalizeRows();
  BCDOUT(PMATRIXLINEAR,"A normalized rows = " << A);

  // get an SVD of A, if second smallest singvalue is too small we have no
  // unique solution, therefore set a relative threshold !
  SVD svd(A, 1e-6);
  
  // since P has 11 degrees of freedom, we need 5.5 independent 2d/3d
  // correspondences in order to get a 1-dimensional nullspace, i.e. a
  // P != 0 as a solution of AP=0
  
  // For six or more correspondences we need a least squares solution
  // with the additional constraint |P|=1
  // That is, we pick the last column of V, which refers to the smallest
  // singular value of S:
  BIAS::Matrix<double> VT = svd.GetVT();
  P.SetFromVector( VT.GetRow(11) );
  
#ifdef BIAS_DEBUG
  Vector<double> S;
  S = svd.GetS();
  BCDOUT(PMATRIXLINEAR,"Singular values in P-Estimation are "<<S
           <<" Rel nullspace dim is "<<svd.RelNullspaceDim()
           <<" zero thresh is "<<svd.GetZeroThreshold());
#endif
  if (svd.RelNullspaceDim()<=1) {
    return true;
  } else {
    //  BIASERR("Detected degenerate case in linear P estimation");
    return false;
  }
}
  
 
bool PMatrixLinear::GetPEstSystemHom(Matrix<double>& A,
                                     const std::vector<HomgPoint3D*>& points3D,
                                     const std::vector<HomgPoint2D*>& points2D) {

  if (points3D.size() != points2D.size()){
      
    BIASERR("pointlist2D has different length (=" << points2D.size()
           << ") than pointlist3D (=" << points3D.size() <<")");
    return false;
  }
    
  A.newsize(points3D.size()* 2, 12);

  for (unsigned i = 0; i < points3D.size(); i++) {

    HomgPoint2D const& u = *points2D[i];
    HomgPoint3D const& X = *points3D[i];
      
    unsigned row_index = i * 2;
    A[row_index][0] = X[0] * u[2];
    A[row_index][1] = X[1] * u[2];
    A[row_index][2] = X[2] * u[2];
    A[row_index][3] = X[3] * u[2];
    A[row_index][4] = 0;
    A[row_index][5] = 0;
    A[row_index][6] = 0;
    A[row_index][7] = 0;
    A[row_index][8] = -X[0] * u[0];
    A[row_index][9] = -X[1] * u[0];
    A[row_index][10] = -X[2] * u[0];
    A[row_index][11] = -X[3] * u[0];
      
    row_index = i * 2 + 1;
    A[row_index][0] =  0;
    A[row_index][1] =  0;
    A[row_index][2] =  0;
    A[row_index][3] =  0;
    A[row_index][4] =  X[0] * u[2];
    A[row_index][5] =  X[1] * u[2];
    A[row_index][6] =  X[2] * u[2];
    A[row_index][7] =  X[3] * u[2];
    A[row_index][8] = -X[0] * u[1];
    A[row_index][9] = -X[1] * u[1];
    A[row_index][10] = -X[2] * u[1];
    A[row_index][11] = -X[3] * u[1];
  }
    
  return true;
    
}


bool PMatrixLinear::GetPEstSystemInHom(Matrix<double>& A, Vector<double>& b,
                                       const std::vector<HomgPoint3D*>& points3D,
                                       const std::vector<HomgPoint2D*>& points2D) {

  if (points3D.size() != points2D.size()){
      
    BIASERR("pointlist2D has different length (=" << points2D.size()
            << ") than pointlist3D (=" << points3D.size() <<")");
    return false;
  }
    
  A.newsize(points3D.size()*2, 11);
  b.newsize(points3D.size()*2);

  for (unsigned i = 0; i < points3D.size(); i++) {

    HomgPoint2D const& u = *points2D[i];
    HomgPoint3D const& X = *points3D[i];
      
    unsigned row_index = i * 2;
    A[row_index][0] = X[0] * u[2];
    A[row_index][1] = X[1] * u[2];
    A[row_index][2] = X[2] * u[2];
    A[row_index][3] = X[3] * u[2];
    A[row_index][4] = 0;
    A[row_index][5] = 0;
    A[row_index][6] = 0;
    A[row_index][7] = 0;
    A[row_index][8] = -X[0] * u[0];
    A[row_index][9] = -X[1] * u[0];
    A[row_index][10] = -X[2] * u[0];
    b[row_index] = X[3] * u[0];
      
    row_index = i * 2 + 1;
    A[row_index][0] =  0;
    A[row_index][1] =  0;
    A[row_index][2] =  0;
    A[row_index][3] =  0;
    A[row_index][4] =  X[0] * u[2];
    A[row_index][5] =  X[1] * u[2];
    A[row_index][6] =  X[2] * u[2];
    A[row_index][7] =  X[3] * u[2];
    A[row_index][8] = -X[0] * u[1];
    A[row_index][9] = -X[1] * u[1];
    A[row_index][10] = -X[2] * u[1];
    b[row_index] = X[3] * u[1];
  }
    
  return true;
    
} 

bool PMatrixLinear::ComputeCalibrated(const vector<HomgPoint3D*> &points3D,
                                      const vector<HomgPoint2D*> &points2D,
                                      PMatrix& Pose) {

  BIASASSERT(points2D.size()>5);
  BIASASSERT(points3D.size()==points2D.size());
#ifdef BIAS_DEBUG
  for (unsigned int i=0;i<points3D.size(); i++) {
    if (!points3D[i]->IsHomogenized()) {
      BIASERR("Unhomogenized point !"<< *points3D[i]);
      BIASABORT;
    }
    if (!points2D[i]->IsHomogenized()) {
      BIASERR("Unhomogenized point !"<< *points2D[i]);
      BIASABORT;
    }
  }
#endif
  
  // set up model model matrix, 9 colums to estimate the 9 entries of R
  // for each pair of the n points contruct a line, and get one 
  // linear constraint on R. with 5 points, we have 10 constraints
  Matrix<double> ModelMatrix(points2D.size()*(points2D.size()-1)/2,9);

  unsigned int countrow=0;
  Vector3<double> B;
  Matrix3x3<double> C;
  for (unsigned int i=0; i<points2D.size(); i++) {
    Matrix3x3<double> CrossProd(points2D[i]->GetSkewSymmetricMatrix());
    for (unsigned int k=i+1; k<points2D.size(); k++) {
      // pick two points (k and i), construct 3D line B:
      B.Set((*points3D[k])[0]- (*points3D[i])[0],
            (*points3D[k])[1]- (*points3D[i])[1],
            (*points3D[k])[2]- (*points3D[i])[2]);
      // compute outer product matrix C of 3D line and image line
      B.OuterProduct(CrossProd * *points2D[k], C);

      // the image line through the 2D points, viewed in 3D space and rotated
      // by the desired rotation matrix must be coplanar to the plane 
      // defined by the two points and the camera center
      
      // copy from cross product matrix into model matrix:
      register double *pMM = ModelMatrix.GetDataArray()[countrow++];
      register double *pC = C.GetData();
      for (register unsigned int d=0; d<9; d++) *pMM++ = *pC++;
    }
  }
  SVD svd(ModelMatrix);

  // we estimated a projective entity lambda*R, however, the nullspace
  // must not have dimension>1, because this means an ambiguity
  if (svd.RelNullspaceDim()>1) {
    Vector<double> S=svd.GetS();
    BCDOUT(PMATRIXLINEAR,
             "Detected degenerate case in linear Rotation estimation."
            <<"Dim of Nullspace is "<<svd.RelNullspaceDim()
            <<" with sing values: "<<S);
    return false;
  }

  // ignore null threshold smallest on singular value, get nullvector:
  Vector<double> RVector(svd.GetVT().GetRow(8));
 
  // compose rmatrix from rvector 
  // we have to scale R, so that it comes close to a rotation
  RMatrix R;

  // first row should have length 1:
  //const double scale = 1.0 / 
  //  sqrt(RVector[0]*RVector[0]+RVector[1]*RVector[1]+RVector[2]*RVector[2]);
  //ROTATION_MATRIX_TYPE* pR = R.GetData();
  //for (unsigned int j=0; j<9; j++) *pR++ = RVector[j]*scale;
  
  //  ROTATION_MATRIX_TYPE* pR = R.GetData(); 
  //for (unsigned int j=0; j<9; j++) *pR++ = RVector[j]; 
  R.SetFromVector(RVector);

  SVD svdR((Matrix<ROTATION_MATRIX_TYPE>)R);
  R = svdR.GetU() * svdR.GetVT();

  // enforce right-handed rotation
  if (R.GetDeterminant()<0.0) R *= -1.0;

  // BIASWARN("using ground truth rotation");
  // R = Pose.GetR();

  // here we have something close to a rotation, trust this and compute C
  // set up DLT equation system where only C is estimated 
  // start with R*x ~= [  I  |-C]X, rearrange and solve M * CVector = 0
  Matrix<double> M(points3D.size()*3,4);
  register unsigned int CurRowInM = 0;
  for (unsigned int i=0; i<points3D.size(); i++) {
    Vector3<double> Rx = R * *(points2D[i]);
    Matrix<double> Rx_cross = Rx.GetSkewSymmetricMatrix();
    for (unsigned int r=0; r<3; r++) {
      M[CurRowInM][0] = Rx_cross[r][0];
      M[CurRowInM][1] = Rx_cross[r][1];
      M[CurRowInM][2] = Rx_cross[r][2];
      Vector3<double> X((*points3D[i])[0],(*points3D[i])[1],(*points3D[i])[2]);
      X = Rx_cross * X;
      M[CurRowInM++][3] = -X[r];
    }
  }
  M.NormalizeRows();
  SVD svdC(M);

  // check for ambiguity (multi-dim. nullspace), which we cant decide
  if (svdC.RelNullspaceDim()>1) {
    Vector<double> c = svdC.GetS();
    BCDOUT(PMATRIXLINEAR,
             "Detected degenerate case in linear center estimation."
            <<"Dim of Nullspace is "<<svdC.RelNullspaceDim()
            <<" with sing values: "<<c);
    return false;
  }
  // pick null vector of smallest sing value (ignore threshold)
  HomgPoint3D CVector(svdC.GetVT().GetRow(3));
  
  // compute center by homogenizing
  Vector3<double> Center(CVector[0]/CVector[3], CVector[1]/CVector[3], 
                         CVector[2]/CVector[3]);

  KMatrix K;
  K.SetIdentity();
  Pose.Compose(K, R, Center);
  return true;
}