tvt.rar_triangulation

#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,