TestHMatrix.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/Math/Random.hh>
#include <Geometry/HMatrix.hh>
#include <Geometry/HMatrixEstimation.hh>
#include <Geometry/RMatrix.hh>

using namespace BIAS;
using namespace std;

/**
   @file TestHMatrix.cpp
   @brief testing HMatrix and HMatrixEstimation
   @relates HMatrix HMatrixEstimation
   @ingroup g_tests
   @author MIP
*/
int main(int argc, char *argv[])
{

  Random rand;
  
  // compute PMatrices for testing
  KMatrix k1;
  KMatrix k2;
  k1.SetIdentity();
  k1[0][0] = k1[1][1] = rand.GetUniformDistributed(200, 400);
  k1[0][2] = k1[1][2] = k1[0][0]/2.0;
  k2.SetIdentity();
  k2[0][0] = k2[1][1] = rand.GetUniformDistributed(200, 400);
  k2[0][2] = k2[1][2] = k2[0][0]/2.0;
    
  RMatrix r1;
  RMatrix r2;
  r1.SetXYZ(rand.GetUniformDistributed(0, 180) * M_PI/180.0, 
            rand.GetUniformDistributed(0, 180) * M_PI/180.0, 
            rand.GetUniformDistributed(0, 180) * M_PI/180.0);
  r2.SetXYZ(rand.GetUniformDistributed(0, 180) * M_PI/180.0,
            rand.GetUniformDistributed(0, 180) * M_PI/180.0, 
            rand.GetUniformDistributed(0, 180) * M_PI/180.0);
  
  Vector3<double> c1(rand.GetUniformDistributed(-100, 100),
                     rand.GetUniformDistributed(-100, 100),
                     rand.GetUniformDistributed(-100, 100));
  Vector3<double> c2(rand.GetUniformDistributed(-100, 100),
                     rand.GetUniformDistributed(-100, 100),
                     rand.GetUniformDistributed(-100, 100));
 
  PMatrix p1(k1, r1, c1);
  PMatrix p2(k2, r2, c2);
  
   
  // testing Normalization
  HMatrix H(p1, p2);
  H = 1.0/H[2][2] * H;
  
  
  // map across plane at infinity 
  HMatrix H1(p1, p2);
  H1 = 1.0/H1[2][2] * H1;
  HMatrix H_gt = k2*r2*r1.Transpose()*k1.Invert();
  H_gt = 1.0/H_gt[2][2] * H_gt;  
  
  if(H1 != H_gt){
    return -1;
  }
  
 
  
  // create ground truth HMatrix and point sets for estimation
  HMatrix H_gt2;
  for(unsigned int i=0; i < 3; i++){
    for(unsigned int j=0; j < 3; j++){
      H_gt2[i][j] = rand.GetUniformDistributed(-100, 100);
    }
  }
  H_gt2[2][2] = 1.0;
  
  unsigned int num_points = 5;
  vector<HomgPoint2D> points2D_1;
  vector<HomgPoint2D> points2D_2;
  HomgPoint2D p2d;
  for(unsigned int i=0; i < num_points; i++){
    p2d = HomgPoint2D(rand.GetUniformDistributed(-100, 100),
                      rand.GetUniformDistributed(-100, 100));
    points2D_1.push_back(p2d);
    p2d = H_gt2 * points2D_1[i];
    p2d.Homogenize();
    points2D_2.push_back(p2d);
  }

  // estimation of H based on points
  HMatrixEstimation estimator;
  HMatrix Hest;
  estimator.Compute(points2D_1, points2D_2, Hest);
  Hest = 1.0/Hest[2][2] * Hest;
  double residual = Hest.GetReprojectionError(points2D_1, points2D_2);
  if(residual > 1e-12){
    return -1;
  }
  
  HMatrix Hopt = Hest;
  estimator.Optimize(Hopt, points2D_1, points2D_2);
  Hopt = 1.0/Hopt[2][2] * Hopt;
  residual = Hopt.GetReprojectionError(points2D_1, points2D_2);
  if(residual > 1e-12){
    return -1;
  }
    
  cout << "done, passed" << endl;
  
  return 0;
}