martinsutils.cpp

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

#include <iostream>
#include <cmath>

#include "martinsUtils.h"

using namespace std;

void lu_u(double *A, int m, int n) {
  int p[m];
  int info;
#ifdef LINUX_OS
  dgetrf(m, n, A, m, p, &info);
#endif
#ifdef MAX_OS
  dgetrf_((__CLPK_integer*) m, (__CLPK_integer*) n, (__CLPK_doublereal*) A,
	  (__CLPK_integer*) m, (__CLPK_integer*) p, (__CLPK_integer*) info);
#endif
  luZero(A, m, n);
}

void luZero(double *u, int m, int n) {
  for(int i = 0; i < m; i++)
    for(int j = 0; j < min(i, n); j++)
      u[m * j + i] = 0;
}

double median(double *x, int n) {
  // destructive, i.e. alters x. ignores inf and nan.
  int nNumbers = 0;
  for(int i = 0; i < n; i++) {
    if(!isnan(x[i]) && !isinf(x[i])) {
      x[nNumbers] = x[i];
      nNumbers++;
    }
  }
  sort(x, x + nNumbers); 
  return (nNumbers == 0) ? (double) NAN : x[nNumbers / 2];
}

void solve_ls(double *A, double *B, int m, int k, int n, int *info) {
  __CLPK_doublereal lwork;
  __CLPK_doublereal *work;
  __CLPK_doublereal workQuery[1];
  if(m == k) {
    int p[m];
    solve(A, B, m, n, p, info);
    return;
  } else {
#ifdef MAC_OS
    lwork = -1;
    dgels_("N", (__CLPK_integer*)&m, (__CLPK_integer*)&n, (__CLPK_integer*)&k,
	   (__CLPK_doublereal*)A, (__CLPK_integer*)&m, (__CLPK_doublereal*)B, (__CLPK_integer*)&m,
	   workQuery, (__CLPK_integer*)&lwork, (__CLPK_integer*)info);
    lwork = (__CLPK_integer) floor(workQuery[0]);
    work = new __CLPK_doublereal[(int)lwork];
    dgels_("N", (__CLPK_integer*)&m, (__CLPK_integer*)&n, (__CLPK_integer*)&k,
	     (__CLPK_doublereal*)A, (__CLPK_integer*)&m, (__CLPK_doublereal*)B, (__CLPK_integer*)&m,
	     work, (__CLPK_integer*)&lwork, (__CLPK_integer*)info);
    delete[] work;
#endif
#ifdef LINUX_OS
#ifdef DEBUG
    cout << "linux dgels m = " << m << ", n = " << n << ", k = " << k << endl;
#endif
    dgels('N', m, k, n, A, m, B, m, info);
#endif
  }
}

void qr(__CLPK_doublereal *A,
	__CLPK_integer m,
	__CLPK_integer n,
	__CLPK_doublereal *R,
	__CLPK_integer p[])
{
  __CLPK_doublereal *tau;
  tau = new __CLPK_doublereal[min(m,n)];
  __CLPK_doublereal *work;
  work = new __CLPK_doublereal[10*m*n];
  __CLPK_doublereal *tmp;
  tmp = new __CLPK_doublereal[m*n];
  __CLPK_integer lwork = 10*m*n;
  __CLPK_integer info;
  int out;
  
  // copy A to temporary storage.
  blasCopy(A, tmp, m * n);

  // factorize
#ifdef MAC_OS
  out = dgeqrf_(&m, &n, tmp, &m, tau, work, &lwork, &info);
#endif
#ifdef LINUX_OS
  dgeqrf(m, n, tmp, m, tau, &info); 
#endif
  
  // fill in R.
  for(int i = 0; i < min(m, n); i++) {
    for (int j = 0; j < i; j++)
      R[m * j + i] = 0.0;
    for (int j = i; j < n; j++)
      R[m * j + i] = tmp[m * j + i];
  }

  // clean up
  delete[] tau, work, tmp;
}

