HMatrixTest.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
*/

/**
   @example HMatrixTest.cpp
   @relates hMatrix, HMatrixEstimation
   @brief example demonstrating H-Matrix
   @ingroup g_examples
   @author MIP
*/

// BIAS
#include "Geometry/HMatrixEstimation.hh"
#include "Base/Math/Random.hh"

using namespace std;
using namespace BIAS;

// ---------------------------------------------------------------------------
int main(int /*argc*/, char** /*argv*/) 
{
    
  HMatrix TheoH(MatrixIdentity), EstH(MatrixZero);

  // set homography TheoH
  TheoH[0][0]=1.5; TheoH[0][1]=  0; TheoH[0][2]=10; 
  TheoH[1][0]=  0; TheoH[1][1]=2.1; TheoH[1][2]=100;
  TheoH[2][0]=  0; TheoH[2][1]=  0; TheoH[2][2]=1;   
  // H-matrix
  std::vector<HomgPoint2D> points_src;
  std::vector<HomgPoint2D> points_dest, points_dest_with_out;
  
  Random R;
  const double noise = 0.3;

  // build pointsets 2D for computation
  for (int i = 0; i <100; i++){ 
    // 2D point
    HomgPoint2D point2d_im1;
    for (int k=0; k<2; k++)
      // point coordinate between 0 and 1000
      point2d_im1[k]=1000.0*rand()/(RAND_MAX+1.0);
    point2d_im1[2]=1;

    // project the point2d to the second image
    HomgPoint2D point2d_im2 = TheoH * point2d_im1;
    point2d_im2.Homogenize();


    point2d_im1[0] += R.GetNormalDistributed(0, noise);
    point2d_im2[0] += R.GetNormalDistributed(0, noise);
    point2d_im1[1] += R.GetNormalDistributed(0, noise);
    point2d_im2[1] += R.GetNormalDistributed(0, noise);
    
    // build correspondences
    points_src.push_back(point2d_im1);
    points_dest.push_back(point2d_im2);
  }
  
  // estimation of H (linearly)
  HMatrixEstimation HEstimator;
  HEstimator.ComputeAffine(points_src, points_dest, EstH);

  // normalize H to contain a one in the lower right corner
  if (fabs(EstH[2][2])>1e-50) EstH /= EstH[2][2];

  cout << "H ground truth is " << TheoH << endl;


  cout << "H Est is " << EstH << endl;
 
  return 0;
}