threeviewtri.cpp

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

  int n = 4;
  solve(A, HworldInv, m, n, p, &info); 

  // apply
  blasCopy(P1, P1tmp, 12);
  blasCopy(P2, P2tmp, 12);
  blasCopy(P3, P3tmp, 12);
  ATimesB(P1tmp, HworldInv, P1, 3, 4, 4);
  ATimesB(P2tmp, HworldInv, P2, 3, 4, 4);
  ATimesB(P3tmp, HworldInv, P3, 3, 4, 4);
}

void addSaturationEquations(double eqs[3][N_MONS],
			    double *eq1, double *eq2, double *eqt,
			    int variable) {
  const int nEquations = 12;
  const int nMons = 209;
  int info;
  double u[nEquations * nMons];
  double uReordered[nEquations * nMons];
  int p[nEquations];
  zeros(u, nEquations, nMons);
  int deg1ind[3] = {3,2,1};
  int deg2ind[3] = {9,6,4};
  int *mapping;
  double row[209];

  //
  // form the 12 equations from our initial three.
  //
  addEquationsForSaturation(u, eq1, eq2, eqt, variable);

  //
  // perform lu decomposition.
  //
  
  saturationLu(u, variable);
  //echelon(u, 12, 209);

  //
  // divide by xi for the first two eqs and by xi^2 for the third.
  //
  mapping = multMap[deg1ind[variable]];
  getRow(9, row, u, 12, 209);
  divide(row, eqs[0], mapping, 73, 0, 1, N_MONS);
  
  mapping = multMap[deg1ind[variable]];
  getRow(10, row, u, 12, 209);
  divide(row, eqs[1], mapping, 73, 0, 1, N_MONS);

  mapping = multMap[deg2ind[variable]];
  getRow(11, row, u, 12, 209);
  divide(row, eqs[2], mapping, 73, 0, 1, N_MONS);
}

void saturationLu(double *u, int variable) {
  double A[12 * 50];
  double Asub[3 * 39];
  double Asubsub[2 * 37];
  int info;
  int p[12];
  int p2[3];
  int p3[2];
  int reorder[3][50] = {{49, 46, 37, 21, 45, 36, 20, 35, 19, 18, 44, 34, 17, 33, 16, 15, 48, 43, 32, 14, 42, 31, 13, 47, 41, 30, 12, 40, 29, 11, 28, 10, 9, 39, 27, 8, 26, 7, 38, 25, 6, 24, 5, 4, 23, 3, 2, 22, 1, 0},
			{48, 43, 32, 15, 39, 27, 10, 23, 5, 1, 42, 31, 14, 26, 9, 4, 49, 46, 37, 21, 44, 34, 18, 47, 41, 30, 13, 38, 25, 8, 22, 3, 0, 45, 36, 20, 33, 17, 40, 29, 12, 24, 7, 2, 35, 19, 16, 28, 11, 6},
			{47, 40, 28, 11, 38, 24, 6, 22, 2, 0, 41, 29, 12, 25, 7, 3, 49, 45, 35, 19, 44, 33, 16, 48, 42, 30, 13, 39, 26, 8, 23, 4, 1, 46, 36, 20, 34, 17, 43, 31, 14, 27, 9, 5, 37, 21, 18, 32, 15, 10}};

  int nonZCols[3][50] = {{106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 123, 124, 125, 130, 131, 132, 133, 139, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 153, 154, 159, 160, 161, 167, 169, 170, 171, 172, 173, 176, 181, 182, 188, 190, 197},
			 {103, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 116, 117, 118, 119, 120, 123, 124, 125, 131, 132, 133, 139, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 153, 154, 159, 160, 161, 167, 169, 170, 171, 172, 173, 176, 181, 182, 188, 190, 197},
			 {103, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 123, 124, 125, 130, 131, 132, 139, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 153, 154, 159, 160, 161, 167, 169, 170, 171, 172, 173, 176, 181, 182, 188, 190, 197}};
  
  // reorder and pick out non-zero columns.
  for(int j = 0; j < 50; j++)
    for(int i = 0; i < 12; i++)
      A[12 * j + i] = 
	u[12 * nonZCols[variable][reorder[variable][j]] + i];
  
  // LU.
  lu_u(A, 12, 50);

  // sub-LU.
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 39; j++)
      Asub[3 * j + i] = A[12 * (j + 11) + (i + 9)];
  lu_u(Asub, 3, 39);
 
  // sub-sub-LU.
  for(int i = 0; i < 2; i++)
    for(int j = 0; j < 37; j++)
      Asubsub[2 * j + i] = Asub[3 * (j + 2) + (i + 1)];
  lu_u(Asubsub, 2, 37);

  // merge.
  for(int i = 0; i < 2; i++)
    for(int j = 0; j < 37; j++)
      Asub[3 * (j + 2) + (i + 1)] = Asubsub[2 * j + i];

  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 39; j++)
      A[12 * (j + 11) + (i + 9)] = Asub[3 * j + i];
  

  for(int j = 0; j < 50; j++)
    for(int i = 0; i < 12; i++)
      u[12 * nonZCols[variable][j] + i] = A[12 * j + i];

  //insert back into u and restore ordering.
  for(int j = 0; j < 50; j++)
    for(int i = 0; i < 12; i++)
      u[12 * nonZCols[variable][reorder[variable][j]] + i] = 
	A[12 * j + i];
}

