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

#include <MathAlgo/SVD.hh>
#include <MathAlgo/Lapack.hh>
#include <Geometry/RMatrix.hh>



using namespace BIAS;
using namespace std;

HMatrix::HMatrix() :
  HMatrixBase() {
}

HMatrix::HMatrix(Matrix3x3<double> Mat) :
  HMatrixBase(Mat) {
}

HMatrix& HMatrix::operator=(const HMatrix& mat)

{
  HMatrixBase::operator=(mat);
  return *this;
}

HMatrix::~HMatrix() {
}


HMatrix HMatrix::GetLinearized(const HomgPoint2D& x) const {
  HMatrix HTaylor(MatrixZero);
  Matrix<double> Jac(2,2,MatrixZero);
  GetJacobian(x,Jac);
  for (unsigned int i=0; i<2; i++) {
    for (unsigned int j=0; j<2; j++) {
      HTaylor[i][j] = Jac[i][j];
    }
  }
  HomgPoint2D y = (*this) * x;
  if (fabs(y[2])<1e-9) {
    // linearization point is at infinity, this doesnt give much information on
    // finite points
    BIASERR("Cannot linearize homography near infinity");
    // return zero matrix here ???
    HTaylor[2][2] = 0.0;
  } else {
    y.Homogenize();
    HTaylor[2][2] = 1.0;
  }  
  y = y - HTaylor * x;
  for (unsigned int i=0; i<2; i++) HTaylor[i][2] = y[i];
  return HTaylor;
}

void HMatrix::
MapAcrossPlane(const PMatrix& P1o, const PMatrix& P2o,
               const HomgPlane3D& scenePlane) {
  // (0 0 0 0) is no valid plane:
  BIASASSERT(scenePlane.NormL2()>1e-5);

  PMatrix P1(P1o);
  PMatrix P2(P2o);
  Vector3<double> C1,C2;
  KMatrix K1, K2;
  RMatrix R1, R2;
  P1.GetK(K1);
  P1.GetR(R1);
  P1.GetC(C1);
  P2.GetK(K2);
  P2.GetR(R2);
  P2.GetC(C2);
  // cout<<"R1="<<R1<<" C1 "<<C1<<endl;
  Vector3<double> normal(scenePlane[0], scenePlane[1], scenePlane[2]);
  double norm = normal.NormL2();
  if (norm/scenePlane.NormL2()<1e-9) {
    //BIASWARN("using infinite homography");
    // plane at infinity => infinity homography
//    (*this) = K1.Invert()*R1.Transpose()*R2*K2;
    (*this) = K2*R2*R1.Transpose()*K1.Invert();
    return;
  }
  normal /= norm;
  Vector3<double> point((-scenePlane[3]/norm)*normal);

  //cout<<"point on plane is "<<point<<", normal is "<<normal<<endl;
#ifdef BIAS_DEBUG
  // make sure point is on plane
  double tester = 0;
  HomgPoint3D(point).ScalarProduct(scenePlane, tester);
  BIASASSERT(fabs(tester)<1e-3);
#endif

  // now we change the coordinate system such that P1 is canonic
  R2 = R1.Transpose()*R2;
  C2 = R1.Transpose()*(C2-C1);
  normal = R1.Transpose()*normal;
  point = R1.Transpose()*(point-C1);

  //cout<<"R2="<<R2<<" C2="<<C2<<" normal="<<normal<<" point="<<point<<endl;
  // and use the planar mapping ...
  (*this) = point.ScalarProduct(normal)*R2.Transpose() - 
    R2.Transpose()*(C2.OuterProduct(normal));
  // finally, add calibrations
  (*this) = K2*(*this)*K1.Invert();
}




