Example7Point.cpp

Example for 7 point pose estimator

Author

MIP

/* 
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 Example7Point.cpp   
   @relates FMatrixEstimation
   @brief Example for 7 point pose estimator
   @ingroup g_examples
   @author MIP
*/

#include <Geometry/FMatrixEstimation.hh>
#include <MathAlgo/SVD.hh>

using namespace BIAS;
using namespace std;

typedef vector<HomgPoint2D> PointVector;

void
loadFlowVectors(char* filename, PointVector& match1, 
        PointVector& match2)
{
  match1.clear();
  match2.clear();
  FILE *stream;
  stream = fopen ( filename , "rb" );
  char *buffer = new char[256];
  float x1, y1, w1, x2, y2, w2;
  bool done=false;
  while (!done) 
    {
      char *r = fgets(buffer, 256, stream);
      if (r == NULL) exit(-1); //error reading from file
      if (buffer[0] == '#' || buffer[0] == '\n') continue; //commentary
      if (feof(stream)) {
          done=true;
          break; 
      }
      sscanf(buffer, "%f %f %f %f %f %f \n", &x1, &y1, &w1, &x2, &y2, &w2);
      match1.push_back(HomgPoint2D(x1, y1, w1));
      match2.push_back(HomgPoint2D(x2, y2, w2));
    }
  fclose(stream);
}

int main(int argc, char *argv[])
{
  vector<HomgPoint2D> p1, p2;
  
  if (argc>=2){
    loadFlowVectors(argv[1], p1, p2);
    for (int i=0; i<(int)p1.size(); i++){
      cout << p1[i]<<"\t"<<p2[i]<<endl;
    }
  } else {
    cerr << argv[0]<<" pointfile\n";
    return -1;
  }

  vector<FMatrix> Fvec;
  FMatrixEstimation fest;

  if (fest.SevenPoint(Fvec, p1, p2)!=0){
    BIASERR("error estimating F");
  }

  SVD svd;
  for (int i=0; i<(int)Fvec.size(); i++){
    svd.Compute((Matrix<double>)Fvec[i]);
    cout << "singular values of "<<i<<"th solution are "
         <<svd.GetS()[0]<<"\t"<<svd.GetS()[1]<<"\t"<<svd.GetS()[2]<<endl;
  } 

  return 0;
}