void addEquationsForSaturation(double *u,
			       double *eq1, double *eq2, double *eqt,
			       int variable) {
  int deg1ind[3][3] = {{0,1,2},
		       {0,1,3},
		       {0,2,3}};
  int deg2ind[3][6] = {{0, 1, 2, 4, 5, 6},
		       {0, 1, 3, 4, 7, 9},
		       {0, 2, 3, 6, 8, 9}};
  int *mapping;
  int currentRow;
  currentRow = 0;

  // multiply eqt with monomials.
  for(int i = 0; i < 6; i++) {
    mapping = multMap[deg2ind[variable][5 - i]];
    multiply(eqt, u, mapping, 73, currentRow, 12, N_MONS);
    currentRow++;
  }
  // multiply eq1 and eq2 with monomials.
  for(int i = 0; i < 3; i++) {
    mapping = multMap[deg1ind[variable][2 - i]];
    multiply(eq1, u, mapping, 73, currentRow, 12, N_MONS);
    multiply(eq2, u, mapping, 73, currentRow + 3, 12, N_MONS);
    currentRow++;
  }
}

void addEquations(double* C, double eqs[N_INITIAL_EQS][209], int startRow) {
  int degree5Ind[3] = {2,5,8};
  int degree6Ind[6] = {0,1,3,4,6,7};
  int nextAInd = 0;

  // equations of degree 6
  for(int j = 19; j >= 0; j--) {
    for(int i = 0; i < N_DEGREE_6; i++) {
      multiply(eqs[degree6Ind[i]], C, multMap[j], N_DEGREE_6_MONS, 
	       nextAInd, N_EQS, N_MONS);
      nextAInd++;
    }
  }
  
  // equations of degree 5
  for(int j = 34; j >= 0; j--) {
    for(int i = 0; i < N_DEGREE_5; i++) {
      multiply(eqs[degree5Ind[i]], C, multMap[j],
	       j < 20 ? N_DEGREE_6_MONS : N_DEGREE_5_MONS,
	       nextAInd, N_EQS, N_MONS);
      nextAInd++;
    }
  }
}

void changeBack(double X[][3], double *HworldInv) {
  // TODO LATER: speed up by making X one-dim.
  for(int i = 0; i < N_SOLUTIONS; i++) {
    double Xtmp1[4], Xtmp2[4];
    blasCopy(X[i], Xtmp1, 3);
    Xtmp1[3] = 1;
    ATimesx(HworldInv, Xtmp1, Xtmp2, 4, 4);
    Xtmp2[0] /= Xtmp2[3];
    Xtmp2[1] /= Xtmp2[3];
    Xtmp2[2] /= Xtmp2[3];
    blasCopy(Xtmp2, X[i], 3);
  }
}