// puts A on echelon form by repeated lu factorizations.
void echelon(double *A, int m, int n) {
  double *subMatrix;
  int p[m];
  int info, r, rNew;
  bool isEchelon;
  int row, col, ii;
  
  // compute LU.
  lu_u(A, m, n);

  // check if A is echelon.
  r = -1;
  isEchelon = true;
  for(int i = 0; i < m; i++) {
    // find first non-zero entry
    for(rNew = i; rNew < n; rNew++) {
      for(ii = i; ii < m; ii++) { 
	if(A[m * rNew + ii] != 0) {
	  goto out;
	}
      }
    }
    out :
    if(rNew == n) return; // definitely still echelon.
    if(rNew > r && ii == i) {
      // ok. still echelon.
      r = rNew;
    } else {
      // not echelon.
      isEchelon = false;
      row = (ii == i) ? i - 1 : i;
      col = rNew;
      break;
    }
  }
  if(isEchelon) return;
  
  // otherwise extract submatrix and put on echelon form.
  int nSub = max(n - col, m - row);
  subMatrix = new double[(m - row) * nSub];
  zeros(subMatrix, m - row, nSub);
  for(int i = row; i < m; i++)
    for(int j = col; j < n; j++)
      subMatrix[(m - row) * (j - col) + (i - row)] = A[m * j + i];
  echelon(subMatrix, m - row, nSub);

  // merge result.
  for(int i = row; i < m; i++)
    for(int j = col; j < n; j++)
      A[m * j + i] = subMatrix[(m - row) * (j - col) + (i - row)];
  
  // clean up.
  delete[] subMatrix;
}

void rowOperate(double* A, int destRow, int opRow, double factor,
		int m, int n) {
  for(int i = 0; i < n; i++)
    A[m * i + destRow] += factor * A[m * i + opRow];
}

void printMatrix(double* A, int m, int n, char* name) {
  cout << name;
  cout << ":\n";
  for(int i = 0; i < m; i++) {
    for(int j = 0; j < n; j++)
      printf("%.10e ", A[j * m + i]);
    cout << "\n";
  }
}

void printMatrix(__CLPK_integer* A, int m, int n, char* name) {
  cout << name;
  cout << ":\n";
  for(int i = 0; i < m; i++) {
    for(int j = 0; j < n; j++)
      printf("%d ", A[j * m + i]);
    cout << "\n";
  }
}

void printMatrixTranspose(double* A, int m, int n, char* name) {
  cout << name;
  cout << ":\n";
  for(int j = 0; j < n; j++) {
    for(int i = 0; i < m; i++)
      printf("%.2e ", A[j * m + i]);
    cout << "\n";
  }
}

void fprintMatrix(double* A, int m, int n, const char* fileName) {
  FILE* file;
  file = fopen(fileName, "wt");
  for(int i = 0; i < m; i++) {
    for(int j = 0; j < n; j++)
      fprintf(file, "%.10f ", A[j * m + i]);
    fprintf(file, "\n");
  }
  fclose(file);
}

int freadMatrix(double* A, int m, int n, const char* fileName) {
  FILE* file;
  if((file = fopen(fileName, "r")) == NULL) {
    cout << "Could not open file: ";
    //cout << fileName;
    //cout "\n";
    exit(1);
  }
  for(int i = 0; i < m; i++)
    for(int j = 0; j < n; j++)
      fscanf(file, "%lf", &A[j * m + i]);

  fclose(file);
}

void eye(double *A, int n) {
  for(int i = 0; i < n; i++)
    for(int j = 0; j < n; j++)
      A[n * j + i] = (i == j);
}

void zeros(double *A, int m, int n) {
  for(int i = 0; i < m; i++)
    for(int j = 0; j < n; j++)
      A[m * j + i] = 0;
  
}

void ones(double *A, int m, int n) {
  for(int i = 0; i < m; i++)
    for(int j = 0; j < n; j++)
      A[m * j + i] = 1.0;
}

void rand(double *A, int m, int n) {
  for(int i = 0; i < m; i++)
    for(int j = 0; j < n; j++)
      A[m * j + i] = rand();
}