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


/**
   @file TestProjectionParametersPerspective.cpp
   @brief Unit test for basic functionality of projection parameters perspective.
   @relates ProjectionParametersPerspective
   @ingroup g_tests
   @author MIP
*/


#include <Base/Common/CompareFloatingPoint.hh>

#include <Geometry/ProjectionParametersPerspective.hh>
#include <Base/Math/Random.hh>
#include <string>
#include <vector>


using namespace BIAS;
using namespace std;


#define EPSILON       1e-8


int main(int argc, char *argv[])
{
  // Future ocean cam
//  const unsigned int width = 960;
//  const unsigned int height = 540;

//  ProjectionParametersPerspective ppp(width, height);
//  ppp.SetUndistortion(-0.1483904123, 0.02849210998, 0,0);
//  ppp.SetFocalLengthAndAspect(869.0050672, 0.9909530544);
//  ppp.SetPrincipal(479.8758225, 258.0573953);
   
  // ingos cam
  const unsigned int width = 1024;
  const unsigned int height = 768;
  ProjectionParametersPerspective ppp(width, height);
  ppp.SetUndistortionInverseRad(-0.2696871916, 0.1517598534, 0.002169664669, 0.001128644153);
  ppp.SetFocalLengthAndAspect(1129.290664, 0.9905520182);
  ppp.SetPrincipal(561.5692167, 348.5405689);
  
  Quaternion<double> Q(0,0,0,1);
  ppp.SetQ(Q);
  Vector3<double> C(0,0,0);
  ppp.SetC(C);
  
  Random rand;
  int percent = -1;

  int countError = 0, countAll = 0;
  double sumError = 0.0, sumSqError = 0.0;
  
  //double px = width/2.0-0.5;
  //double py = height/2.0-0.5;
  
  unsigned int numCams = 100;   //1000
  unsigned int numPoints = 100; //1000
  
  HomgPoint2D p2d, p2d_p;
  HomgPoint3D p3d;
  double maxError = 0.0;
  double dist=0, error=0;

  for (unsigned int cams=0; cams < numCams; cams++)
  {
    ppp.SetFocalLengthAndAspect(rand.GetUniformDistributed(700, 900), 1.0);
    ppp.SetPrincipal(rand.GetUniformDistributed(500, 600),
                     rand.GetUniformDistributed(300, 400));
    ppp.SetUndistortionInverseRad(rand.GetUniformDistributed(-0.2, -0.1),
                        rand.GetUniformDistributed(0.0, 0.03), 0,0);
//                      rand.GetUniformDistributed(-0.015, 0.015),
//                      rand.GetUniformDistributed(-0.015, 0.015));
    
    for (unsigned int points=0; points < numPoints; points++)
    {
      if (points == 0) {
        // use origin as first 2d point
        p2d[0] = p2d[1] = 0.0;
        p2d[2] = 1.0;
        dist = 5000.0;
      } else {
        // generate random 2d point and depth
        p2d[0] = rand.GetUniformDistributed(10, width-11); 
        p2d[1] = rand.GetUniformDistributed(10, height-11);
        p2d[2] = 1.0;
        dist = rand.GetUniformDistributed(100.0, 5000.0);
      }

      p3d = ppp.UnProjectToPoint(p2d, dist);
      p2d_p = ppp.Project(p3d);
      error = (p2d_p.GetEuclidean()-p2d.GetEuclidean()).NormL2();
      cout << "point " << p2d << " error " << error << endl;
      if(Greater(error, EPSILON)){
        countError++;
/*
        BIASERR("Pretty high error for unproject-project " << error
                << " for " << ppp << " point " << p2d << " 3D " << p3d
                << " projected " << p2d_p << endl);
        return -1;
*/
      }
      if(maxError < error){
        maxError = error;
      }     
      sumError += error;
      sumSqError += error*error;
      countAll++;

      //int pr = int(100.0*double(cams*numPoints+points)/(numCams*numPoints));
    }

    // show progress
    int pr = int(100.0*double(cams)/numCams);
    if (percent < pr) {
      while(percent < pr) {
        cout << ".";
        percent++;
      }
      cout << " " << percent << "% ";
    }
  }

  double meanError = sumError / (double)countAll;
  double stdError = sqrt(sumSqError / (double)countAll - meanError*meanError);

  cout << "\n\n----------------------------------------------------------\n"
       << " count of errors > " << EPSILON << " : " << countError
       << " (" << 100.0*(double)countError/(double)countAll << "%)\n"
       << " average error : " << meanError << " +- " << stdError << endl
       << " max. error : " << maxError << endl
       << "----------------------------------------------------------\n\n";
  
  return 0;
}