// solve by eigenvector method.
//void extractSolutions(double X[][3], double *mx, int *nSols) {
//}

// solve by eigenvalue method.
void extractSolutions(double X[][3],
		      double *mx, double *my, double *mz,
		      int *nSols) {
  
  int nReal = 0;
  double xRealEig[N_BASIS], yRealEig[N_BASIS], zRealEig[N_BASIS];
  double imagEig[N_BASIS];
  double V[N_BASIS * N_BASIS];
  int realInd[N_BASIS];

  // eigen-decompose.
#ifdef LINUX_OS
  int info;
  dgeev('V', 'N', N_BASIS, mx, N_BASIS, xRealEig, imagEig, V, N_BASIS,
  	NULL, N_BASIS, &info);
#endif
#ifdef MAC_OS
  __CLPK_integer nBasis = N_BASIS;
  __CLPK_integer info;
  __CLPK_integer lwork = -1;
  __CLPK_doublereal workQuery[1];
  __CLPK_doublereal *work;
  dgeev_("V", "N", &nBasis, (__CLPK_doublereal*) mx, &nBasis,
	(__CLPK_doublereal*) xRealEig,
	(__CLPK_doublereal*) imagEig,
	(__CLPK_doublereal*) V,
	&nBasis, NULL, &nBasis,
	workQuery, &lwork,
	&info);
  lwork = (__CLPK_integer) floor(workQuery[0]);
  work = new __CLPK_doublereal[lwork];
  dgeev_("V", "N", &nBasis, (__CLPK_doublereal*) mx, &nBasis,
	(__CLPK_doublereal*) xRealEig,
	(__CLPK_doublereal*) imagEig,
	(__CLPK_doublereal*) V,
	&nBasis, NULL, &nBasis,
	work, &lwork,
	&info);
  delete[] work;
#endif

  // count the number of real solutions.
  for(int i = 0; i < N_BASIS; i++) {
    if(imagEig[i] == 0) {
      realInd[nReal] = i;
      X[nReal][0] = xRealEig[i];
      nReal++;
    }
  }
  *nSols = nReal;

  // extract eigenvectors of real solutions.
  double *Vreal = new double[N_BASIS * nReal];
  double *Vtmp = new double[N_BASIS * nReal];
  for(int i = 0; i < nReal; i++)
    blasCopy(V + N_BASIS * realInd[i], Vreal + N_BASIS * i, N_BASIS);

  // compute the eigenvalues for my and mz by multiplication.

  // my:
#ifdef LINUX_OS
  dgemm('T', 'N', N_BASIS, nReal, N_BASIS, 1, my, N_BASIS, Vreal, N_BASIS, 0, Vtmp, N_BASIS);
#endif
#ifdef MAC_OS
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
	      N_BASIS, nReal, N_BASIS, 1, my,
	      N_BASIS, Vreal, N_BASIS, 0, Vtmp, N_BASIS);  
#endif

  // ...and element wise division.
  for(int i = 0; i < N_BASIS; i++)
    for(int j = 0; j < nReal; j++)
      Vtmp[N_BASIS * j + i] /= Vreal[N_BASIS * j + i];
  
  // find the median.
  int midPoint = N_BASIS / 2;
  for(int j = 0; j < nReal; j++) {
    X[j][1] = median(Vtmp + N_BASIS * j, N_BASIS);
  }

  // mz:
#ifdef LINUX_OS
  dgemm('T', 'N', N_BASIS, nReal, N_BASIS, 1, mz, N_BASIS, Vreal, N_BASIS, 0, Vtmp, N_BASIS);
#endif
#ifdef MAC_OS
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
	      N_BASIS, nReal, N_BASIS, 1, mz, N_BASIS,
	      Vreal, N_BASIS, 0, Vtmp, N_BASIS);  