void HMatrix::GetMappedRange(const unsigned int OrigWidth, 
                             const unsigned int OrigHeight,
                             BIAS::Vector2<double> & corner,
                             double & sizeWidth, double & sizeHeight) const {

  double minX, maxX, minY, maxY; // extrema

  // rect. coordinates of the 4 src. corners.
  BIAS::HomgPoint2D mapped[4];

  // 0  3
  // 1  2
  mapped[0]= (*this) * BIAS::HomgPoint2D(0, 0, 1);
  // H * '0' left,  upper

  mapped[1]= (*this) * BIAS::HomgPoint2D(0, OrigHeight, 1);
  // H * '1' left,  lower

  mapped[2]= (*this) * BIAS::HomgPoint2D(OrigWidth, OrigHeight, 1);
  // H * '2' right, lower

  mapped[3]= (*this) * BIAS::HomgPoint2D(OrigWidth, 0, 1);
  // H * '3' right, upper

  // calc. extrema(xmy) of mapped corners
  // init with first point:
  mapped[0].Homogenize(); // let w==1
  minX=maxX=mapped[0][0]; // x euclid
  minY=maxY=mapped[0][1]; // y suclid  
  // homogenize and calc extrema
  for (unsigned int i=1; i<4; i++) {
    mapped[i].Homogenize();
    // find new extrema
    minX = (mapped[i][0]<minX) ? mapped[i][0] : minX; // x
    maxX = (mapped[i][0]>maxX) ? mapped[i][0] : maxX;
    minY = (mapped[i][1]<minY) ? mapped[i][1] : minY; // y
    maxY = (mapped[i][1]>maxY) ? mapped[i][1] : maxY;
  };

  corner=BIAS::Vector2<double>(minX, minY);
  sizeWidth=maxX-minX;
  sizeHeight=maxY-minY;
}

double HMatrix::GetMappedError(const HomgPoint2D &PointPicOne, const HomgPoint2D &PointPicTwo) const {

  HomgPoint2D x1 = PointPicOne;
  HomgPoint2D x2 = PointPicTwo;
  x1.Homogenize();
  x2.Homogenize();
  HomgPoint2D mapped((*this) * x1);
  mapped.Homogenize();
  return (mapped[0]-x2[0])*(mapped[0]-x2[0]) + (mapped[1]-x2[1])*(mapped[1]-x2[1]);
}

void HMatrix::GetJacobian(const HomgPoint2D& x, Matrix<double>& Jac) const {
#ifdef BIAS_DEBUG
  if (!x.IsHomogenized()) {
    BIASERR("supplied unhomogenized point !");
    BIASABORT;
  }
  if ((Jac.GetRows()!=2) ||(Jac.GetCols()!=2)) {
    BIASERR("Jacobian has wrong size !");
    BIASABORT;
  }
#endif

  // determine denominator w of mapped point
  const double factor = ((*this)[2][0]*x[0]+(*this)[2][1]*x[1]+(*this)[2][2]);
  // Is the point mapped to the line at infinity?
  if (fabs(factor)<1e-10) {
    // Jac will be complicated to obtain with lots of special cases.
    Jac.SetZero();
    // This catches finite points mapped to infinity in a very rough sense:
    // there may be cases where lim(dH/dx) is not simply infinity but does 
    // not exist or where one might even obtain a finite value using l'Hospital
    // Running along the line at infinity will typically have a different
    // derivative than runnig orthogonally.
    // Such cases cannot be handled well in 2D euclidean space and someone
    // should think of a correct implementation here.
    // For now, we simply set a huge value to avoid division by zero.
    BIASWARN("Trying to obtain Jacobian of point mapped to infinity");
    if ((*this)[0][0] * (*this)[2][1]*x[1] + (*this)[0][0]*(*this)[2][2] - (*this)[0][1]*x[1]*(*this)[2][0] - (*this)[0][2]*(*this)[2][0]<0.0)
      Jac[0][0] = -HUGE_VAL;
    else
      Jac[0][0] = HUGE_VAL;

    if ((*this)[1][0]*x[0]*(*this)[2][1] - (*this)[1][1]*(*this)[2][0]*x[0] - (*this)[1][1]*(*this)[2][2] + (*this)[1][2]*(*this)[2][1] < 0.0)
      Jac[1][1] = HUGE_VAL;
    else
      Jac[1][1] = -HUGE_VAL;
    return;
  }

  // make scaled version to improve conditioning
  HMatrix scaled((*this)/factor);

  // ( x' y' )^T = H( (x y)^T), assume w==w'==1
  const double enumx = (scaled[0][0]*x[0]+scaled[0][1]*x[1]+scaled[0][2]);
  const double enumy = (scaled[1][0]*x[0]+scaled[1][1]*x[1]+scaled[1][2]);

  // x'/dx
  Jac[0][0] = (scaled[0][0] - scaled[2][0]*enumx);
  // x'/dy
  Jac[0][1] = (scaled[0][1] - scaled[2][1]*enumx);
  // y'/dx                                        
  Jac[1][0] = (scaled[1][0] - scaled[2][0]*enumy);
  // y'/dy
  Jac[1][1] = (scaled[1][1] - scaled[2][1]*enumy);


}

