threeviewtri.cpp

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

#include <iostream>
#include <unistd.h>
#include <cmath>
#include <cassert>

#ifdef MAC_OS
#include <veclib/cblas.h>
#include <veclib/clapack.h>
#endif

#ifdef LINUX_OS
#include <acml.h>
#endif

#include "martinsUtils.h"
#include <vector>
#include "threeViewTri.h"
#include "helpers.h"
#include "monomialMaps.h"
#include "setupEquations.h"

using namespace std;

void threeViewTri(int nPoints,
		  double **x1, double **P1, 
		  double **x2, double **P2,
		  double **x3, double **P3,
		  double **X) { 
  threeViewTriInternal(nPoints, x1, P1, x2, P2, x3, P3, X, false);
}

void threeViewTriVerbose(int nPoints,
			 double **x1, double **P1, 
			 double **x2, double **P2,
			 double **x3, double **P3,
			 double **X) { 
  threeViewTriInternal(nPoints, x1, P1, x2, P2, x3, P3, X, true);
}

void threeViewTriInternal(int nPoints,
			  double **x1, double **P1, 
			  double **x2, double **P2,
			  double **x3, double **P3,
			  double **X, bool doVerbose) {

  double XSols[N_SOLUTIONS][3];
  int nSols;
  double x1p[2], x2p[2], x3p[2], d1[2], d2[2], d3[2];

  // loop through all triplets of corresponding image points and triangulate.
  for(int i = 0; i < nPoints; i++) {
    double minErr, error;
    bool inFront1, inFront2, inFront3;
    minErr = 1e+99;
    error = 0;

    // calculate all plausible (real) solutions
    if(doVerbose) cout << "point " << i << endl;
    threeViewTriSolver(x1[i], P1[i],
		       x2[i], P2[i],
		       x3[i], P3[i], 
		       XSols, &nSols);
    		       
    // verbose.
    if(doVerbose) cout << "number of real solutions: " << nSols << endl;
    
    // print out every nth iteration.
    if(i % 10 == 0 && i != 0)
	cout << i << endl;
 
    for(int k = 0; k < 3; k++) X[i][k] = 0;

    // find out which is the right solution. (it should be possible to
    // make this section a bit faster by making heavier use of blas routines
    // instead of looping.)
    for(int k = 0; k < nSols; k++) {
      // reproject into the three views
      inFront1 = project(XSols[k], P1[i], x1p);
      inFront2 = project(XSols[k], P2[i], x2p);
      inFront3 = project(XSols[k], P3[i], x3p);

      // check for positive depths
      if(!inFront1 || !inFront2 || !inFront3) continue;

      // measure the error
      error = 0;
      diff(x1[i], x1p, d1, 2);
      error += norm2(d1, 2);
      diff(x2[i], x2p, d2, 2);
      error += norm2(d2, 2);
      diff(x3[i], x3p, d3, 2);
      error += norm2(d3, 2);

      // is this a new min?
      if(error < minErr) {
	minErr = error;
	blasCopy(XSols[k], X[i], 3);
      }
    }
    
  }  
}

// takes three 2x1 image points x1, x2, x3 and three cameras P1, P2, P3.
// outputs all plausible (real) solutions X and the number of solutions nSols. 
void threeViewTriSolver(double *x1, double *P1,
			double *x2, double *P2,
			double *x3, double *P3,
			double X[N_SOLUTIONS][3], int *nSols) {

  double *C, *CReordered, *Cbiss, *Cprim;
  double *P;
  double *Mx, *My, *Mz;
  double *mx, *my, *mz;
  double *P1tmp, *P2tmp, *P3tmp;
  double Him1[3 * 3], Him2[3 * 3], Him3[3 * 3], HworldInv[4 * 4];
  double eqx[209], eqy[209], eqz[209], eqt[209];
  double eqs[N_INITIAL_EQS][209];

  for(int i = 0; i < 9; i++) zeros(eqs[i], 1, 209);
  
  C = new double[N_EQS * N_MONS];
  CReordered = new double[N_EQS * N_MONS];
  Cbiss = new double[N_EQS * N_ELIMCOLS];
  Cprim = new double[N_EQS * N_BASIS];
  Mx = new double[N_MONS * N_BASIS];
  My = new double[N_MONS * N_BASIS];
  Mz = new double[N_MONS * N_BASIS];
  mx = new double[N_BASIS * N_BASIS];
  my = new double[N_BASIS * N_BASIS];
  mz = new double[N_BASIS * N_BASIS];  
  P = new double[N_BASIS * N_MONS];
  P1tmp = new double[3 * 4];
  P2tmp = new double[3 * 4];
  P3tmp = new double[3 * 4];
    
  // copy camera matrices to temporary storage
  blasCopy(P1, P1tmp, 12);
  blasCopy(P2, P2tmp, 12);
  blasCopy(P3, P3tmp, 12);
  
  //
  // 0. change coordinate system.
  //
  
  changeImageCoordinates(x1, P1tmp, Him1);
  changeImageCoordinates(x2, P2tmp, Him2);
  changeImageCoordinates(x3, P3tmp, Him3);

  changeWorldCoordinates(P1tmp, P2tmp, P3tmp, HworldInv);
  
  // change to 2x4 P matrices.
  for(int j = 0; j < 4; j++) {
    P1tmp[2 * j + 0] = P1tmp[3 * j + 0];
    P1tmp[2 * j + 1] = P1tmp[3 * j + 1];

    P2tmp[2 * j + 0] = P2tmp[3 * j + 0];
    P2tmp[2 * j + 1] = P2tmp[3 * j + 1];

    P3tmp[2 * j + 0] = P3tmp[3 * j + 0];
    P3tmp[2 * j + 1] = P3tmp[3 * j + 1];
  }
  
#ifdef DEBUG
  cout << "step 1..." << endl;
#endif
  //
  // 1. build initial C (coefficient matrix).
  //
  setupEquations(P1tmp, P2tmp, P3tmp, eqx, eqy, eqz, eqt);
  
#ifdef DEBUG
  cout << "step 2..." << endl;
#endif
  //
  // 2. perform saturation steps.
  //
   int startRow = 0;
  int variable = 0;
  addSaturationEquations(eqs + startRow,
			 eqy, eqz, eqt, variable);
  
  startRow = 3;
  variable = 1;
  addSaturationEquations(eqs + startRow,
			 eqx, eqz, eqt, variable);
  
  startRow = 6;
  variable = 2;
  addSaturationEquations(eqs + startRow,
			 eqx, eqy, eqt, variable);
  
#ifdef DEBUG
  cout << "step 3..." << endl;
#endif
  //
  // 3. multiply up to degree 9.
  //
  startRow = 0;
  addEquations(C, eqs, startRow);

  // reorder the columns of C.
  int reorder[209] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 164, 165, 166, 167, 170, 99, 133, 134, 135, 158, 159, 160, 161, 162, 163, 168, 169, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208};
  for(int j = 0; j < N_MONS; j++)
    blasCopy(C + N_EQS * reorder[j], CReordered + N_EQS * j, N_EQS);
  