#endif

  for(int i = 0; i < N_BASIS; i++)
    for(int j = 0; j < nReal; j++)
      Vtmp[N_BASIS * j + i] /= Vreal[N_BASIS * j + i];
  
  // find the median.
  for(int j = 0; j < nReal; j++) {
    sort(Vtmp + N_BASIS * j, Vtmp + N_BASIS * (j + 1));
    X[j][2] = Vtmp[N_BASIS * j + midPoint];
  }

  // clean up.
  delete[] Vreal;
  delete[] Vtmp;
}

void buildMx(double *Mx, double *My, double *Mz) {
  int indX[154] = {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, 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, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 132, 133, 134, 135, 138, 44, 88, 89, 90, 121, 122, 123, 124, 125, 126, 136, 137, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 154, 155, 156, 157, 169, 170, 158, 171, 172, 173, 174, 175, 176, 177, 178, 185, 186, 187, 188, 190, 191, 192, 193, 194};
  int indY[154] = {1, 3, 4, 6, 7, 8, 10, 11, 12, 13, 15, 16, 17, 18, 19, 21, 22, 23, 24, 25, 26, 28, 29, 30, 31, 32, 33, 34, 36, 37, 38, 39, 40, 41, 42, 43, 45, 46, 47, 48, 49, 50, 51, 52, 56, 58, 59, 61, 62, 63, 65, 66, 67, 68, 70, 71, 72, 73, 74, 76, 77, 78, 79, 80, 81, 83, 84, 85, 86, 87, 88, 89, 91, 92, 93, 94, 95, 100, 102, 103, 105, 106, 107, 109, 110, 111, 112, 114, 115, 116, 117, 118, 120, 121, 122, 123, 124, 125, 127, 133, 135, 136, 138, 142, 53, 96, 97, 98, 128, 129, 130, 131, 160, 161, 139, 140, 143, 144, 145, 147, 148, 149, 150, 151, 153, 163, 164, 165, 166, 167, 155, 157, 169, 158, 171, 172, 174, 175, 176, 177, 179, 180, 181, 182, 183, 186, 188, 189, 191, 193, 195, 196, 197, 198};
  int indZ[154] = {2, 4, 5, 7, 8, 9, 11, 12, 13, 14, 16, 17, 18, 19, 20, 22, 23, 24, 25, 26, 27, 29, 30, 31, 32, 33, 34, 35, 37, 38, 39, 40, 41, 42, 43, 44, 46, 47, 48, 49, 50, 51, 52, 53, 57, 59, 60, 62, 63, 64, 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 84, 85, 86, 87, 88, 89, 90, 92, 93, 94, 95, 96, 101, 103, 104, 106, 107, 108, 110, 111, 112, 113, 115, 116, 117, 118, 119, 121, 122, 123, 124, 125, 126, 128, 134, 136, 137, 139, 143, 54, 97, 98, 159, 129, 130, 131, 160, 161, 162, 140, 141, 144, 145, 146, 148, 149, 150, 151, 152, 163, 164, 165, 166, 167, 168, 156, 169, 170, 171, 172, 173, 175, 176, 177, 178, 180, 181, 182, 183, 184, 187, 189, 190, 192, 194, 196, 197, 198, 199};
  zeros(Mx, N_MONS, N_BASIS);
  zeros(My, N_MONS, N_BASIS);
  zeros(Mz, N_MONS, N_BASIS);
  for(int j = 0; j < N_BASIS; j++) {
    Mx[N_MONS * j + indX[j]] = 1;
    My[N_MONS * j + indY[j]] = 1;
    Mz[N_MONS * j + indZ[j]] = 1;
  }
}

void buildP(double* P, double* Cprim) {
  for(int i = 0; i < N_BASIS; i++) {
    // quotient space part (identity).
    for(int j = 0; j < N_BASIS; j++)
      P[N_BASIS * (N_ELIMCOLS + j) + i] = (i == j);
     
    // reduction part.
    for(int j = 0; j < N_ELIMCOLS; j++)
      P[N_BASIS * j + i] = -Cprim[N_EQS * i + j];
  }
}