int HMatrix::FactorizeAffineMatrixRLeft(const Matrix2x2<double>& A, Matrix2x2<double>& phi,
                                        Matrix2x2<double>& theta, double& d1, double& d2) {
  // TODO: use svd2x2 here !
  SVD mysvd;
  if (mysvd.Compute(A)!=0)
    return -1;
  d1 = mysvd.GetS()[0];
  d2 = mysvd.GetS()[1];
  phi = mysvd.GetVT();
  theta = mysvd.GetU() * phi;
  // theta[0][0] = theta[1][1] = 0.5 * theta[0][0] + 0.5 * theta[1][1];
  return 0;
}

int HMatrix::FactorizeAffineMatrixRRight(const Matrix2x2<double>& A, Matrix2x2<double>& phi,
                                         Matrix2x2<double>& theta, double& d1, double& d2) {
  // TODO: use svd2x2 here !
  SVD mysvd;

  int res = mysvd.Compute(A);
  if (res!=0) {
    BIASERR("Factorization of affine matrix failed ..."<<res<<flush);
    return -1;
  }
  d1 = mysvd.GetS()[0];
  d2 = mysvd.GetS()[1];
  phi = mysvd.GetU().Transpose();
  theta = mysvd.GetU() * mysvd.GetVT();
  // theta[0][0] = theta[1][1] = 0.5 * theta[0][0] + 0.5 * theta[1][1];
  return res;
}

int HMatrix::ComposeAffineMatrixRLeft(Matrix2x2<double>& A, const Matrix2x2<double>& phi,
                                      const Matrix2x2<double>& theta, const double& d1,
                                      const double& d2) {
  int res = 0;
  Matrix2x2<double> d(MatrixZero);
  d[0][0] = d1;
  d[1][1] = d2;

  A = theta * phi.Invert() * d * phi;
  return res;
}

int HMatrix::ComposeAffineMatrixRRight(Matrix2x2<double>& A, const Matrix2x2<double>& phi,
                                       const Matrix2x2<double>& theta, const double& d1,
                                       const double& d2) {
  int res = 0;
  Matrix2x2<double> d(MatrixZero);
  d[0][0] = d1;
  d[1][1] = d2;
  A = phi.Invert() * d * phi * theta;
  return res;
}

double HMatrix::GetReprojectionError(std::vector<BIAS::HomgPoint2D>& p1,
                                     std::vector<BIAS::HomgPoint2D>& p2) {
  
   HomgPoint2D x1,x2, mapped1, mapped2;
   
   if(p1.size() != p2.size()) BIASWARN("HMatrix::GetReprojectionError HomgPoint2D vectors have different sizes!");
   int size = (p1.size()<p2.size())? p1.size():p2.size();
   
   double error = 0;
   Matrix3x3<double> invH = (*this);
   invH.InvertIP();
   for(int i=0; i < size; i++){
    x1 = p1[i];
    x2 = p2[i];
    x1.Homogenize();
    x2.Homogenize();
    mapped1 = ((*this) * x1);
    mapped1.Homogenize();
    mapped2 = invH * x2;
    mapped2.Homogenize();
    error += mapped1.DistanceSquared(x2);
    error += mapped2.DistanceSquared(x1);
   }
   error /= size;
   return error;
}


double HMatrix::
ComputeLinearizationError(const HomgPoint2D& x0,
                          const HomgPoint2D& y) const {
  
  HomgPoint2D truePoint = (*this)*y;
  truePoint.Homogenize();
  HMatrix HLin = this->GetLinearized(x0);
  HomgPoint2D approxPoint = HLin * y;
  approxPoint.Homogenize();

  return (truePoint-approxPoint).NormL2();
}