From 69d0d26b0938fe9c11edd8ff312d97737a32e24c Mon Sep 17 00:00:00 2001 From: Julien Michel Date: Fri, 1 Mar 2019 12:59:10 +0000 Subject: [PATCH 1/3] REFAC: Remove ransac binaries and calls --- c/ransac.c | 644 -------------------------------------- c/ransac_cases.c | 796 ----------------------------------------------- makefile | 6 +- s2plib/sift.py | 30 +- 4 files changed, 9 insertions(+), 1467 deletions(-) delete mode 100644 c/ransac.c delete mode 100644 c/ransac_cases.c diff --git a/c/ransac.c b/c/ransac.c deleted file mode 100644 index 5ed76d5f..00000000 --- a/c/ransac.c +++ /dev/null @@ -1,644 +0,0 @@ -#include -#include -#include - -#include "fail.c" -#include "xmalloc.c" -#include "xfopen.c" -#include "random.c" - -// generic function -// evaluate the error of a datapoint according to a model -// (implementing this function is necessary for each ransac case) -typedef float (ransac_error_evaluation_function)( - float *model, - float *datapoint, - void *usr - ); - - -// generic function -// compute the model defined from a few data points -// (shall return 0 if no model could be computed) -// (implementing this function is necessary for each ransac case) -typedef int (ransac_model_generating_function)( - float *out_model, // parameters of the computed model - float *data, // data points - void *usr - ); - -// generic function -// tell whether a given model is good enough (e.g., not severely distorted) -// (this function is optional, and only serves as an optimization hint) -typedef bool (ransac_model_accepting_function)( - float *model, - void *usr); - - -// API function: evaluate a given model over the data, and fill a mask with the -// inliers (according to the given allowed error). This function returns the -// number of inliers. -int ransac_trial( - // output - bool *out_mask, // array mask identifying the inliers - - // input data - float *data, // array of input data - float *model, // parameters of the model - float max_error, // maximum allowed error - - // input context - int datadim, // dimension of each data point - int n, // number of data points - ransac_error_evaluation_function *mev, - - // decoration - void *usr - ) -{ - int cx = 0; - for (int i = 0; i < n; i++) - { - float *datai = data + i*datadim; - float e = mev(model, datai, usr); - if (!(e >= 0)) fprintf(stderr, "WARNING e = %g\n", e); - assert(e >= 0); - out_mask[i] = e < max_error; - if (out_mask[i]) - cx += 1; - } - return cx; -} - -// utility function: return a random number in the interval [a, b) -static int random_index(int a, int b) -{ - int r = a + lcg_knuth_rand()%(b - a); - assert(r >= a); - assert(r < b); - return r; -} - -// comparison function for the qsort call below -static int compare_ints(const void *aa, const void *bb) -{ - const int *a = (const int *)aa; - const int *b = (const int *)bb; - return (*a > *b) - (*a < *b); -} - -// check whether a vector of n ints has different entries -// (sorts the vector inplace) -static bool are_different(int *t, int n) -{ - qsort(t, n, sizeof*t, compare_ints); - for (int i = 1; i < n; i++) - if (t[i-1] == t[i]) - return false; - return true; -} - -static int randombounds(int a, int b) -{ - if (b < a) - fail("the interval [%d, %d] is empty!", a, b); - if (b == a) - return b; - return a + lcg_knuth_rand()%(b - a + 1); -} - -static void swap(void *a, void *b, size_t s) -{ -#if 0 -#error "memcpy is way slower!" - char t[s]; - memcpy(t, a, s); - memcpy(a, b, s); - memcpy(b, t, s); -#else - char *x = a; - char *y = b; - for (unsigned int i = 0; i < s; i++, x++, y++) - { - char t = *x; - *x = *y; - *y = t; - } -#endif -} - -// fisher-yates -void shuffle(void *t, int n, size_t s) -{ - char *c = t; - - for (int i = 0; i < n-1; i++) - swap(c + s*i, c + s*randombounds(i, n-1), s); -} - -static void fill_random_shuffle(int *idx, int n, int a, int b) -{ - int m = b - a; - assert(n > 0); - assert(m > 0); - assert(m >= n); - int t[m]; - for (int i = a; i < b; i++) - t[i] = i; - shuffle(t, m, sizeof*t); - for (int i = 0; i < n; i++) - idx[i] = t[i]; -} - -// generate a set of n different ints between a and b -static void fill_random_indices(int *idx, int n, int a, int b) -{ - if (b-a==n) {for(int i=0;i (b-a)) {fill_random_shuffle(idx, n, a, b);return;} - // TODO fisher yates shuffle and traverse it by blocks of length nfit - int safecount = 0; - do { - for (int i = 0; i < n; i++) - idx[i] = random_index(a, b); - safecount += 1; - } while (safecount < 100 && !are_different(idx, n)); - if (safecount == 100) - fail("could not generate any model"); - //fprintf(stderr, "fri"); - //for (int i = 0; i < n; i++) - // fprintf(stderr, "\t%d", idx[i]); - //fprintf(stderr, "\n"); -} - -#define MAX_MODELS 10 - - -// RANSAC -// -// Given a list of data points, find the parameters of a model that fits to -// those points. Several models are tried, and the model with the highest -// number of inliers is kept. -// -// A basic idea of this kind of ransac is that a maximum allowed error is fixed -// by hand, and then the inliers of a model are defined as the data points -// which fit the model up to the allowed error. The RANSAC algorithm randomly -// tries several models and keeps the one with the largest number of inliers. -int ransac( - // output - //int *out_ninliers, // number of inliers - bool *out_mask, // array mask identifying the inliers - float *out_model, // model parameters - - // input data - float *data, // array of input data - - // input context - int datadim, // dimension of each data point - int n, // number of data points - int modeldim, // number of model parameters - ransac_error_evaluation_function *mev, - ransac_model_generating_function *mgen, - int nfit, // data points needed to produce a model - - // input parameters - int ntrials, // number of models to try - int min_inliers, // minimum allowed number of inliers - float max_error, // maximum allowed error - - // decoration - ransac_model_accepting_function *macc, - void *usr - ) -{ - fprintf(stderr, "running RANSAC over %d datapoints of dimension %d\n", - n, datadim); - fprintf(stderr, "will try to find a model of size %d from %d points\n", - modeldim, nfit); - fprintf(stderr, "we will make %d trials and keep the best with e<%g\n", - ntrials, max_error); - fprintf(stderr, "a model must have more than %d inliers\n", - min_inliers); - - if (n < nfit) - return 0; - int best_ninliers = 0; - float best_model[modeldim]; - bool *best_mask = xmalloc(n * sizeof*best_mask); - bool *tmp_mask = xmalloc(n * sizeof*best_mask); - - for (int i = 0; i < ntrials; i++) - { - int indices[nfit]; - fill_random_indices(indices, nfit, 0, n); - - float x[nfit*datadim]; - for (int j = 0; j < nfit; j++) - for (int k = 0; k < datadim; k++) - x[datadim*j + k] = data[datadim*indices[j] + k]; - - float model[modeldim*MAX_MODELS]; - int nm = mgen(model, x, usr); - if (!nm) - continue; - if (macc && !macc(model, usr)) - continue; - - // generally, nm=1 - for (int j = 0; j < nm; j++) - { - float *modelj = model + j*modeldim; - int n_inliers = ransac_trial(tmp_mask, data, modelj, - max_error, datadim, n, mev, usr); - - if (n_inliers > best_ninliers) - { - best_ninliers = n_inliers; - for(int k = 0; k < modeldim; k++) - best_model[k] = modelj[k]; - for(int k = 0; k < n; k++) - best_mask[k] = tmp_mask[k]; - } - } - } - - fprintf(stderr, "RANSAC found this best model:"); - for (int i = 0; i < modeldim; i++) - fprintf(stderr, " %g", best_model[i]); - fprintf(stderr, "\n"); - if (0) { - FILE *f = xfopen("/tmp/ramo.txt", "w"); - for (int i = 0; i < modeldim; i++) - fprintf(f,"%lf%c",best_model[i],i==modeldim-1?'\n':' '); - xfclose(f); - } - //fprintf(stderr, "errors of outliers:"); - //for (int i = 0; i < n; i++) - // if (!best_mask[i]) { - // float e = mev(best_model, data+i*datadim, usr); - // fprintf(stderr, " %g", e); - // } - //fprintf(stderr, "\n"); - //fprintf(stderr, "errors of inliers:"); - //for (int i = 0; i < n; i++) - // if (best_mask[i]) { - // float e = mev(best_model, data+i*datadim, usr); - // fprintf(stderr, " %g", e); - // } - //fprintf(stderr, "\n"); - //fprintf(stderr, "errors of data points:\n"); - //for (int i = 0; i < n; i++) { - // float e = mev(best_model, data+i*datadim, usr); - // fprintf(stderr, "\t%g\t%s\n", e, best_mask[i]?"GOOD":"bad"); - //} - - int return_value = 0; - if (best_ninliers >= min_inliers) - { - return_value = best_ninliers; - } else - return_value = 0; - - for (int j = 0; j < modeldim; j++) - if (!isfinite(best_model[j])) - fail("model_%d not finite", j); - - if (out_model) - for(int j = 0; j < modeldim; j++) - out_model[j] = best_model[j]; - if (out_mask) - for(int j = 0; j < n; j++) - out_mask[j] = best_mask[j]; - - free(best_mask); - free(tmp_mask); - - return return_value; -} - -#ifndef OMIT_MAIN - -#include -#include - -#include "ransac_cases.c" // example functions for RANSAC input -#include "parsenumbers.c" // function "read_ascii_floats" - -int main_cases(int c, char *v[]) -{ - if (c != 6 && c != 7 && c != 8) { - fprintf(stderr, "usage:\n\t%s {line,aff,affn,fm} " - // 0 1 - "ntrials maxerr minliers omodel [omask [oinliers]] 5 ? v[5] : 0; - char *filename_omask = c > 6 ? v[6] : 0; - char *filename_inliers = c > 7 ? v[7] : 0; - - // declare context variables - int modeldim, datadim, nfit; - ransac_error_evaluation_function *model_evaluation; - ransac_model_generating_function *model_generation; - ransac_model_accepting_function *model_acceptation = NULL; - void *user_data = NULL; - - - // fill context variables according to ransac case - if (0 == strcmp(model_id, "line")) { - datadim = 2; - modeldim = 3; - nfit = 2; - model_evaluation = distance_of_point_to_straight_line; - model_generation = straight_line_through_two_points; - - } else if (0 == strcmp(model_id, "aff")) { - datadim = 4; - modeldim = 6; - nfit = 3; - model_evaluation = affine_match_error; - model_generation = affine_map_from_three_pairs; - - } else if (0 == strcmp(model_id, "affn")) { - datadim = 4; - modeldim = 6; - nfit = 3; - model_evaluation = affine_match_error; - model_generation = affine_map_from_three_pairs; - model_acceptation = affine_map_is_reasonable; - - } else if (0 == strcmp(model_id, "hom")) { - datadim = 4; - modeldim = 9; - nfit = 4; - model_evaluation = homographic_match_error; - model_generation = homography_from_four; - //model_acceptation = homography_is_reasonable; - model_acceptation = NULL; - - } else if (0 == strcmp(model_id, "aff3d")) { - datadim = 6; - modeldim = 12; - nfit = 4; - model_evaluation = affine3d_match_error; - model_generation = affine3d_map_from_four_pairs; - - } else if (0 == strcmp(model_id, "fm")) { // fundamental matrix - datadim = 4; - modeldim = 9; - nfit = 7; - model_evaluation = epipolar_error; - model_generation = seven_point_algorithm; - //model_acceptation = fundamental_matrix_is_reasonable; - - } else if (0 == strcmp(model_id, "fmn")) { // fundamental matrix - int main_hack_fundamental_matrix(int,char*[]); - return main_hack_fundamental_matrix(c-1, v+1); - } else if (0 == strcmp(model_id, "fmnt")) { // fundamental matrix - int main_hack_fundamental_trimatrix(int,char*[]); - return main_hack_fundamental_trimatrix(c-1, v+1); - } else { - printf("unrecognized model \"%s\"\n", model_id); - return EXIT_FAILURE; - } - - // read input data - int n; - float *data = read_ascii_floats(stdin, &n); - n /= datadim; - - // call the ransac function to fit a model to data - float model[modeldim]; - bool *mask = xmalloc(n * sizeof*mask); - int n_inliers = ransac(mask, model, data, datadim, n, modeldim, - model_evaluation, model_generation, - nfit, ntrials, minliers, maxerr, - model_acceptation, user_data); - - - // print a summary of the results - if (n_inliers > 0) { - printf("RANSAC found a model with %d inliers\n", n_inliers); - printf("parameters ="); - for (int i = 0; i < modeldim; i++) - printf(" %g", model[i]); - printf("\n"); - } else printf("RANSAC found no model\n"); - - // if requested, save the inlying data points - if (filename_inliers) { - FILE *f = xfopen(filename_inliers, "w"); - for (int i = 0; i < n; i++) - if (mask[i]) { - for(int d = 0; d < datadim; d++) - fprintf(f,"%g ",data[i*datadim+d]); - fprintf(f,"\n"); - } - xfclose(f); - } - - // if requested, save the inlier mask - if (filename_omask) { - FILE *f = xfopen(filename_omask, "w"); - for (int i = 0; i < n; i++) - fprintf(f, mask[i]?" 1":" 0"); - fprintf(f, "\n"); - xfclose(f); - } - - // if requested, save the model - if (filename_omodel) { - FILE *f = xfopen(filename_omodel, "w"); - for (int i = 0; i < modeldim; i++) - fprintf(f, "%lf%c", model[i], i==modeldim-1?'\n':' '); - xfclose(f); - } - - - return EXIT_SUCCESS; -} - -int main_hack_fundamental_matrix(int c, char *v[]) -{ - if (c < 4) { - fprintf(stderr, "usage:\n\t%s fmn " - // -1 0 - "ntrials maxerr minliers [inliers] 0) { - printf("RANSAC found a model with %d inliers\n", n_inliers); - printf("parameters ="); - for (int i = 0; i < modeldim; i++) - printf(" %g", model[i]); - printf("\n"); - //fprintf(stderr, "errors of data points:\n"); - //ransac_error_evaluation_function *ep = epipolar_error; - //for (int i = 0; i < n; i++) { - // float e = ep(model, data+i*datadim, NULL); - // fprintf(stderr, "\t%g\t%s\n", e, mask[i]?"GOOD":"bad"); - //} - } else printf("RANSAC found no model\n"); - -#if 0 - float gamod[9] = {2.8709e-09, -4.3669e-08, 1.0966e-02, - 4.0071e-08, 3.6767e-10, 3.4892e-03, - -1.2060e-02, -4.3969e-03, 1.0000e+00}; - printf("as a comparison, use a fixed model:"); - printf("parameters ="); - for (int i = 0; i < modeldim; i++) - printf(" %g", gamod[i]); - printf("\n"); - fprintf(stderr, "errors of data points:\n"); - ransac_error_evaluation_function *ep = epipolar_error; - for (int i = 0; i < n; i++) { - float e = ep(gamod, data+i*datadim, NULL); - fprintf(stderr, "\t%g\t%s\n", e,"unknown"); - } -#endif - - // if needed, print the inliers - if (inliers_filename) { - FILE *f = xfopen(inliers_filename, "w"); - for (int i = 0; i < n; i++) - if (mask[i]) { - for(int d = 0; d < datadim; d++) - fprintf(f,"%g ",data[i*datadim+d]); - fprintf(f,"\n"); - } - xfclose(f); - } - if (false) { - FILE *f = xfopen("/tmp/omask.txt", "w"); - for (int i = 0; i < n; i++) - fprintf(f, mask[i]?" 1":" 0"); - fprintf(f, "\n"); - xfclose(f); - } - - free(mask); - free(data); - - return EXIT_SUCCESS; -} - -int main_hack_fundamental_trimatrix(int c, char *v[]) -{ - if (c < 4) { - fprintf(stderr, "usage:\n\t%s fmnt " - // -1 0 - "ntrials maxerr minliers [inliers] 0) { - printf("RANSAC found a model with %d inliers\n", n_inliers); - printf("parameters ="); - for (int i = 0; i < modeldim; i++) - printf(" %g", model[i]); - printf("\n"); - //fprintf(stderr, "errors of data points:\n"); - //ransac_error_evaluation_function *ep = epipolar_error_triplet; - //for (int i = 0; i < n; i++) { - // float e = ep(model, data+i*datadim, NULL); - // fprintf(stderr, "\t%g\t%s\n", e, mask[i]?"GOOD":"bad"); - //} - } else printf("RANSAC found no model\n"); - -#if 0 - float gamod[9] = {2.8709e-09, -4.3669e-08, 1.0966e-02, - 4.0071e-08, 3.6767e-10, 3.4892e-03, - -1.2060e-02, -4.3969e-03, 1.0000e+00}; - printf("as a comparison, use a fixed model:"); - printf("parameters ="); - for (int i = 0; i < modeldim; i++) - printf(" %g", gamod[i]); - printf("\n"); - fprintf(stderr, "errors of data points:\n"); - ransac_error_evaluation_function *ep = epipolar_error; - for (int i = 0; i < n; i++) { - float e = ep(gamod, data+i*datadim, NULL); - fprintf(stderr, "\t%g\t%s\n", e,"unknown"); - } -#endif - - // if needed, print the inliers - if (inliers_filename) { - FILE *f = xfopen(inliers_filename, "w"); - for (int i = 0; i < n; i++) - if (mask[i]) { - for(int d = 0; d < datadim; d++) - fprintf(f,"%g ",data[i*datadim+d]); - fprintf(f,"\n"); - } - xfclose(f); - } - if (false) { - FILE *f = xfopen("/tmp/omask.txt", "w"); - for (int i = 0; i < n; i++) - fprintf(f, mask[i]?" 1":" 0"); - fprintf(f, "\n"); - xfclose(f); - } - - - free(mask); - free(data); - - return EXIT_SUCCESS; -} - - - -int main(int c, char *v[]) -{ - return main_cases(c, v); -} -#endif//OMIT_MAIN diff --git a/c/ransac_cases.c b/c/ransac_cases.c deleted file mode 100644 index be2ab640..00000000 --- a/c/ransac_cases.c +++ /dev/null @@ -1,796 +0,0 @@ -#include "vvector.h" - - -// RANSAC INPUT EXAMPLES - -// ************************************ -// ************************************ -// ************************************ -// ************************************ -// -// 1. straight lines -// datadim = 2 (coordinates of each point) -// modeldim = 3 (coefficients of straight line) -// nfit = 2 (number of points that determine a straight line) - -// instance of "ransac_error_evaluation_function" -float distance_of_point_to_straight_line(float *line, float *point, void *usr) -{ - float n = hypot(line[0], line[1]); - float a = line[0]/n; - float b = line[1]/n; - float c = line[2]/n; - float x = point[0]; - float y = point[1]; - float e = a*x + b*y + c; - return fabs(e); -} - - -// instance of "ransac_model_generating_function" -int straight_line_through_two_points(float *line, float *points, void *usr) -{ - float ax = points[0]; - float ay = points[1]; - float bx = points[2]; - float by = points[3]; - float n = hypot(bx - ax, by - ay); - if (!n) return 0; - float dx = -(by - ay)/n; - float dy = (bx - ax)/n; - line[0] = dx; - line[1] = dy; - line[2] = -(dx*ax + dy*ay); - - // assert that the line goes through the two points - float e1 = distance_of_point_to_straight_line(line, points, NULL); - float e2 = distance_of_point_to_straight_line(line, points+2, NULL); - assert(hypot(e1, e2) < 0.001); - return 1; -} - -int find_straight_line_by_ransac(bool *out_mask, float line[3], - float *points, int npoints, - int ntrials, float max_err) -{ - return ransac(out_mask, line, points, 2, npoints, 3, - distance_of_point_to_straight_line, - straight_line_through_two_points, - 4, ntrials, 3, max_err, NULL, NULL); -} - -// ************************************ -// ************************************ -// ************************************ -// ************************************ -// -// 2. affine maps between pairs of points -// datadim = 4 (coordinates of each pair of points) -// modeldim = 6 (coefficients of the affine map) -// nfit = 3 (number of pairs that determine an affine map) - -// utility function: evaluate an affine map -static void affine_map(float *y, float *A, float *x) -{ - y[0] = A[0]*x[0] + A[1]*x[1] + A[2]; - y[1] = A[3]*x[0] + A[4]*x[1] + A[5]; -} - -// instance of "ransac_error_evaluation_function" -static float affine_match_error(float *aff, float *pair, void *usr) -{ - float p[2] = {pair[0], pair[1]}; - float q[2] = {pair[2], pair[3]}; - float ap[2]; - affine_map(ap, aff, p); - float e = hypot(ap[0] - q[0], ap[1] - q[1]); - return e; -} - - -// naive linear algebra -static void apply_matrix6(double y[6], double A[6][6], double x[6]) -{ - for (int i = 0; i < 6; i++) - { - double r = 0; - for (int j = 0; j < 6; j++) - r += A[i][j] * x[j]; - y[i] = r; - } -} - -// FIND AFFINITY -// -// Assuming that there is an affine tranformation -// -// A*x = x' -// -// or, in coordinates, -// -// /p q a\ /x\ /x'| -// |r s b| . |y| = |x'| -// \0 0 1/ \1/ \1 / -// -// the following function finds the matrix A from a set of three point pairs. -// It returns a condition number. If the condition number is too close to -// zero, the result may be bad. -// -// A[] = {p, q, a, r, s, b} -// -static -double find_affinity(double *A, double *x, double *y, double *xp, double *yp) -{ - double caca = y[1]*x[0] + y[2]*x[1] + x[2]*y[0] - - x[1]*y[0] - x[2]*y[1] - y[2]*x[0]; - double invB[6][6] = { - {y[1]-y[2], y[2]-y[0], y[0]-y[1], 0, 0, 0}, - {x[2]-x[1], x[0]-x[2], x[1]-x[0], 0, 0, 0}, - {x[1]*y[2]-x[2]*y[1], x[2]*y[0]-x[0]*y[2], - x[0]*y[1]-x[1]*y[0], 0, 0, 0}, - {0, 0, 0, y[1]-y[2], y[2]-y[0], y[0]-y[1]}, - {0, 0, 0, x[2]-x[1], x[0]-x[2], x[1]-x[0]}, - {0, 0, 0, x[1]*y[2]-x[2]*y[1], x[2]*y[0]-x[0]*y[2], - y[1]*x[0]-x[1]*y[0]} - }; - for (int i = 0; i < 6; i++) - for (int j = 0; j < 6; j++) - invB[j][i] /= caca; - double Xp[6] = {xp[0], xp[1], xp[2], yp[0], yp[1], yp[2]}; - apply_matrix6(A, invB, Xp); - return caca; -} - -// instance of "ransac_model_generating_function" -int affine_map_from_three_pairs(float *aff, float *pairs, void *usr) -{ - // call the function "find_affinity" from elsewhere - double x[3] = {pairs[0], pairs[4], pairs[8]}; - double y[3] = {pairs[1], pairs[5], pairs[9]}; - double xp[3] = {pairs[2], pairs[6], pairs[10]}; - double yp[3] = {pairs[3], pairs[7], pairs[11]}; - double A[6]; - double r = find_affinity(A, x, y, xp, yp); - for (int i = 0; i < 6; i++) - aff[i] = A[i]; - return fabs(r) > 0.001; -} - -// instance of "ransac_model_accepting_function" -bool affine_map_is_reasonable(float *aff, void *usr) -{ - float a = aff[0]; - float b = aff[1]; - float c = aff[3]; - float d = aff[4]; - float det = a*d - b*c; - if (det < 0) return false; - if (fabs(det) > 100) return false; - if (fabs(det) < 0.01) return false; - double n = a*a + b*b + c*c + d*d; - if (n > 10) return false; - if (n < 0.1) return false; - return true; -} - -int find_affinity_by_ransac(bool *out_mask, float affinity[6], - float *pairs, int npairs, - int ntrials, float max_err) -{ - return ransac(out_mask, affinity, pairs, 4, npairs, 6, - affine_match_error, - affine_map_from_three_pairs, - 3, ntrials, 4, max_err, - affine_map_is_reasonable, NULL); -} - -// ************************************ -// ************************************ -// ************************************ -// ************************************ -// -// 3. projective maps between pairs of points -// datadim = 4 (coordinates of each pair of points) -// modeldim = 9 (coefficients of the homographic map) -// nfit = 4 (number of pairs that determine an homographic map) - -// ************************************ -// ************************************ -// ************************************ -// ************************************ -// -// -// TODO: homograpy, fundamental matrix - -#include "homographies.c" - -// instance of "ransac_error_evaluation_function" -static float homographic_match_error(float *hom, float *pair, void *usr) -{ - double p[2] = {pair[0], pair[1]}; - double q[2] = {pair[2], pair[3]}; - double H[3][3] = {{hom[0], hom[1], hom[2]}, - {hom[3], hom[4], hom[5]}, - {hom[6], hom[7], hom[8]}}; - double Hp[2]; - apply_homography(Hp, H, p); - double r = hypot(Hp[0] - q[0], Hp[1] - q[1]); - return r; -} - - -// instance of "ransac_model_generating_function" -int homography_from_four(float *hom, float *pairs, void *usr) -{ - double a[2] = {pairs[0], pairs[1]}; - double x[2] = {pairs[2], pairs[3]}; - double b[2] = {pairs[4], pairs[5]}; - double y[2] = {pairs[6], pairs[7]}; - double c[2] = {pairs[8], pairs[9]}; - double z[2] = {pairs[10], pairs[11]}; - double d[2] = {pairs[12], pairs[13]}; - double w[2] = {pairs[14], pairs[15]}; - double R[3][3], *RR=R[0]; - homography_from_eight_points(R, a, b, c, d, x, y, z, w); - int r = 1; - for (int i = 0; i < 9; i++) - if (isfinite(RR[i])) - hom[i] = RR[i]; - else - r = 0; - return r; -} - -//// instance of "ransac_model_accepting_function" -//bool homography_is_resaonable(float *hom, void *usr) -//{ -// // 0 1 2 a b p -// // 3 4 5 c d q -// // 6 7 8 r s t -// if (fabs(t) < 1e-15) return false; -// float a = hom[0]; -// float b = aff[1]; -// float c = aff[3]; -// float d = aff[4]; -// float det = a*d - b*c; -// if (det < 0) return false; -// if (fabs(det) > 100) return false; -// if (fabs(det) < 0.01) return false; -// double n = a*a + b*b + c*c + d*d; -// if (n > 10) return false; -// if (n < 0.1) return false; -// return true; -//} - - -// ************************************ -// ************************************ -// ************************************ -// ************************************ -// -// 4. fundamental matrices -// datadim = 4 (coordinates of each pair) -// modeldim = 9 (fundamental matrix) -// nfit = 7 (seven-point algorithm) - - -static float fnorm(float *a, int n) -{ - if (n == 1) - return fabs(*a); - if (n == 2) - return hypot(a[0], a[1]); - else - return hypot(*a, fnorm(a+1, n-1)); -} - -#include "moistiv_epipolar.c" -// instance of "ransac_model_generating_function" -int seven_point_algorithm(float *fm, float *p, void *usr) -{ - int K[7] = {0, 1, 2, 3, 4, 5, 6}; - float m1[14] = {p[0],p[1], p[4],p[5], p[8],p[9], p[12],p[13], - p[16],p[17], p[20],p[21], p[24],p[25] }; - float m2[14] = {p[2],p[3], p[6],p[7], p[10],p[11], p[14],p[15], - p[18],p[19], p[22],p[23], p[26],p[27] }; - float z[3], F1[4][4]={{0}}, F2[4][4]={{0}}; - // this is so braindead it's not funny - int r = moistiv_epipolar(m1, m2, K, z, F1, F2); - //MAT_PRINT_4X4(F1); - //MAT_PRINT_4X4(F2); - int ridx = 0; - //if (r == 3) ridx = random_index(0, 3); - //fprintf(stderr, "z = %g\n", z[ridx]); - int cx = 0; - for (int k = 0; k < r; k++) - for (int j = 1; j <= 3; j++) - for (int i = 1; i <= 3; i++) - if (isfinite(z[k])) - fm[cx++] = F1[i][j] + z[k]*F2[i][j]; - //fprintf(stderr, "\tspa = %g %g %g %g %g %g %g %g %g\n", - // fm[0], fm[1], fm[2], - // fm[3], fm[4], fm[5], - // fm[6], fm[7], fm[8]); - for (int k = 0; k < r; k++) - { - float *fmk = fm + 9*k; - float nfmk = fnorm(fmk, 9); - for (int i = 0; i < 9; i++) - fmk[i] /= nfmk; - } - assert(r==0 || r==1 || r==2 || r == 3); - return r; -} - -// instance of "ransac_error_evaluation_function" -static float epipolar_algebraic_error(float *fm, float *pair, void *usr) -{ - float p[3] = {pair[0], pair[1], 1}; - float q[3] = {pair[2], pair[3], 1}; - float fp[3] = {fm[0]*p[0] + fm[1]*p[1] + fm[2], - fm[3]*p[0] + fm[4]*p[1] + fm[5], - fm[6]*p[0] + fm[7]*p[1] + fm[8]}; - float qfp = q[0]*fp[0] + q[1]*fp[1] + q[2]*fp[2]; - return fabs(qfp); -} - -// instance of "ransac_error_evaluation_function" -static float epipolar_euclidean_error(float *fm, float *pair, void *usr) -{ - float p[3] = {pair[0], pair[1], 1}; - float q[3] = {pair[2], pair[3], 1}; - float pf[3] = {fm[0]*p[0] + fm[3]*p[1] + fm[6], - fm[1]*p[0] + fm[4]*p[1] + fm[7], - fm[2]*p[0] + fm[5]*p[1] + fm[8]}; - float npf = hypot(pf[0], pf[1]); - if (npf == 0) // the epipolar line is (0, 0, 1), ie the line at infinity - return INFINITY; - else { - pf[0] /= npf; pf[1] /= npf; pf[2] /= npf; - float pfq = pf[0]*q[0] + pf[1]*q[1] + pf[2]*q[2]; - return fabs(pfq); - } -} - -// instance of "ransac_error_evaluation_function" -static float epipolar_euclidean_error_sym(float *fm, float *pair, void *usr) -{ - float Tfm[9] = {fm[0], fm[3], fm[6], - fm[1], fm[4], fm[7], - fm[2], fm[5], fm[8]}; - float Tpair[4] = {pair[2], pair[3], pair[0], pair[1]}; - float e1 = epipolar_euclidean_error(fm, pair, usr); - float e2 = epipolar_euclidean_error(Tfm, Tpair, usr); - return fmax(e1, e2); -} - - -//// instance of "ransac_error_evaluation_function" -//static float epipolar_symmetric_euclidean_error(float *fm, float *pair, void *u) -//{ -// fail("must transpose F!"); -// float rpair[4] = {pair[2], pair[3], pair[0], pair[1]}; -// float a = epipolar_euclidean_error(fm, pair, u); -// float b = epipolar_euclidean_error(fm, rpair, u); -// return hypot(a, b); -//} - -// instance of "ransac_error_evaluation_function" -static float epipolar_error(float *fm, float *pair, void *usr) -{ - ransac_error_evaluation_function *f; - //f = epipolar_algebraic_error; - f = epipolar_euclidean_error; - //f = epipolar_euclidean_error_sym; - return f(fm, pair, usr); -} - -// instance of "ransac_error_evaluation_function" -static float epipolar_error_triplet(float *fm, float *pair, void *usr) -{ - ransac_error_evaluation_function *f; - f = epipolar_euclidean_error; - float pair2[4] = {pair[0], pair[1], pair[4], pair[5]}; - float fa = f(fm, pair, usr); - float fb = f(fm+9, pair2, usr); - return hypot(fa,fb); -} - -// instance of "ransac_model_generating_function" -int two_seven_point_algorithms(float *fm, float *p, void *usr) -{ - // p has 42 numbers - float p1[28] = {p[0],p[1],p[2],p[3], - p[6],p[7],p[8],p[9], - p[12],p[13],p[14],p[15], - p[18],p[19],p[20],p[21], - p[24],p[25],p[26],p[27], - p[30],p[31],p[32],p[33], - p[36],p[37],p[38],p[39]}; - float p2[28] = {p[0],p[1],p[4],p[5], - p[6],p[7],p[10],p[11], - p[12],p[13],p[16],p[17], - p[18],p[19],p[22],p[23], - p[24],p[25],p[28],p[29], - p[30],p[31],p[34],p[35], - p[36],p[37],p[40],p[41]}; - float fm1[27], fm2[27]; - int r1 = seven_point_algorithm(fm1, p1, usr); - int r2 = seven_point_algorithm(fm2, p2, usr); - for (int i = 0; i < r1; i++) - for (int j = 0; j < r2; j++) { - float *fmij = fm + 18*(r1*i+j); - for (int k = 0; k < 9; k++) { - fmij[k] = fm1[9*i+k]; - fmij[k+9] = fm2[9*i+k]; - } - } - for (int i = 0; i < r1*r2*18; i++) - if (!isfinite(fm[i])) - //{ - // for (int j = 0; j < 9*r1; j++) - // fprintf(stderr, "\tfm1[%d]=%g\n", - // j, fm1[j]); - // for (int j = 0; j < 9*r2; j++) - // fprintf(stderr, "\tfm2[%d]=%g\n", - // j, fm2[j]); - fail("not finite fm[%d]=%g\n", i, fm[i]); - //} - return r1 * r2; -} - -static void mprod33(float *ab_out, float *a_in, float *b_in) -{ - float (*about)[3] = (void*)ab_out; - float (*a)[3] = (void*)a_in; - float (*b)[3] = (void*)b_in; - float ab[3][3] = {{0}}; - for (int j = 0; j < 3; j++) - for (int i = 0; i < 3; i++) - for (int k = 0; k < 3; k++) - { - if (!isfinite(a[i][k])) - fail("mprod A not finite %d %d", i, k); - if (!isfinite(b[k][j])) - fail("mprod B not finite %d %d", k, j); - ab[i][j] += a[i][k] * b[k][j]; - } - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - about[i][j] = ab[i][j]; -} - -int find_fundamental_matrix_by_ransac( - bool *out_mask, float out_fm[9], - float *pairs, int npairs, - int ntrials, float max_err) -{ - // compute input statistics - double pmean[4] = {0, 0, 0, 0}, pdev[4] = {0, 0, 0, 0}; - for (int i = 0; i < npairs; i++) - for (int j = 0; j < 4; j++) - pmean[j] += (pairs[4*i+j])/npairs; - for (int i = 0; i < npairs; i++) - for (int j = 0; j < 4; j++) - pdev[j] += (fabs(pairs[4*i+j]-pmean[j]))/npairs; - float PDev = (pdev[0]+pdev[1]+pdev[2]+pdev[3])/4; - float pDev[4] = {PDev, PDev, PDev, PDev}; - - fprintf(stderr, "pmean = %g %g %g %g\n", - pmean[0], pmean[1], pmean[2], pmean[3]); - fprintf(stderr, "pdev = %g %g %g %g\n", - pdev[0], pdev[1], pdev[2], pdev[3]); - fprintf(stderr, "normalization factor = %g\n", PDev); - - // normalize input - float *pairsn = xmalloc(4*npairs*sizeof*pairsn); - for (int i = 0; i < npairs; i++) - for (int j = 0; j < 4; j++) - pairsn[4*i+j] = (pairs[4*i+j]-pmean[j])/PDev; - - //for (int i = 0; i < npairs; i++) - //for (int j = 0; j < 4; j++) - // fprintf(stderr, "%g %g %g %g\n", - // pairsn[4*i+0], pairsn[4*i+1], - // pairsn[4*i+2], pairsn[4*i+3]); - float max_err_n = max_err / PDev; - - // set up algorithm context - int datadim = 4; - int modeldim = 9; - int nfit = 7; - ransac_error_evaluation_function *f_err = epipolar_error; - ransac_model_generating_function *f_gen = seven_point_algorithm; - ransac_model_accepting_function *f_acc = NULL; - - // run algorithm on normalized data - float nfm[9]; - int n_inliers = ransac(out_mask, nfm, - pairsn, datadim, npairs, modeldim, - f_err, f_gen, nfit, ntrials, nfit+1, max_err_n, - f_acc, NULL); - - // un-normalize result - float Atrans[9] = {1/pDev[0], 0, 0, - 0, 1/pDev[1], 0, - -pmean[0]/pDev[0], -pmean[1]/pDev[1], 1}; - float B[9] = {1/pDev[2], 0, -pmean[2]/pDev[2], - 0, 1/pDev[3], -pmean[3]/pDev[3], - 0, 0, 1}; - float tmp[9]; - mprod33(tmp, nfm, B); - mprod33(out_fm, Atrans, tmp); - - //// print matrices - //fprintf(stderr, "Atrans= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // Atrans[0],Atrans[1],Atrans[2],Atrans[3],Atrans[4],Atrans[5],Atrans[6],Atrans[7],Atrans[8]); - //fprintf(stderr, "nfm= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // nfm[0],nfm[1],nfm[2],nfm[3],nfm[4],nfm[5],nfm[6],nfm[7],nfm[8]); - //fprintf(stderr, "B= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // B[0],B[1],B[2],B[3],B[4],B[5],B[6],B[7],B[8]); - //fprintf(stderr, "out_fm= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // out_fm[0],out_fm[1],out_fm[2],out_fm[3],out_fm[4],out_fm[5],out_fm[6],out_fm[7],out_fm[8]); - - //// for each pair, display its normalized and un-normalized errors - //for (int i = 0; i < npairs; i++) - //{ - // float en = f_err(nfm, pairsn+4*i, NULL); - // float en2 = epipolar_algebraic_error(nfm, pairsn+4*i, NULL); - // float eu = f_err(out_fm, pairs+4*i, NULL); - // fprintf(stderr, "en,eu = %g\t%g\t{%g}\t%g\n", en, eu,eu/en,en2); - //} - - - free(pairsn); - return n_inliers; -} - -// finds a pair of fundamental matrices -int find_fundamental_pair_by_ransac(bool *out_mask, float out_fm[18], - float *trips, int ntrips, - int ntrials, float max_err) -{ - // compute input statistics - double tmean[6] = {0, 0, 0, 0, 0, 0}, tdev[6] = {0, 0, 0, 0, 0, 0}; - for (int i = 0; i < ntrips; i++) - for (int j = 0; j < 6; j++) - tmean[j] += (trips[6*i+j])/ntrips; - for (int i = 0; i < ntrips; i++) - for (int j = 0; j < 6; j++) - tdev[j] += (fabs(trips[6*i+j]-tmean[j]))/ntrips; - float TDev = (tdev[0]+tdev[1]+tdev[2]+tdev[3]+tdev[4]+tdev[5])/6; - float tDev[6] = {TDev, TDev, TDev, TDev, TDev, TDev}; - - fprintf(stderr, "normalization factor = %g\n", TDev); - - // normalize input - float *tripsn = xmalloc(6*ntrips*sizeof*tripsn); - for (int i = 0; i < ntrips; i++) - for (int j = 0; j < 6; j++) - tripsn[6*i+j] = (trips[6*i+j]-tmean[j])/TDev; - - //for (int i = 0; i < ntrips; i++) - //for (int j = 0; j < 6; j++) - // fprintf(stderr, "%g %g %g %g %g %g\n", - // pairsn[6*i+0], pairsn[6*i+1], - // pairsn[6*i+2], pairsn[6*i+3], - // pairsn[6*i+4], pairsn[6*i+5]); - float max_err_n = max_err / TDev; - - // set up algorithm context - int datadim = 6; - int modeldim = 18; - int nfit = 7; - ransac_error_evaluation_function *f_err = epipolar_error_triplet; - ransac_model_generating_function *f_gen = two_seven_point_algorithms; - ransac_model_accepting_function *f_acc = NULL; - - // run algorithm on normalized data - float nfm[modeldim]; - int n_inliers = ransac(out_mask, nfm, - tripsn, datadim, ntrips, modeldim, - f_err, f_gen, nfit, ntrials, nfit+1, max_err_n, - f_acc, NULL); - - // un-normalize result - float Atrans[9] = {1/tDev[0], 0, 0, - 0, 1/tDev[1], 0, - -tmean[0]/tDev[0], -tmean[1]/tDev[1], 1}; - float B[9] = {1/tDev[2], 0, -tmean[2]/tDev[2], - 0, 1/tDev[3], -tmean[3]/tDev[3], - 0, 0, 1}; - float tmp[9]; - mprod33(tmp, nfm, B); - mprod33(out_fm, Atrans, tmp); - float Atrans2[9] = {1/tDev[0], 0, 0, - 0, 1/tDev[1], 0, - -tmean[0]/tDev[0], -tmean[1]/tDev[1], 1}; - float B2[9] = {1/tDev[4], 0, -tmean[4]/tDev[4], - 0, 1/tDev[5], -tmean[5]/tDev[5], - 0, 0, 1}; - mprod33(tmp, nfm+9, B2); - mprod33(out_fm+9, Atrans2, tmp); - - //for(int i = 0; i < modeldim; i++) - // fprintf(stderr, "model_norm[%d] = %g\n", i, out_fm[i]); - - // print matrices - //fprintf(stderr, "Atrans= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // Atrans[0],Atrans[1],Atrans[2],Atrans[3],Atrans[4],Atrans[5],Atrans[6],Atrans[7],Atrans[8]); - //fprintf(stderr, "nfm= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // nfm[0],nfm[1],nfm[2],nfm[3],nfm[4],nfm[5],nfm[6],nfm[7],nfm[8]); - //fprintf(stderr, "B= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // B[0],B[1],B[2],B[3],B[4],B[5],B[6],B[7],B[8]); - //fprintf(stderr, "out_fm= [%g %g %g ; %g %g %g ; %g %g %g]\n", - // out_fm[0],out_fm[1],out_fm[2],out_fm[3],out_fm[4],out_fm[5],out_fm[6],out_fm[7],out_fm[8]); - - // for each pair, display its normalized and un-normalized errors - //for (int i = 0; i < npairs; i++) - //{ - // float en = f_err(nfm, pairsn+4*i, NULL); - // float en2 = epipolar_algebraic_error(nfm, pairsn+4*i, NULL); - // float eu = f_err(out_fm, pairs+4*i, NULL); - // fprintf(stderr, "en,eu = %g\t%g\t{%g}\t%g\n", en, eu,eu/en,en2); - //} - - - free(tripsn); - return n_inliers; -} - - -// ************************************ -// ************************************ -// ************************************ -// ************************************ -// -// 6. affine maps between pairs of spatial points -// datadim = 6 (coordinates of each pair of points) -// modeldim = 12 (coefficients of the affine map) -// nfit = 4 (number of pairs that determine an affine map) - -// utility function -static void affine3d_eval(float y[3], float A[12], float x[3]) -{ - y[0] = A[0]*x[0] + A[1]*x[1] + A[2]*x[2] + A[3]; - y[1] = A[4]*x[0] + A[5]*x[1] + A[6]*x[2] + A[7]; - y[2] = A[8]*x[0] + A[9]*x[1] + A[10]*x[2] + A[11]; -} - -// instance of "ransac_error_evaluation_function" -static float affine3d_match_error(float *aff, float *pair, void *usr) -{ - float x[3] = {pair[0], pair[1], pair[2]}; - float y[3] = {pair[3], pair[4], pair[5]}; - float Ax[3]; affine3d_eval(Ax, aff, x); - float d[3] = {Ax[0] - y[0], Ax[1] - y[1], Ax[2] - y[2]}; - float r = fnorm(d, 3); - return r; -} - -// auxiliary function -static void affine3d_map_from_canonical_basis(float A[12], - float o[3], float x[3], float y[3], float z[3]) -{ - float r[12] = { - x[0]-o[0], y[0]-o[0], z[0]-o[0], o[0], - x[1]-o[1], y[1]-o[1], z[1]-o[1], o[1], - x[2]-o[2], y[2]-o[2], z[2]-o[2], o[2]}; - for (int i = 0; i < 12; i++) - A[i] = r[i]; - if (0) { // check correctness - float X[3][3] = { - {x[0], x[1], x[2]}, - {y[0], y[1], y[2]}, - {z[0], z[1], z[2]} - }; - for (int i = 0; i < 3; i++) - { - float oi[3] = {0}; oi[i] = 1; - float xi[3] = {X[i][0], X[i][1], X[i][2]}; - float aoi[3]; - affine3d_eval(aoi, A, oi); - float ddd[3] = {aoi[0]-xi[0],aoi[1]-xi[1],aoi[2]-xi[2]}; - float eee = fnorm(ddd, 3); - fprintf(stderr, "oeee %d = %g\n", i, eee); - } - } -} - -// auxiliary function -static void affine3d_map_compose(float AB[12], float A[12], float B[12]) -{ - float a[4][4] = { - {A[0], A[1], A[2], A[3]}, - {A[4], A[5], A[6], A[7]}, - {A[8], A[9], A[10], A[11]}, - {0,0,0,1}}; - float b[4][4] = { - {B[0], B[1], B[2], B[3]}, - {B[4], B[5], B[6], B[7]}, - {B[8], B[9], B[10], B[11]}, - {0,0,0,1}}; - float ab[4][4]; - MATRIX_PRODUCT_4X4(ab, a, b); - float r[12] = { - ab[0][0], ab[0][1], ab[0][2], ab[0][3], - ab[1][0], ab[1][1], ab[1][2], ab[1][3], - ab[2][0], ab[2][1], ab[2][2], ab[2][3] - }; - for (int i = 0; i < 12; i++) - AB[i] = r[i]; -} - -// auxiliary function -static float affine3d_map_invert(float iA[12], float A[12]) -{ - float a[3][3] = { - {A[0], A[1], A[2]}, - {A[4], A[5], A[6]}, - {A[8], A[9], A[10]} - }; - float t[3] = {A[3], A[7], A[11]}; - float ia[3][3], det; - INVERT_3X3(ia, det, a); - float iat[3]; - MAT_DOT_VEC_3X3(iat, ia, t); - float r[12] = { - ia[0][0], ia[0][1], ia[0][2], -iat[0], - ia[1][0], ia[1][1], ia[1][2], -iat[1], - ia[2][0], ia[2][1], ia[2][2], -iat[2] - }; - for (int i = 0; i < 12; i++) - iA[i] = r[i]; - return det; -} - -// instance of "ransac_model_generating_function" -static int affine3d_map_from_four_pairs(float *aff, float *pairs, void *usr) -{ - float x[4][3] = { - {pairs[0], pairs[1], pairs[2]}, - {pairs[6], pairs[7], pairs[8]}, - {pairs[12], pairs[13], pairs[14]}, - {pairs[18], pairs[19], pairs[20]} - }; - float y[4][3] = { - {pairs[3], pairs[4], pairs[5]}, - {pairs[9], pairs[10], pairs[11]}, - {pairs[15], pairs[16], pairs[17]}, - {pairs[21], pairs[22], pairs[23]} - }; - float A0X[12], A0Y[12], AX0[12], AXY[12];; - affine3d_map_from_canonical_basis(A0X, x[0], x[1], x[2], x[3]); - affine3d_map_from_canonical_basis(A0Y, y[0], y[1], y[2], y[3]); - float det = affine3d_map_invert(AX0, A0X); - affine3d_map_compose(AXY, A0Y, AX0); - for (int i = 0; i < 12; i++) - aff[i] = AXY[i]; - int r = fabs(det) > 0.0001 ? 1 : 0; - - if (0) { // check correctness - for (int i = 0; i < 4; i++) - { - float xi[3] = {x[i][0], x[i][1], x[i][2]}; - float yi[3] = {y[i][0], y[i][1], y[i][2]}; - float axi[3]; - affine3d_eval(axi, aff, xi); - float ddd[3] = {axi[0]-yi[0],axi[1]-yi[1],axi[2]-yi[2]}; - float eee = fnorm(ddd, 3); - fprintf(stderr, "eee %d = %g\n", i, eee); - } - } - - if (0) { // verbose computation - fprintf(stderr, "aff3d four pairs:"); - for (int i = 0; i < 24; i++) - fprintf(stderr, " %g", pairs[i]); - fprintf(stderr, "\naff3d model:"); - for (int i = 0; i < 12; i++) - fprintf(stderr, " %g", aff[i]); - fprintf(stderr, "\n\n"); - } - - return r; -} - diff --git a/makefile b/makefile index fd746fdf..735eb73a 100644 --- a/makefile +++ b/makefile @@ -122,7 +122,7 @@ PROGRAMS = $(addprefix $(BINDIR)/,$(SRC)) SRC = $(SRCIIO) $(SRCKKK) SRCIIO = downsa backflow synflow imprintf qauto morsi\ morphoop cldmask remove_small_cc plambda homwarp pview -SRCKKK = disp_to_h colormesh disp2ply multidisp2ply bin2asc ransac plyflatten plyextrema +SRCKKK = disp_to_h colormesh disp2ply multidisp2ply bin2asc plyflatten plyextrema imscript: $(BINDIR) $(PROGRAMS) @@ -138,10 +138,6 @@ $(SRCDIR)/rpc.o: c/rpc.c c/xfopen.c $(BINDIR)/bin2asc: c/bin2asc.c $(CC) $(CFLAGS) $^ -o $@ -$(BINDIR)/ransac: c/ransac.c c/fail.c c/xmalloc.c c/xfopen.c c/homographies.c\ - c/ransac_cases.c c/parsenumbers.c c/random.c - $(CC) $(CFLAGS) $< -lm -o $@ - $(BINDIR)/disp_to_h: $(SRCDIR)/iio.o $(SRCDIR)/rpc.o c/disp_to_h.c c/vvector.h c/rpc.h c/read_matrix.c $(CC) $(CFLAGS) c/iio.o $(SRCDIR)/rpc.o c/disp_to_h.c $(IIOLIBS) -o $@ diff --git a/s2plib/sift.py b/s2plib/sift.py index e29aa1e7..ceb15da9 100644 --- a/s2plib/sift.py +++ b/s2plib/sift.py @@ -43,8 +43,7 @@ def image_keypoints(im, x, y, w, h, max_nb=None, extra_params=''): return keyfile -def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, - model=None, epipolar_threshold=10): +def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, epipolar_threshold=10): """ Find matches among two lists of sift keypoints. @@ -60,9 +59,6 @@ def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, (ie ratio between distance to nearest and distance to second nearest), the commonly used value for the threshold is 0.6. F (optional): affine fundamental matrix - model (optional, default is None): model imposed by RANSAC when - searching the set of inliers. If None all matches are considered as - inliers. epipolar_threshold (optional, default is 10): maximum distance allowed for a point to the epipolar line of its match. @@ -71,7 +67,8 @@ def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, """ # compute matches mfile = common.tmpfile('.txt') - cmd = "matching %s %s -o %s --sift-threshold %f" % (k1, k2, mfile, sift_thresh) + cmd = "matching %s %s -o %s --sift-threshold %f" % ( + k1, k2, mfile, sift_thresh) if method == 'absolute': cmd += " --absolute" if F is not None: @@ -81,18 +78,6 @@ def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, cmd += " --epipolar-threshold {}".format(epipolar_threshold) common.run(cmd) - matches = np.loadtxt(mfile) - if matches.ndim == 2: # filter outliers with ransac - if model == 'fundamental' and len(matches) >= 7: - common.run("ransac fmn 1000 .3 7 %s < %s" % (mfile, mfile)) - elif model == 'homography' and len(matches) >= 4: - common.run("ransac hom 1000 1 4 /dev/null /dev/null %s < %s" % (mfile, - mfile)) - elif model == 'hom_fund' and len(matches) >= 7: - common.run("ransac hom 1000 2 4 /dev/null /dev/null %s < %s" % (mfile, - mfile)) - common.run("ransac fmn 1000 .2 7 %s < %s" % (mfile, mfile)) - if os.stat(mfile).st_size > 0: # return numpy array of matches return np.loadtxt(mfile) @@ -128,11 +113,12 @@ def matches_on_rpc_roi(im1, im2, rpc1, rpc2, x, y, w, h): # if less than 10 matches, lower thresh_dog. An alternative would be ASIFT thresh_dog = 0.0133 for i in range(2): - p1 = image_keypoints(im1, x, y, w, h, extra_params='--thresh-dog %f' % thresh_dog) - p2 = image_keypoints(im2, x2, y2, w2, h2, extra_params='--thresh-dog %f' % thresh_dog) + p1 = image_keypoints( + im1, x, y, w, h, extra_params='--thresh-dog %f' % thresh_dog) + p2 = image_keypoints(im2, x2, y2, w2, h2, + extra_params='--thresh-dog %f' % thresh_dog) matches = keypoints_match(p1, p2, method, cfg['sift_match_thresh'], - F, model='fundamental', - epipolar_threshold=cfg['max_pointing_error']) + F, epipolar_threshold=cfg['max_pointing_error']) if matches is not None and matches.ndim == 2 and matches.shape[0] > 10: break thresh_dog /= 2.0 From 39fd59b10dac89c097a15cb4bbe43c5f1f147c9f Mon Sep 17 00:00:00 2001 From: Carlo de Franchis Date: Sat, 23 Mar 2019 19:12:40 +0100 Subject: [PATCH 2/3] [refactor] replace ransac binary with ransac Python module --- s2p/sift.py | 23 ++++++++++++++++++----- setup.py | 1 + 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/s2p/sift.py b/s2p/sift.py index e7a7c4d8..a5836dc7 100644 --- a/s2p/sift.py +++ b/s2p/sift.py @@ -11,6 +11,7 @@ import numpy as np import rasterio as rio from numpy.ctypeslib import ndpointer +import ransac from s2p import common from s2p import rpc_utils @@ -130,7 +131,8 @@ def image_keypoints(im, x, y, w, h, max_nb=None, thresh_dog=0.0133, nb_octaves=8 return keyfile -def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, epipolar_threshold=10): +def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, + epipolar_threshold=10, model=None, ransac_max_err=0.3): """ Find matches among two lists of sift keypoints. @@ -148,6 +150,11 @@ def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, epipolar F (optional): affine fundamental matrix epipolar_threshold (optional, default is 10): maximum distance allowed for a point to the epipolar line of its match. + model (optional, default is None): model imposed by RANSAC when + searching the set of inliers. If None all matches are considered as + inliers. + ransac_max_err (float): maximum allowed epipolar error for + RANSAC inliers. Optional, default is 0.3. Returns: if any, a numpy 2D array containing the list of inliers matches. @@ -165,8 +172,13 @@ def keypoints_match(k1, k2, method='relative', sift_thresh=0.6, F=None, epipolar cmd += " --epipolar-threshold {}".format(epipolar_threshold) common.run(cmd) - if os.stat(mfile).st_size > 0: # return numpy array of matches - return np.loadtxt(mfile) + matches = np.atleast_2d(np.loadtxt(mfile)) + if model == 'fundamental' and len(matches) >= 7: + inliers = ransac.find_fundamental_matrix(matches, ntrials=1000, + max_err=ransac_max_err)[0] + matches = matches[inliers] + + return matches def matches_on_rpc_roi(im1, im2, rpc1, rpc2, x, y, w, h): @@ -202,8 +214,9 @@ def matches_on_rpc_roi(im1, im2, rpc1, rpc2, x, y, w, h): for i in range(2): p1 = image_keypoints(im1, x, y, w, h, thresh_dog=thresh_dog) p2 = image_keypoints(im2, x2, y2, w2, h2, thresh_dog=thresh_dog) - matches = keypoints_match(p1, p2, method, cfg['sift_match_thresh'], - F, epipolar_threshold=cfg['max_pointing_error']) + matches = keypoints_match(p1, p2, method, cfg['sift_match_thresh'], F, + epipolar_threshold=cfg['max_pointing_error'], + model='fundamental') if matches is not None and matches.ndim == 2 and matches.shape[0] > 10: break thresh_dog /= 2.0 diff --git a/setup.py b/setup.py index 276e3200..78ebdb03 100644 --- a/setup.py +++ b/setup.py @@ -34,6 +34,7 @@ def run(self): 'utm', 'pyproj', 'bs4', + 'ransac', 'requests'] setup(name="s2p", From 4e870cd7dee1ee838d5d58f312146d179081228f Mon Sep 17 00:00:00 2001 From: Carlo de Franchis Date: Sat, 23 Mar 2019 23:29:19 +0100 Subject: [PATCH 3/3] [refactor] remove unused moistiv_epipolar.c --- c/moistiv_epipolar.c | 715 ------------------------------------------- 1 file changed, 715 deletions(-) delete mode 100644 c/moistiv_epipolar.c diff --git a/c/moistiv_epipolar.c b/c/moistiv_epipolar.c deleted file mode 100644 index 90c45de8..00000000 --- a/c/moistiv_epipolar.c +++ /dev/null @@ -1,715 +0,0 @@ -// somewhat de-uglyfied code from moistiv - -#include - -#include "fail.c" - -#ifndef M_PI -#define M_PI 3.1416 -#endif - - -/*--------------------------- MegaWave2 module -----------------------------*/ -/* mwcommand - name = {stereomatch}; - author = {"Lionel Moisan"}; - version = {"1.1"}; - function = {"Detect and rate the best stereo correspondence between 2D point matches"}; - usage = { - 'v'->verb "verbose", - 's'->stop "stop as soon as the first meaningful correspondence is found", - 't':[t=10000]->t "maximum number of ransac trials (default: 10000)", - 'n'->n_flag "in order NOT to reinitialize the random seed", - 'm':[mode=3]->mode "mode: 0=deterministic 1=ransac 2=optimized ransac (ORSA) 3=automatic", - u1->u1 "input: image 1 (used for dimensions)", - p1->p1 "input: 1st Flist of 2D points", - p2->p2 "input: 2nd Flist of 2D points", - f<-f "output: fundamental matrix (3x3 Flist)", - index<-index "output: indexes of matching pairs (1-Flist)", - lnfa<-stereomatch "output: meaningfulness of the matching (-log10(nfa))" -}; -*/ - -/*---------------------------------------------------------------------- - v1.0 (10/2007): initial version from private file stereomatch3.c (LM) - v1.1 (11/2008): useless lines removed -----------------------------------------------------------------------*/ - -//#include -//#include -//#include -//#include -//#include "mw.h" - - -/*-------------------- GENERAL PURPOSE ROUTINES --------------------*/ - -/* routines for vectors and matrices */ - -static -float *vector(int nl, int nh) -{ - float *v; - - v=(float *)malloc((unsigned) (nh-nl+1)*sizeof(float)); - if (!v) fail("allocation failure in vector()"); - return v-nl; -} - -static -float **matrix(int nrl, int nrh, int ncl, int nch) -{ - int i; - float **m; - - m=(float **) malloc((unsigned) (nrh-nrl+1)*sizeof(float*)); - if (!m) fail("allocation failure 1 in matrix()"); - m -= nrl; - for(i=nrl;i<=nrh;i++) { - m[i]=(float *) malloc((unsigned) (nch-ncl+1)*sizeof(float)); - if (!m[i]) fail("allocation failure 2 in matrix()"); - m[i] -= ncl; - } - return m; -} - -static -void free_vector(float *v, int nl, int nh) -{ - free((char*) (v+nl)); -} - -static -void free_matrix(float **m, int nrl, int nrh, int ncl, int nch) -{ - int i; - - for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl)); - free((char*) (m+nrl)); -} - -/* Singular Value Decomposition routine */ - -//static float at,bt,ct; -#define PYTHAG(a,b) hypot(a,b) -//#define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? -//(ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0)) - -static float maxarg1,maxarg2; -#define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\ - (maxarg1) : (maxarg2)) -#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) - -static -void svdcmp(float **a, int m, int n, float *w, float **v) -{ - int flag,i,its,j,jj,k,l,nm; - float c,f,h,s,x,y,z; - float anorm=0.0,g=0.0,scale=0.0; - float *rv1; - - if (m=1;i--) { - if (i < n) { - if (g) { - for (j=l;j<=n;j++) - v[j][i]=(a[i][j]/a[i][l])/g; - for (j=l;j<=n;j++) { - for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j]; - for (k=l;k<=n;k++) v[k][j] += s*v[k][i]; - } - } - for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0; - } - v[i][i]=1.0; - g=rv1[i]; - l=i; - } - for (i=n;i>=1;i--) { - l=i+1; - g=w[i]; - if (i < n) - for (j=l;j<=n;j++) a[i][j]=0.0; - if (g) { - g=1.0/g; - if (i != n) { - for (j=l;j<=n;j++) { - for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j]; - f=(s/a[i][i])*g; - for (k=i;k<=m;k++) a[k][j] += f*a[k][i]; - } - } - for (j=i;j<=m;j++) a[j][i] *= g; - } else { - for (j=i;j<=m;j++) a[j][i]=0.0; - } - ++a[i][i]; - } - for (k=n;k>=1;k--) { - for (its=1;its<=30;its++) { - flag=1; - for (l=k;l>=1;l--) { - nm=l-1; - if (fabs(rv1[l])+anorm == anorm) { - flag=0; - break; - } - if (fabs(w[nm])+anorm == anorm) break; - } - if (flag) { - c=0.0; - s=1.0; - for (i=l;i<=k;i++) { - f=s*rv1[i]; - if (fabs(f)+anorm != anorm) { - g=w[i]; - h=PYTHAG(f,g); - w[i]=h; - h=1.0/h; - c=g*h; - s=(-f*h); - for (j=1;j<=m;j++) { - y=a[j][nm]; - z=a[j][i]; - a[j][nm]=y*c+z*s; - a[j][i]=z*c-y*s; - } - } - } - } - z=w[k]; - if (l == k) { - if (z < 0.0) { - w[k] = -z; - for (j=1;j<=n;j++) v[j][k]=(-v[j][k]); - } - break; - } - if (its == 30) fail("No convergence in 30 SVDCMP iterations"); - x=w[l]; - nm=k-1; - y=w[nm]; - g=rv1[nm]; - h=rv1[k]; - f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); - g=PYTHAG(f,1.0); - f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x; - c=s=1.0; - for (j=l;j<=nm;j++) { - i=j+1; - g=rv1[i]; - y=w[i]; - h=s*g; - g=c*g; - z=PYTHAG(f,h); - rv1[j]=z; - c=f/z; - s=h/z; - f=x*c+g*s; - g=g*c-x*s; - h=y*s; - y=y*c; - for (jj=1;jj<=n;jj++) { - x=v[jj][j]; - z=v[jj][i]; - v[jj][j]=x*c+z*s; - v[jj][i]=z*c-x*s; - } - z=PYTHAG(f,h); - w[j]=z; - if (z) { - z=1.0/z; - c=f*z; - s=h*z; - } - f=(c*g)+(s*y); - x=(c*y)-(s*g); - for (jj=1;jj<=m;jj++) { - y=a[jj][j]; - z=a[jj][i]; - a[jj][j]=y*c+z*s; - a[jj][i]=z*c-y*s; - } - } - rv1[l]=0.0; - rv1[k]=f; - w[k]=x; - } - } - free_vector(rv1,1,n); -} - -#undef SIGN -#undef MAX -#undef PYTHAG - - -/* Compute the real roots of a third order polynomial */ -/* returns 1 or 3, the number of roots found */ - -static -int FindCubicRoots(float coeff[4], float x[3]) -{ - float a1 = coeff[2] / coeff[3]; - float a2 = coeff[1] / coeff[3]; - float a3 = coeff[0] / coeff[3]; - - double Q = (a1 * a1 - 3 * a2) / 9; - double R = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) / 54; - double Qcubed = Q * Q * Q; - double d = Qcubed - R * R; - - /* Three real roots */ - if (d >= 0) { - double theta = acos(R / sqrt(Qcubed)); - double sqrtQ = sqrt(Q); - x[0] = -2 * sqrtQ * cos( theta / 3) - a1 / 3; - x[1] = -2 * sqrtQ * cos((theta + 2 * M_PI) / 3) - a1 / 3; - x[2] = -2 * sqrtQ * cos((theta + 4 * M_PI) / 3) - a1 / 3; - return (3); - } - - /* One real root */ - else { - double e = pow(sqrt(-d) + fabs(R), 1. / 3.); - if (R > 0) - e = -e; - x[0] = (e + Q / e) - a1 / 3.; - return (1); - } -} - - -///* logarithm (base 10) of binomial coefficient */ -//float logcombi(k,n) -// int k,n; -//{ -// double r; -// int i; -// -// if (k>=n || k<=0) return(0.); -// if (n-k>3)%(n-i); -// for (j=0;j=k[j];j++) r++; -// j0 = j; -// for (j=i;j>j0;j--) k[j]=k[j-1]; -// k[j0]=r; -// } -//} - -/*-------------------- END OF GENERAL PURPOSE ROUTINES --------------------*/ - - -///* float comparison for qsort() */ -//static int compf(i,j) -// void *i,*j; -//{ -// float a,b; -// -// a = *((float *)i); -// b = *((float *)j); -// return(ab?1:0)); -//} -// -///* find the increasing sequence of squared distances to epipolar lines */ -///* e[n*2] = distances, e[n*2+1] = indexes (to cast into an int) */ -// -//void matcherrorn(F,p1,p2,e) -// float **F; -// Flist p1,p2; -// float *e; -//{ -// int i; -// float x,y,a,b,c,d; -// -// for (i=0;isize;i++) { -// x = p1->values[i*2]; -// y = p1->values[i*2+1]; -// a = F[1][1]*x+F[1][2]*y+F[1][3]; -// b = F[2][1]*x+F[2][2]*y+F[2][3]; -// c = F[3][1]*x+F[3][2]*y+F[3][3]; -// d = (a*p2->values[i*2]+b*p2->values[i*2+1]+c); -// e[i*2] = (d*d)/(a*a+b*b); -// e[i*2+1] = (float)i; -// } -// qsort(e,p1->size,2*sizeof(float),compf); -//} - - -/*---------- compute the epipolar geometry associated to 7 pairs ----------*/ -/* */ -/* INPUT: the points are (m1[k[i]*2],m1[k[i]*2+1]), m2... 0size!=p2->size || p1->size<7) -// mwerror(FATAL,1,"Inconsistent sizes. "); -// n = p1->size; -// -// /* tabulate logcombi */ -// loge0 = (float)log10(3.*(double)(n-7)); -// logcn = makelogcombi_n(n); -// logc7 = makelogcombi_k(7,n); -// -// /* choose mode */ -// if (*mode==3) { -// if (logcn[7]<=(float)log10((double)(*t))) -// *mode=0; -// else *mode=2; -// } -// if (verb) -// switch(*mode) { -// case 0: -// i = (int)(0.5+pow(10.,logc7[n])); -// printf("I will use deterministic mode (systematic search).\n"); -// printf("I have to test %d bases\n",i); -// break; -// case 1: -// printf("I will use pure stochastic mode with no optimization.\n"); -// break; -// case 2: -// printf("I will use optimized stochastic mode (ORSA).\n"); -// } -// -// /* normalize coordinates */ -// nx = (float)u1->ncol; -// ny = (float)u1->nrow; -// norm = 1./(float)sqrt((double)(nx*ny)); -// logalpha0 = (float)(log10(2.)+0.5*log10((double)((nx*nx+ny*ny)*norm*norm))); -// for (i=0;ivalues[i*2 ] = (p1->values[i*2 ]-0.5*nx)*norm; -// p1->values[i*2+1] = (p1->values[i*2+1]-0.5*ny)*norm; -// p2->values[i*2 ] = (p2->values[i*2 ]-0.5*nx)*norm; -// p2->values[i*2+1] = (p2->values[i*2+1]-0.5*ny)*norm; -// } -// -// /* allocate and initialize memory */ -// c = matrix(1,9,1,9); -// w = vector(1,9); -// v = matrix(1,9,1,9); -// F = matrix(1,3,1,3); -// F1 = matrix(1,3,1,3); -// F2 = matrix(1,3,1,3); -// if(f) { -// mw_change_flist(f,3,3,3); -// mw_clear_flist(f,0.); -// } -// delete_index = (index?0:1); -// index = mw_change_flist(index,n,0,1); -// e = (float *)malloc(2*n*sizeof(float)); -// id = (int *)malloc(n*sizeof(int)); -// for (i=0;ivalues,p2->values,idk,z,F1,F2); -// -// /* loop on roots */ -// for (;m--;) { -// -// for (i=1;i<=3;i++) -// for (j=1;j<=3;j++) -// F[i][j] = F1[i][j]+z[m]*F2[i][j]; -// -// /* sort errors */ -// matcherrorn(F,p1,p2,e); -// -// /* find most meaningful subset */ -// minepscur = minlogalphacur = 10000.; -// for (i=7;ivalues[(l-1)*f->dim+j-1] = F[l][j]; -// for (i=0;i<=minicur;i++) -// index->values[i] = e[i*2+1]; -// index->size = minicur+1; -// } else better=0; -// -// if (*mode==2 && ((better && minepsall<0.) || -// (niter==maxniter && !optimization))) { -// if (!optimization) maxniter = niter + (*t)/10; -// optimization = 1; -// /* final optimization */ -// if (verb) { -// printf(" nfa=%f size=%d (niter=%d)\n",minepsall,miniall+1,niter); -// printf("optimization...\n"); -// } -// nid = miniall+1; -// for (j=0;j<=miniall;j++) -// id[j] = (int)index->values[j]; -// } -// -// } -// -// /* prepare next list of points */ -// if (*mode==0) -// for (i0=6;i0>=0 && k[i0]==k[i0+1]-1;i0--); -// -// if (stop && minepsall<0.) cont=0; -// else if (*mode==0) cont=(i0>=0?1:0); -// else cont=(niter