HMatrixEstimation.cpp

#include "HMatrixEstimation.hh"

#include <MathAlgo/SVD.hh>
#include <MathAlgo/Lapack.hh>
#include <Base/Geometry/KMatrix.hh>
#include "Parametrization.hh"
#include "MathAlgo/Minpack.hh"

using namespace BIAS;
using namespace std;

int HMatrixEstimation::
HErrorFunction_(void *p, int m, int n, const double *x, double *fvec, int iflag)
{
  HMatrixEstimation *pCurrentObjectH = (HMatrixEstimation *)p;
  vector<double> params;
  params.reserve(8);
  for (unsigned int i = 0; i < 8; i++) params.push_back(x[i]);
  BIAS::HMatrix H;
  Parametrization *ParaH = new Parametrization;
  ParaH->ParamsToHMatrix(H, params);
   
  HMatrix Hinv;
  int res = H.GetInverse(Hinv); 
  if (res != 0) {
    iflag = -42;
    return -1;
  }
  BIASASSERT(res == 0); 
  
  const unsigned int uiDataSize = 4*pCurrentObjectH->pointsFrom_.size();
  const vector<BIAS::HomgPoint2D> &p1 = pCurrentObjectH->pointsFrom_;
  const vector<BIAS::HomgPoint2D> &p2 = pCurrentObjectH->pointsTo_;

  double errorsum = 0;
  for (unsigned int j = 0; j < uiDataSize; j+=4) {
    HomgPoint2D ep1 = Hinv*p2[j/4];
    ep1.Homogenize();
    fvec[j]   = ep1[0]-p1[j/4][0];
    fvec[j+1] = ep1[1]-p1[j/4][1];
    HomgPoint2D ep2 = H*p1[j/4];
    ep2.Homogenize();
    fvec[j+2] = ep2[0]-p2[j/4][0];
    fvec[j+3] = ep2[1]-p2[j/4][1];
    errorsum += (ep1-p1[j/4]).NormL2() +(ep2-p2[j/4]).NormL2(); 
  }
  delete ParaH;
  return 0;
}

int HMatrixEstimation::
Optimize(BIAS::HMatrix& H, const std::vector<BIAS::HomgPoint2D> &p1, 
         const std::vector<BIAS::HomgPoint2D> &p2)
{
  BIASASSERT(p1.size() == p2.size());
  if (p1.size() < 5) {
    BIASERR("At least 5 points correspondences are needed for homography optimization!");
    return HE_NOT_ENOUGH_POINTS;
  }

  // We need to call an external function from Levenberg-Marquardt,
  // prepare data structures here. Warning: This section is not thread-safe!
  pointsFrom_ = p1;
  pointsTo_ = p2;

  vector<double> vecHParamsStart, vecHParamsResult; 
  Vector<double> HParamsStart(8), HParamsResult(8);
  Parametrization *ParaH = new Parametrization;
  ParaH->HMatrixToParams(H, vecHParamsStart);
  for (unsigned int i = 0; i < 8; i++)
    HParamsStart[i] = vecHParamsStart[i];
  long int res = LevenbergMarquardt(&HErrorFunction_, this,p1.size()*4,8,
                                    HParamsStart, HParamsResult,
                                    MINPACK_DEF_TOLERANCE);
  if (res != 0) {
    BIASERR("Error in Levenberg-Marquardt optimization of homography!");
    return HE_LM_ERROR;
  }

  vecHParamsResult.resize(8);
  for (unsigned int i=0; i<8; i++)  vecHParamsResult[i] = HParamsResult[i];
  ParaH->ParamsToHMatrix(H, vecHParamsResult);
  delete ParaH;
  ParaH = NULL;

  H_ = H;
  
  return HE_OK;
}

int HMatrixEstimation::
Compute(const vector<BIAS::HomgPoint2D>& points1,
        const vector<BIAS::HomgPoint2D>& points2, HMatrix& H)
{
  int result = 0;
  BIASASSERT(points1.size() == points2.size());
  if (points1.size() < 4) {
    BIASERR("At least 4 points correspondences are needed for homography estimation!");
    return HE_NOT_ENOUGH_POINTS;
  }
  pointsFrom_ = points1;
  pointsTo_ = points2;

  const std::vector<BIAS::HomgPoint2D> *pp1 = &points1;
  const std::vector<BIAS::HomgPoint2D> *pp2 = &points2;
  std::vector<BIAS::HomgPoint2D> p1norm;
  std::vector<BIAS::HomgPoint2D> p2norm;
  KMatrix K1, K2;
  if (normalizeHartley_) {
    normalization_.Hartley(points1, p1norm, K1);
    normalization_.Hartley(points2, p2norm, K2);
    pp1 = &p1norm;
    pp2 = &p2norm;
  }

  Matrix<double> A(2*points1.size(), 9);
  Vector<double> S;
  Matrix<double> U, VT;

  register unsigned int p=0, p2=0;
  for (p=0, p2=0; p<2*(*pp1).size(); p+=2, p2++){
 
    A[p][0] = A[p][1] = A[p][2] = 0.0;
    A[p+1][3] = A[p+1][4] = A[p+1][5] = 0.0;

    A[p+1][0] = (*pp2)[p2][2] * (*pp1)[p2][0];
    A[p+1][1] = (*pp2)[p2][2] * (*pp1)[p2][1];
    A[p+1][2] = (*pp2)[p2][2] * (*pp1)[p2][2];

    A[p][3] = - A[p+1][0];
    A[p][4] = - A[p+1][1];
    A[p][5] = - A[p+1][2];

    A[p][6] = (*pp2)[p2][1] * (*pp1)[p2][0];
    A[p][7] = (*pp2)[p2][1] * (*pp1)[p2][1];
    A[p][8] = (*pp2)[p2][1] * (*pp1)[p2][2];
    
    A[p+1][6] = - (*pp2)[p2][0] * (*pp1)[p2][0];
    A[p+1][7] = - (*pp2)[p2][0] * (*pp1)[p2][1];
    A[p+1][8] = - (*pp2)[p2][0] * (*pp1)[p2][2];
  }
  
  result = General_singular_value_decomposition(A, S, U, VT);
 
  if (result == 0){
    for (p=0; p<9; p++){
      H.GetData()[p] = VT[8][p];
    }
  } else {
    H.SetZero();
  }

  // undo Hartley normalization
  if (normalizeHartley_) H = K2.Invert() * H * K1;

  H_ = H;

  return result;
}