#ifdef DEBUG
  cout << "step 4..." << endl;
#endif
  //
  // 4. eliminate and produce T, P(modulo mapping),
  // Mx and mx(action matrices).
  //

  // construct Cbiss and Cprim.
  blasCopy(CReordered, Cbiss, N_EQS * N_ELIMCOLS);
  // this line uses pointer arithmetic to go to the right starting point in C.
  blasCopy(CReordered + N_EQS * N_ELIMCOLS, Cprim, N_EQS * N_BASIS);


#ifdef DEBUG
  cout << "step 4a..." << endl;
#endif
  // eliminate
  int info;
  // This function call changes Cprim (to -T).
  //solve_ls(Cbiss, Cprim, N_EQS, N_ELIMCOLS, N_BASIS, &info);
  solve_ls(Cbiss, Cprim, N_EQS, N_ELIMCOLS, N_BASIS, &info);

#ifdef DEBUG
  cout << "step 4b..." << endl;
#endif
  // construct the modulo matrix P.
  buildP(P, Cprim);
  
  // construct the large action matrix Mx.
  buildMx(Mx, My, Mz);
  
#ifdef DEBUG
  cout << "step 4c..." << endl;
#endif
  // construct the real action matrix mx.
  // TODO LATER: speedup by explicit generation of mx instead of using matrix
  // multiplication.
  ATimesB(P, Mx, mx, N_BASIS, N_MONS, N_BASIS);
  ATimesB(P, My, my, N_BASIS, N_MONS, N_BASIS);
  ATimesB(P, Mz, mz, N_BASIS, N_MONS, N_BASIS);
  
#ifdef DEBUG
  cout << "step 5..." << endl;
#endif
  //
  // 5. extract solutions from the eigenvectors/eigenvalues.                     
  //
  extractSolutions(X, mx, my, mz, nSols);
  
#ifdef DEBUG
  cout << "step 6..." << endl;
#endif
  //
  // 6. go back to the original coordinates
  //
  changeBack(X, HworldInv);

  // clean up.
  delete[] C;
  delete[] CReordered;
  delete[] Cbiss;
  delete[] Cprim;
  delete[] Mx;
  delete[] My;
  delete[] Mz;
  delete[] mx;
  delete[] my;
  delete[] mz;
  delete[] P;
  delete[] P1tmp;
  delete[] P2tmp;
  delete[] P3tmp;
}

void changeImageCoordinates(double *x, double *P, double *Him) {
  // build Him
  eye(Him, 3);
  Him[6] = -x[0];
  Him[7] = -x[1];
  for(int i = 0; i < 9; i++)
    Him[i] *= 1e-3;
  Him[8] = 1;

  // apply
  double Ptmp[12];
  blasCopy(P, Ptmp, 12);
  ATimesB(Him, Ptmp, P, 3, 3, 4); 
}

void changeWorldCoordinates(double *P1, double *P2, double *P3, double *HworldInv) {
  int info;
  int p[4];
  double A[4 * 4];
  double P1tmp[3 * 4], P2tmp[3 * 4], P3tmp[3 * 4];

  // calculate Hworld
  for(int j = 0; j < 4; j++)
    A[4 * j + 0] = P1[3 * j + 2];
  for(int j = 0; j < 4; j++)
    A[4 * j + 1] = P2[3 * j + 2];
  for(int j = 0; j < 4; j++)
    A[4 * j + 2] = P3[3 * j + 2];
  for(int j = 0; j < 4; j++)
    A[4 * j + 3] = 1;
  
  eye(HworldInv, 4);
  for(int j = 0; j < 4; j++)
    HworldInv[4 * j + 3] = 1;
  
  int m = 4;