threeviewtri.h

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

#ifndef THREE_VIEW_TRI
#define THREE_VIEW_TRI

#include "martinsUtils.h"

#define N_SOLUTIONS 154

#define N_EQS 225
#define N_MONS 209
#define N_ELIMCOLS 55
#define N_BASIS (N_MONS - N_ELIMCOLS)
#define N_DEGREE_5 3
#define N_DEGREE_6 6
#define N_INITIAL_EQS 9
#define N_DEGREE_6_MONS 73
#define N_DEGREE_5_MONS 45

// call one of these two methods when integrating the three view triangulater
// in other code.
void threeViewTri(int nPoints,
		  double **x1, double **P1, 
		  double **x2, double **P2,
		  double **x3, double **P3,
		  double **X);

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

// these methods are for internal use
void threeViewTriInternal(int nPoints,
		  double **x1, double **P1, 
		  double **x2, double **P2,
		  double **x3, double **P3,
		  double **X, bool doVerbose);

void threeViewTriSolver(double *x1, double *P1,
			double *x2, double *P2,
			double *x3, double *P3,
			double X[N_SOLUTIONS][3], int *nSols);

void changeImageCoordinates(double *x, double *P, double *Him);

void changeWorldCoordinates(double *P1, double *P2, double *P3,
			    double *Hworld);

void changeBack(double X[][3], double *Hworld);

void extractSolutions(double X[][3],
		      double *mx, double *my, double *mz, 
		      int *nSols);

void buildMx(double *Mx, double *My, double *Mz);

void buildP(double* P, double* Cprim);

void addEquationsForSaturation(double *u,
			       double *eq1, double *eq2, double *eqt,
			       int variable);

void addSaturationEquations(double eqs[N_INITIAL_EQS][N_MONS],
			    double *eq1, double *eq2, double *eqt,
			    int variable);

void saturationLu(double *u, int variable);

void addEquations(double* C, double eqs[N_INITIAL_EQS][N_MONS],
		  int startRow);

inline int project(double *X, double *P, double *x) {
  double xTmp[3];
  double XTmp[4];
#ifdef MAC_OS
  cblas_dcopy(3, X, 1, XTmp, 1);
#endif
#ifdef LINUX_OS
  blasCopy(X, XTmp, 3);
#endif
  XTmp[3] = 1;
  ATimesx(P, XTmp, xTmp, 3, 4);
  x[0] = xTmp[0] / xTmp[2];
  x[1] = xTmp[1] / xTmp[2];
  return xTmp[2] > 0;
};

inline double norm2(double *x, int n) {
  double n2;
  n2 = 0;
  for(int i = 0; i < n; i++) n2 += x[i] * x[i];
  return n2;
};

inline void diff(double *x, double *y, double *z, int n) {
  for(int i = 0; i < n; i++) z[i] = x[i] - y[i];
};
#endif