tvt.cpp

Code for L2-optimal three view triangulation based on calculation of stationary points

#include<iostream>

//#include <tnt.h>

#include"threeViewTri.h"

#define _(i,j,m) [j * m + i]

// TODO: more info here

// takes a file name as argument. triangulates and writes the result to std out. 
// the file is supposed to have the following format.
//
// N_points
// x11 x12 x21 x22 x31 x32 P111 P112 ... (the three cameras in row major order)
// .
// . (N_points rows)
// .
int main(int argc, char** argv) {
  using namespace std;
  FILE* file;
  int       status;
  int       nPoints;
  double    **x1, **x2, **x3;
  double    **P1, **P2, **P3;
  double    **X;
  
  //TNT::Stopwatch Q;

  if(argc!=2 || !strcmp(argv[1], "--help")) {
    cout << "Usage: tvt file\n";
    exit(1);
  }

  // open file
  #ifdef DEBUG 
     cout << "opening file...\n";
  #endif
  if((file = fopen(argv[1], "r"))==NULL) {
    cout << "Error opening file ";
    cout << argv[1];
    cout << "\n";
    exit(1);
  }

  // read number of points.
  double nP;
  fscanf(file, "%lf", &nP);
  nPoints = (int) nP;
  cout << "number of points to read: ";
  cout << nPoints;
  cout << "\n";

#ifdef DEBUG 
  cout << "allocating memory...\n";
  cout << " - image points\n";
#endif
  x1 = new double*[nPoints];
  x2 = new double*[nPoints];
  x3 = new double*[nPoints];
     
  P1 = new double*[nPoints];
  P2 = new double*[nPoints];
  P3 = new double*[nPoints];
  for(int i = 0; i < nPoints; i++) {
    x1[i] = new double[2];
    x2[i] = new double[2];
    x3[i] = new double[2];
#ifdef DEBUG 
    cout << " - cameras\n";
#endif
    P1[i] = new double[3 * 4];
    P2[i] = new double[3 * 4];
    P3[i] = new double[3 * 4];
  }
       
#ifdef DEBUG 
  cout << " - result\n";
#endif
  X = new double*[nPoints];
  for(int i = 0; i < nPoints; i++)
    X[i] = new double[3];

  // read data.
  
  cout << "reading data...\n";
  for(int i = 0; i < nPoints; i++) {
    //x1
    fscanf(file, "%lf", &x1[i][0]);
    fscanf(file, "%lf", &x1[i][1]);
    //x2
    fscanf(file, "%lf", &x2[i][0]);
    fscanf(file, "%lf", &x2[i][1]);
    //x3
    fscanf(file, "%lf", &x3[i][0]);
    fscanf(file, "%lf", &x3[i][1]);
    
    //P1
    for(int j = 0; j < 3; j++)
      for(int k = 0; k < 4; k++)
	fscanf(file, "%lf", &P1[i][k * 3 + j]);
    
    //P2
    for(int j = 0; j < 3; j++)
      for(int k = 0; k < 4; k++)
	fscanf(file, "%lf", &P2[i][k * 3 + j]);

    //P3
    for(int j = 0; j < 3; j++)
      for(int k = 0; k < 4; k++)
	fscanf(file, "%lf", &P3[i][k * 3 + j]);
  }
  
  // triangulate.
  cout << "triangulating...\n";
  //Q.start();
  threeViewTri(nPoints, x1, P1, x2, P2, x3, P3, X);
  //Q.stop();

  // output result.
#ifdef DEBUG 
  cout << "outputting results...\n";
  
  cout << "x1:\n";
  printf("%lf ", x1[0][0]);
  printf("%lf\n\n", x1[0][1]);
  
  cout << "P1:\n";
  for(int j = 0; j < 3; j++) {
    for(int k = 0; k < 4; k++) {
      cout << P1[0][k * 3 + j];
      cout << " ";
    }
    cout << "\n";
  }
#endif
  cout << "output: \n";
  for(int i = 0; i < nPoints; i++) {
    for(int j = 0; j < 3; j++) {
      printf("%.8f", X[i][j]);
      cout << " ";
    }
    cout << "\n";
  }

  // clean up
#ifdef DEBUG 
  cout << "deallocating memory...\n";
#endif
  
  for(int i = 0; i < nPoints; i++) {
    delete[] x1[i];
    delete[] x2[i];
    delete[] x3[i];
    delete[] P1[i];
    delete[] P2[i];
    delete[] P3[i];
    delete[] X[i];
  }
  delete[] x1;
  delete[] x2;
  delete[] x3;
  delete[] P1;
  delete[] P2;
  delete[] P3;
  delete[] X;
  fclose(file);
  cout << "done!\n";
  //cout << "triangulated " << nPoints << " points in " << Q.read() << " seconds\n";
  cout << "triangulated " << nPoints << " points.\n";
  //cout << "=> " << Q.read() / nPoints * 1000 << " milliseconds per point\n";
  
}