void compute_planar_homography_n_points(float *x0, float *y0, float *x1, float *y1, int n, float **H) { /////////////////////////////////////////////////////// Normalize points to have baricenter [0][0] and standard deviation sqrt(2.0f) //if (DEBUG) printf("Compute homography with %d matches\n", n); ////////////////// Compute Baricenter float lx = 0.0, ly = 0.0; float rx = 0.0, ry = 0.0; for (int i=0; i< n; i++) { lx += x0[i]; ly += y0[i]; rx += x1[i]; ry += y1[i]; } lx /= (float) n; ly /= (float) n; rx /= (float) n; ry /= (float) n; /////////////// Normalize points without modifying original vectors float *px0 = new float[n]; float *py0 = new float[n]; float *px1 = new float[n]; float *py1 = new float[n]; float spl= 0.0f, spr = 0.0f; for(int i=0; i < n; i++) { px0[i] = x0[i] - lx; py0[i] = y0[i] - ly; px1[i] = x1[i] - rx; py1[i] = y1[i] - ry; spl += sqrtf(px0[i] * px0[i] + py0[i]*py0[i]); spr += sqrtf(px1[i] * px1[i] + py1[i]*py1[i]); } spl = sqrtf(2.0f) / spl; spr = sqrtf(2.0f) / spr; for (int i=0; i< n; i++) { px0[i] *= spl; py0[i] *= spl; px1[i] *= spr; py1[i] *= spr; } /////////////////////////////////////////////////////// Minimization problem || Ah || float **Tpl = allocate_float_matrix(3,3); float **Timinv = allocate_float_matrix(3,3); // similarity transformation of the plane Tpl[0][0] = spl; Tpl[0][1] = 0.0; Tpl[0][2] = -spl*lx; Tpl[1][0] = 0.0; Tpl[1][1] = spl; Tpl[1][2] = -spl*ly; Tpl[2][0] = 0.0; Tpl[2][1] = 0.0; Tpl[2][2] = 1.0; // inverse similarity transformation of the image Timinv[0][0] = 1.0/spr; Timinv[0][1] = 0.0 ; Timinv[0][2] = rx; Timinv[1][0] = 0.0 ; Timinv[1][1] = 1.0/spr; Timinv[1][2] = ry; Timinv[2][0] = 0.0 ; Timinv[2][1] = 0.0 ; Timinv[2][2] = 1.0; ///////////////// System matrix float **A = allocate_float_matrix(2*n,9); for(int i=0, eq=0; i < n; i++, eq++) { float xpl = px0[i], ypl = py0[i], xim = px1[i], yim = py1[i]; A[eq][0] = A[eq][1] = A[eq][2] = 0.0; A[eq][3] = -xpl; A[eq][4] = -ypl; A[eq][5] = -1.0; A[eq][6] = yim * xpl; A[eq][7] = yim * ypl; A[eq][8] = yim; eq++; A[eq][0] = xpl; A[eq][1] = ypl; A[eq][2] = 1.0; A[eq][3] = A[eq][4] = A[eq][5] = 0.0; A[eq][6] = -xim * xpl; A[eq][7] = -xim * ypl; A[eq][8] = -xim; } ///////////////// SVD float **U = allocate_float_matrix(2*n,9); float **V = allocate_float_matrix(9,9); float *W = new float[9]; compute_svd(A,U,V,W,2*n,9); // Find the index of the least singular value int imin = 0; for (int i=1; i < 9; i++) if ( W[i] < W[imin] ) imin = i; ////////////////// Denormalize H = Timinv * V.col(imin)* Tpl; float **matrix = allocate_float_matrix(3,3); float **result = allocate_float_matrix(3,3); int k=0; for(int i=0; i < 3; i++) for(int j=0; j < 3; j++, k++) matrix[i][j] = V[k][imin]; product_square_float_matrixes(result,Timinv,matrix,3); product_square_float_matrixes(H,result,Tpl,3); desallocate_float_matrix(U ,2*n,9); desallocate_float_matrix(V,9,9); delete[] W; desallocate_float_matrix(Tpl,3,3); desallocate_float_matrix(Timinv,3,3); desallocate_float_matrix(matrix,3,3); desallocate_float_matrix(result,3,3); desallocate_float_matrix(A,2*n,9); delete[] px0; delete[] py0; delete[] px1; delete[] py1; }
/// Compute homography using svd + Ransac void compute_ransac_planar_homography_n_points(float *x0, float *y0, float *x1, float *y1, int n, int niter, float tolerance, float **H) { // Initialize seed srand( (long int) time (NULL)); float tolerance2 = tolerance * tolerance; float **Haux = allocate_float_matrix(3,3); int *accorded = new int[n]; int *paccorded = new int[n]; int naccorded = 0; int pnaccorded = 0; for(int iter = 0; iter < niter; iter++) { // Choose 4 indexos from 1..n without repeated values int indexos[4]; int acceptable = 0; while (!acceptable) { acceptable = 1; for(int i=0; i < 4; i++) indexos[i] = (int) floor(rand()/(double)RAND_MAX * (double) n); // Check if indexos are repeated for(int i=0; i < 4; i++) for(int j=i+1; j < 4; j++) if (indexos[i] == indexos[j]) acceptable = 0; } // Store selected matches float px0[4] , py0[4], px1[4] , py1[4]; for(int i=0; i < 4; i++) { px0[i] = x0[indexos[i]]; py0[i] = y0[indexos[i]]; px1[i] = x1[indexos[i]]; py1[i] = y1[indexos[i]]; } // Compute planar homography compute_planar_homography_n_points(px0, py0, px1, py1, 4, Haux); // Which matches are according to this transformation pnaccorded = 0; for(int i=0; i < n; i++) { float vec[3]; vec[0] = x0[i]; vec[1] = y0[i]; vec[2] = 1.0f; float res[3]; float_vector_matrix_product(Haux, vec ,res , 3); if (res[2] != 0.0f) { res[0] /= res[2]; res[1] /= res[2]; float dif = (res[0] - x1[i]) * (res[0] - x1[i]) + (res[1] - y1[i]) * (res[1] - y1[i]); if (dif < tolerance2) { paccorded[pnaccorded] = i; pnaccorded++; } } } if (DEBUG) printf("%d: %d %d %d %d --> %d \n", iter, indexos[0],indexos[1],indexos[2],indexos[3], pnaccorded); if (pnaccorded > naccorded) { naccorded = pnaccorded; for(int i=0; i < naccorded; i++) accorded[i] = paccorded[i]; for(int i=0; i < 3; i++) for(int j=0; j < 3; j++) H[i][j] = Haux[i][j]; } } desallocate_float_matrix(Haux,3,3); }
void compute_moisan_planar_homography_n_points(float *x0, float *y0, float *x1, float *y1, int n, int niter, float &epsilon, float **H, int &counter, int recursivity) { // Initialize seed srand( (long int) time (NULL) ); float **Haux = allocate_float_matrix(3,3); float *dist = new float[n]; float *dindexos = new float[n]; int nselected = 0; float *i0selected = new float[n]; float *j0selected = new float[n]; float *i1selected = new float[n]; float *j1selected = new float[n]; /* tabulate logcombi */ float loge0 = (float)log10((double)(n-4)); float *logcn = makelogcombi_n(n); float *logc4 = makelogcombi_k(4,n); // Normalize points using minimum and maximum coordinates float xmin, xmax, ymin, ymax; xmin = xmax = x1[0]; ymin = ymax = x1[0]; for(int i=0; i < n; i++) { if (x1[i] < xmin) xmin = x1[i]; if (y1[i] < ymin) ymin = y1[i]; if (x1[i] > xmax) xmax = x1[i]; if (y1[i] > ymax) ymax = y1[i]; } float mepsilon = 10000000.0f; for(int iter = 0; iter < niter; iter++) { // Initializing distances and indexos for the whole vector for(int i=0; i < n ; i++) { dist[i] = 0.0; dindexos[i] = (float) i; } // Choose 4 indexos from 1..n without repeated values int indexos[4]; int acceptable = 0; while (!acceptable) { acceptable = 1; for(int i=0; i < 4; i++) indexos[i] = (int) floor(rand()/(double)RAND_MAX * (double) n); // Check if indexos are repeated for(int i=0; i < 4; i++) for(int j=i+1; j < 4; j++) if (indexos[i] == indexos[j]) acceptable = 0; } // Store selected matches float px0[4] , py0[4], px1[4] , py1[4]; for(int i=0; i < 4; i++) { px0[i] = x0[indexos[i]]; py0[i] = y0[indexos[i]]; px1[i] = x1[indexos[i]]; py1[i] = y1[indexos[i]]; } // Compute planar homography compute_planar_homography_n_points(px0, py0, px1, py1, 4, Haux); for(int i=0; i < n; i++) { float vec[3]; vec[0] = x0[i]; vec[1] = y0[i]; vec[2] = 1.0f; float res[3]; float_vector_matrix_product(Haux, vec ,res , 3); if (res[2] != 0.0f) { res[0] /= res[2]; res[1] /= res[2]; float dif = (res[0] - x1[i]) * (res[0] - x1[i]) + (res[1] - y1[i]) * (res[1] - y1[i]); dist[i] = dif; //if (dif < tolerance2) { paccorded[pnaccorded] = i; pnaccorded++; } } else dist[i] = 100000000.0f; } // Order distances quick_sort(dist,dindexos,n); // Look for most meaningful subset for(int i=4 ; i < n; i++) { float rigidity = dist[i]; float logalpha = 0.5f*(float)log10((double) rigidity); float nfa = (float) loge0 + (float)(i-3) * logalpha + logcn[i+1] + logc4[i+1] ; if (nfa < mepsilon) { mepsilon = nfa; for(int ki=0; ki < 3; ki++) for(int kj=0; kj < 3; kj++) H[ki][kj] = Haux[ki][kj]; nselected = i; for(int ki=0; ki < nselected; ki++) { i0selected[ki] = x0[(int) dindexos[ki]]; j0selected[ki] = y0[(int) dindexos[ki]]; i1selected[ki] = x1[(int) dindexos[ki]]; j1selected[ki] = y1[(int) dindexos[ki]]; } } } } epsilon = mepsilon; counter++; if (counter < recursivity) compute_moisan_planar_homography_n_points(i0selected,j0selected,i1selected,j1selected,nselected,niter,epsilon,H,counter,recursivity); desallocate_float_matrix(Haux,3,3); delete[] dist; delete[] dindexos; delete[] i0selected; delete[] j0selected; delete[] i1selected; delete[] j1selected; }
/* Apply the method developed by Matthew Brown (see BMVC 02 paper) to fit a 3D quadratic function through the DOG function values around the location (s,r,c), i.e., (scale,row,col), at which a peak has been detected. Return the interpolated peak position as a vector in "offset", which gives offset from position (s,r,c). The returned value is the interpolated DOG magnitude at this peak. */ float FitQuadratic(float offset[3], flimage* dogs, int s, int r, int c) { float g[3]; flimage *dog0, *dog1, *dog2; int i; //s = 1; r = 128; c = 128; float ** H = allocate_float_matrix(3, 3); /* Select the dog images at peak scale, dog1, as well as the scale below, dog0, and scale above, dog2. */ dog0 = &dogs[s-1]; dog1 = &dogs[s]; dog2 = &dogs[s+1]; /* Fill in the values of the gradient from pixel differences. */ g[0] = ((*dog2)(c,r) - (*dog0)(c,r)) / 2.0; g[1] = ((*dog1)(c,r+1) - (*dog1)(c,r-1)) / 2.0; g[2] = ((*dog1)(c+1,r) - (*dog1)(c-1,r)) / 2.0; /* Fill in the values of the Hessian from pixel differences. */ H[0][0] = (*dog0)(c,r) - 2.0 * (*dog1)(c,r) + (*dog2)(c,r); H[1][1] = (*dog1)(c,r-1) - 2.0 * (*dog1)(c,r) + (*dog1)(c,r+1); H[2][2] = (*dog1)(c-1,r) - 2.0 * (*dog1)(c,r) + (*dog1)(c+1,r); H[0][1] = H[1][0] = ( ((*dog2)(c,r+1) - (*dog2)(c,r-1)) - ((*dog0)(c,r+1) - (*dog0)(c,r-1)) ) / 4.0; H[0][2] = H[2][0] = ( ((*dog2)(c+1,r) - (*dog2)(c-1,r)) - ((*dog0)(c+1,r) - (*dog0)(c-1,r)) ) / 4.0; H[1][2] = H[2][1] = ( ((*dog1)(c+1,r+1) - (*dog1)(c-1,r+1)) - ((*dog1)(c+1,r-1) - (*dog1)(c-1,r-1)) ) / 4.0; /* Solve the 3x3 linear sytem, Hx = -g. Result, x, gives peak offset. Note that SolveLinearSystem destroys contents of H. */ offset[0] = - g[0]; offset[1] = - g[1]; offset[2] = - g[2]; // for(i=0; i < 3; i++){ // // for(j=0; j < 3; j++) printf("%f ", H[i][j]); // printf("\n"); // } // printf("\n"); // // for(i=0; i < 3; i++) printf("%f ", offset[i]); // printf("\n"); float solution[3]; lusolve(H, solution, offset,3); // printf("\n"); // for(i=0; i < 3; i++) printf("%f ", solution[i]); // printf("\n"); desallocate_float_matrix(H,3,3); delete[] H; /*memcheck*/ /* Also return value of DOG at peak location using initial value plus 0.5 times linear interpolation with gradient to peak position (this is correct for a quadratic approximation). */ for(i=0; i < 3; i++) offset[i] = solution[i]; return ((*dog1)(c,r) + 0.5 * (solution[0]*g[0]+solution[1]*g[1]+solution[2]*g[2])); }