InvestigateEpipoleEstimation.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003, 2004    (see file CONTACTS 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 InvestigateEpipoleEstimation.cpp
   @relates EpipoleEstimation
   @brief example investigating epipole estimation
   @ingroup g_examples
   @author MIP
*/

#include "bias_config.h"
#include "Geometry/EpipoleEstimation.hh"
#include "Base/Math/Random.hh"
#include "Base/Geometry/KMatrix.hh"
#include "Base/Geometry/RMatrixBase.hh"
#include "Geometry/FMatrix.hh"
#include "Geometry/PMatrix.hh"
#include "Base/Debug/TimeMeasure.hh"

#include <iostream>
#include <iomanip>

#define EPIPOLE_FACTOR 15.0
#define BACK 0
#define LINE 70
#define MATCH 255

using namespace BIAS;
using namespace std;

// minor noise in camera position
BIAS::RMatrixBase R;
BIAS::KMatrix K, Kinv;
BIAS::HomgPoint2D true_epipole, estimated_epipole, estimated_epipole2, center;
vector<BIAS::HomgPoint2D> match1, match2;

void CreateMatches2(vector<HomgPoint2D>& match1, vector<HomgPoint2D>& match2, 
                    unsigned int match_count, unsigned int width, 
                    unsigned int height, double spatial_noise_sigma, 
                    double r_ang)
{
  PMatrix P1, P2;
  RMatrixBase R1, R2;
  HomgPoint3D C1, C2;
  KMatrix K;
  K.SetIdentity();
  K[0][0]=K[1][1]=840.0;
  K[0][2]=(width-1.0)/2.0;
  K[1][2]=(height-1.0)/2.0;
  R1.SetIdentity();
  R2.SetXYZ(r_ang, 0.0, 0.0);
  C1.Set(0.0, 0.0, 0.0);
  // movement 45 deg to camera plane 
  // for about 45 centimeter
  C2.Set(0.3, 0.0, 0.3); 

  P1.Compose(K, R1, C1);
  P2.Compose(K, R2, C2);

  vector<PMatrix> pvec;
  pvec.push_back(P1);
  pvec.push_back(P2);
  //  cout << pvec[0] << "\n" << pvec[1] << endl;

  true_epipole = P2 * C1;
  true_epipole.Homogenize();

  vector<vector<HomgPoint2D> > i_matches, n_matches;
  vector<HomgPoint3D> p3d;

  CreateMatches(pvec, match_count, width, height, spatial_noise_sigma,
                i_matches, n_matches, p3d, 2.0, -30.0, 30.0, -30.0, 30.0, 
                -30.0, 30.0, false);

  match1.resize(match_count);
  match2.resize(match_count);
  for (unsigned i=0; i<match_count; i++){
    match1[i]=n_matches[i][0];
    match2[i]=n_matches[i][1];
    //    cout << n_matches[i][0]<<"\t"<<n_matches[i][1]<<endl;
  }
  //cout << endl<<endl;
}

void CreateMatches(vector<BIAS::HomgPoint2D>& match1,  
                   vector<BIAS::HomgPoint2D>& match2, 
                   unsigned int match_count, BIAS::HomgPoint2D epipole, 
                   unsigned int width, unsigned int height, 
                   double maxdisparity, double mindisparity,
                   double spatial_noise_sigma)
{
  Random ran;
  BIAS::HomgPoint2D p1, p2;
  double dist, p1epipoledist;
  

  match1.clear();
  match2.clear();
  p1[2]=p2[2]=1.0;
  for (unsigned int i=0; i<match_count; i++){
    do {
      // create first point per random
      p1[0]=ran.GetUniformDistributed(0.0, (double)width-1.0);
      p1[1]=ran.GetUniformDistributed(0.0, (double)height-1.0);
      // distance from epipole
      p1epipoledist=sqrt((epipole[0]-p1[0])*(epipole[0]-p1[0])+
                         (epipole[1]-p1[1])*(epipole[1]-p1[1]));
      do dist=ran.GetUniformDistributed(mindisparity, maxdisparity);
      while (dist>=p1epipoledist);
      //cout << p1epipoledist << "  " << dist << endl;
      p2[0]=p1[0]+(epipole[0]-p1[0])*dist/p1epipoledist
        + ran.GetNormalDistributed(0.0, spatial_noise_sigma);
      p2[1]=p1[1]+(epipole[1]-p1[1])*dist/p1epipoledist
        + ran.GetNormalDistributed(0.0, spatial_noise_sigma);
      // add some noise to camera orientation
      p2=K*R*Kinv*p2;
      p2.Homogenize();
    } while (p2[0]<0.0 || p2[0]>(double)(width-1) ||
             p2[1]<0.0 || p2[1]>(double)(height-1));
    //cout << setw(4) << i << ":\t"<<p1 << "\t"<<p2 << endl;
    match1.push_back(p1);
    match2.push_back(p2);
  }
  //cout << endl;
}

void AddErr(double& mean_ep_dist, double& mean_ang, double& mean_el_dist)
{
  mean_ep_dist += estimated_epipole.Distance(true_epipole);

  HomgLine2D l1, l2;
  vector<HomgPoint2D>::iterator it1, it2;
  Matrix3x3<double> E;
  double ma=0.0, md=0.0;
  int c=0;
  E.SetAsCrossProductMatrix(true_epipole);
  for (it1=match1.begin(), it2=match2.begin(); 
       it1!=match1.end() && it2!=match2.end(); it1++, it2++, c++){
    l1.Set(true_epipole, *it2);
    l1.Homogenize();
    l2.Set(estimated_epipole, *it2);
    l2.Homogenize();
    ma+=fabs(atan2(l1[1], l1[0])-atan2(l2[1], l2[0]));
    md+=(*it1).ScalarProduct(E*(*it2));
  }
  mean_ang=ma/(double)c;
  mean_el_dist=md/(double)c;
}

int main(int argc, char *argv[])
{
  ofstream os("errcp3.txt");
  ofstream os2("errls3.txt");
  Random ran;
  double spatial_noise_sigma=0.0;
  double max_disparity=50.0;
  double mindisparity=2.0;
  unsigned int match_count=80;
  unsigned int width=640, height=480;
  int count=100;
  double mean_ep_dist_ls, mean_ep_dist_cp, mean_ang_ls, mean_ang_cp;
  double mean_el_dist_ls, mean_el_dist_cp;
  EpipoleEstimation ee;
  TimeMeasure tc;
  //double cf=1000/(2.4*1024*1024*1024);
  //cerr <<cf<<endl;
  center.Set(width/2, height/2);

  if (argc>=2) match_count=atoi(argv[1]);
  if (argc>=3) spatial_noise_sigma=atof(argv[2]);

  K.Fill(0.0);
  K[0][0]=K[1][1]=840.0;
  K[0][2]=double(width)/2.0;
  K[1][2]=double(height)/2.0;
  K[2][2]=1.0;
  Kinv=K.Invert();

  true_epipole[2]=1.0;
  true_epipole[0]=
    ran.GetUniformDistributed(-EPIPOLE_FACTOR*(double)width,
                              (EPIPOLE_FACTOR+1.0)*(double)width);
  true_epipole[1]=ran.GetUniformDistributed(-EPIPOLE_FACTOR*(double)height, 
                                            EPIPOLE_FACTOR+1.0*(double)height);

  CreateMatches2(match1, match2, match_count, width, height,
                       spatial_noise_sigma, 0.0);
  
  os <<"# EpipoleEstimationCrossProduct\n"
     <<"# true epipole: "<<true_epipole[0]<<" "<<true_epipole[1]
     <<" "<<true_epipole[2]<<endl
     <<"# "<<match_count<<" matches"
     <<"\n# max disparity: "<<max_disparity
     <<"\n# min disparity: "<<mindisparity<<endl;
  os2 <<"# EpipoleEstimationCrossProduct\n"
      <<"# true epipole: "<<true_epipole[0]<<" "<<true_epipole[1]
      <<" "<<true_epipole[2]<<endl
      <<"# "<<match_count<<" matches"
      <<"\n# max disparity: "<<max_disparity
      <<"\n# min disparity: "<<mindisparity<<endl;
      
  double err_r;
  
  for (spatial_noise_sigma=0.0; spatial_noise_sigma<1.05; 
       spatial_noise_sigma+=0.1){
    os <<endl<<endl<<"# spatial noise: "<<spatial_noise_sigma
       <<"\n# spat-nois\trot-err(DEG)\trot-err(RAD)\tabs-dist-err"
       <<"\tabs-angle-err\tabs-el-dist-err\n";
    os2 <<endl<<endl<<"# spatial noise: "<<spatial_noise_sigma
        <<"\n# spat-nois\trot-err(DEG)\trot-err(RAD)\tabs-dist-err"
       <<"\tabs-angle-err\tabs-el-dist-err\n";
    for (err_r=1e-3; err_r<=0.5; err_r*=2.0){
      mean_ep_dist_ls = mean_ep_dist_cp = mean_ang_ls = mean_ang_cp =
        mean_el_dist_ls = mean_el_dist_cp = 0.0;
      for (int c=0; c<count; c++){
        CreateMatches2(match1, match2, match_count, width, height,
                       spatial_noise_sigma, err_r);
        
        if (ee.CrossProductLeastSquaresSVD(match1,match2,
                                           estimated_epipole)!=0){
          BIASERR("error estimating epipole");
          return 0;
        }
        AddErr(mean_ep_dist_cp, mean_ang_cp, mean_el_dist_cp);
          
        if (ee.MatchLengthWeightedLeastSquares(match1, match2, 
                                               estimated_epipole)!=0){
          BIASERR("error estimating epipole");
          return 0;
        }
        AddErr(mean_ep_dist_ls, mean_ang_ls, mean_el_dist_ls);

        cout <<".";
        cout.flush();
      } // for (int c=0; c<count; c++){
      mean_ep_dist_ls /= count;
      mean_ep_dist_cp /= count;
      mean_ang_ls /= count;
      mean_ang_cp /= count;
      mean_el_dist_ls /= count;
      mean_el_dist_cp /= count;
 
      os2 <<setw(6)<<spatial_noise_sigma<< "\t"<< setw(6) << err_r/2/M_PI*360 
         << setw(12) << err_r << "\t" << setw(12)
         <<mean_ep_dist_ls << "\t" << setw(12) 
         <<mean_ang_ls<<"\t"<<mean_el_dist_ls<<endl;

      os <<setw(6)<<spatial_noise_sigma<< "\t"<< setw(6) << err_r/2/M_PI*360 
          << setw(12) <<err_r << "\t" << setw(12) 
          <<mean_ep_dist_cp << "\t" << setw(12) 
         <<mean_ang_cp<<"\t"<<mean_el_dist_cp<<endl;
      
      cout <<endl;
      cout <<setw(12)<<spatial_noise_sigma<<setw(12)<<err_r<<"\t"<<setw(12)
           <<mean_ep_dist_ls 
           << "\t"<<setw(12)<<mean_ang_ls<<"\t"<<mean_el_dist_ls<<endl;
      cout <<setw(12)<<spatial_noise_sigma<<setw(12)<<err_r<<"\t"<<setw(12)
           <<mean_ep_dist_cp 
           << "\t"<<setw(12)<<mean_ang_cp<<"\t"<<mean_el_dist_cp<<endl;
    }
  }
  os2.close();
  os.close();
  return 0;
}