int HMatrixEstimation::
ComputeAffine(const vector<BIAS::HomgPoint2D>& points1,
              const vector<BIAS::HomgPoint2D>& points2, HMatrix& H)
{
  // Use direct linear transformation on point pairs (p, p')
  // solving p x (H * p') = 0 which leads to A * h = 0
  // where A is a matrix computed from p and p' and h is the column-wise
  // vector representation for matrix H

  BIASASSERT(points1.size() == points2.size());
  
  pointsFrom_ = points1;
  pointsTo_ = points2;

  // perform Hartley normalization
  const std::vector<BIAS::HomgPoint2D> *pp1 = &points1;
  const std::vector<BIAS::HomgPoint2D> *pp2 = &points2;
  std::vector<BIAS::HomgPoint2D> p1norm;
  std::vector<BIAS::HomgPoint2D> p2norm;
  KMatrix K1, K2;
  if (normalizeHartley_) {
    normalization_.Hartley(points1, p1norm, K1);
    normalization_.Hartley(points2, p2norm, K2);
    pp1 = &p1norm;
    pp2 = &p2norm;
  }

  // initialize model matrix with zero
  Matrix<double> A(2*points1.size(), 7, MatrixZero);
  H.SetIdentity();

  // fill rest of model matrix, same as for general H case, but leaving 
  // out last row of H (only 7 parameters of which one is redundant)  
  const unsigned int nummeas = 2*(*pp1).size();
  for (register unsigned int p=0, p2=0; p<nummeas; p+=2, p2++){
    A[p][3]   = -(A[p+1][0] = (*pp2)[p2][2] * (*pp1)[p2][0]);
    A[p][4]   = -(A[p+1][1] = (*pp2)[p2][2] * (*pp1)[p2][1]);
    A[p][5]   = -(A[p+1][2] = (*pp2)[p2][2] * (*pp1)[p2][2]);
    
    A[p][6]   =   (*pp2)[p2][1] * (*pp1)[p2][2];
    A[p+1][6] = - (*pp2)[p2][0] * (*pp1)[p2][2];
  }
  
  // solve using SVD (at least 1d solution space)
  Vector<double> S;
  Matrix<double> U, VT; 
  if (General_singular_value_decomposition(A, S, U, VT) == 0){
    // normalize to H[2][2] = 1
    const HMATRIX_TYPE normalization = 1.0 / VT[6][6];
    HMATRIX_TYPE* pHData = H.GetData();
    for (int p = 0; p < 6; p++){
       *pHData++ = VT[6][p] * normalization;
    }    
  } else {
    // SVD failed, no solution
    return HE_SVD_ERROR;
  }

  // undo Hartley normalization
  if (normalizeHartley_) H = K2.Invert() * H * K1;

  H_ = H;

  return HE_OK;
}

int HMatrixEstimation::
Compute(const std::vector<Vector2<double> >& fromPoint, 
        const std::vector<Vector2<double> >& toPoints, HMatrix &H)
{
  BIASASSERT(fromPoint.size() == toPoints.size());
  vector<BIAS::HomgPoint2D> fromPointHomg(fromPoint.size());
  vector<BIAS::HomgPoint2D> toPointsHomg(toPoints.size());
  for (unsigned int i = 0; i < fromPoint.size(); i++) {
    fromPointHomg[i][0]= fromPoint[i][0];
    fromPointHomg[i][1]= fromPoint[i][1];
    fromPointHomg[i][2]= 1.0;
    toPointsHomg[i][0]= toPoints[i][0];
    toPointsHomg[i][1]= toPoints[i][1];
    toPointsHomg[i][2]= 1.0;    
  }
  return Compute(fromPointHomg, toPointsHomg, H);
}

double HMatrixEstimation::GetError() const
{
  double err = 0;
  BIAS::HomgPoint2D mapped;
  for (unsigned int c = 0; c < pointsFrom_.size(); c++) {
    mapped = H_ * pointsFrom_[c];
    mapped.Homogenize();
    mapped -= pointsTo_[c];
    err += mapped[0]*mapped[0] + mapped[1]*mapped[1];
  }
  err /= (double)pointsFrom_.size();
  return err;
}