593 lines
16 KiB
C++
593 lines
16 KiB
C++
|
//
|
|||
|
|
// C++ Implementation: stereomatch
|
|||
|
|
//
|
|||
|
|
// Description: eliminate the false matches with epipolar geometry constraint.
|
|||
|
|
// See http://www.math-info.univ-paris5.fr/~moisan/epipolar/
|
|||
|
|
//
|
|||
|
|
// Copyright (c) 2007 Lionel Moisan <Lionel.Moisan@parisdescartes.fr>
|
|||
|
|
// Changelog : 2011 Use Eigen SVD <Pierre Moulon>
|
|||
|
|
//
|
|||
|
|
// Copyright: See COPYING file that comes with this distribution
|
|||
|
|
//
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
#include <assert.h>
|
|||
|
|
#include <stdio.h>
|
|||
|
|
#include <stdlib.h>
|
|||
|
|
#include <math.h>
|
|||
|
|
#include <time.h>
|
|||
|
|
#include "orsa.h"
|
|||
|
|
#include <third_party/Eigen/Cholesky>
|
|||
|
|
#include <third_party/Eigen/Core>
|
|||
|
|
#include <third_party/Eigen/Eigenvalues>
|
|||
|
|
#include <third_party/Eigen/LU>
|
|||
|
|
#include <third_party/Eigen/QR>
|
|||
|
|
#include <third_party/Eigen/SVD>
|
|||
|
|
|
|||
|
|
#ifndef M_PI
|
|||
|
|
#define M_PI 3.14159265358979323846
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/*-------------------- GENERAL PURPOSE ROUTINES --------------------*/
|
|||
|
|
|
|||
|
|
/* routines for vectors and matrices */
|
|||
|
|
|
|||
|
|
float *vector(int nl, int nh)
|
|||
|
|
{
|
|||
|
|
float *v;
|
|||
|
|
|
|||
|
|
v=(float *)malloc((unsigned) (nh-nl+1)*sizeof(float));
|
|||
|
|
if (!v) {
|
|||
|
|
// mwerror(FATAL,1,"allocation failure in vector()");
|
|||
|
|
fprintf(stderr, "allocation failure in vector()\n");
|
|||
|
|
exit(EXIT_FAILURE); /* indicate failure.*/
|
|||
|
|
}
|
|||
|
|
return v-nl;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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) {
|
|||
|
|
// mwerror(FATAL,1,"allocation failure 1 in matrix()");
|
|||
|
|
fprintf(stderr, "allocation failure 1 in matrix()\n");
|
|||
|
|
exit(EXIT_FAILURE); /* indicate failure.*/
|
|||
|
|
}
|
|||
|
|
m -= nrl;
|
|||
|
|
for(i=nrl;i<=nrh;i++) {
|
|||
|
|
m[i]=(float *) malloc((unsigned) (nch-ncl+1)*sizeof(float));
|
|||
|
|
if (!m[i]) {
|
|||
|
|
// mwerror(FATAL,1,"allocation failure 2 in matrix()");
|
|||
|
|
fprintf(stderr, "allocation failure 2 in matrix()\n");
|
|||
|
|
exit(EXIT_FAILURE); /* indicate failure.*/
|
|||
|
|
}
|
|||
|
|
m[i] -= ncl;
|
|||
|
|
}
|
|||
|
|
return m;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void free_vector(float *v, int nl, int nh)
|
|||
|
|
{
|
|||
|
|
free((char*) (v+nl));
|
|||
|
|
|
|||
|
|
nh = nh; // to remove the warning "unused parameter ‘nh’"
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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));
|
|||
|
|
|
|||
|
|
nch = nch; // to remove the warning "unused parameter ‘nh’"
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* Compute the real roots of a third order polynomial */
|
|||
|
|
/* returns 1 or 3, the number of roots found */
|
|||
|
|
|
|||
|
|
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(int k, int n)
|
|||
|
|
{
|
|||
|
|
double r;
|
|||
|
|
int i;
|
|||
|
|
|
|||
|
|
if (k>=n || k<=0) return(0.);
|
|||
|
|
if (n-k<k) k=n-k;
|
|||
|
|
r = 0.;
|
|||
|
|
for (i=1;i<=k;i++)
|
|||
|
|
r += log10((double)(n-i+1))-log10((double)i);
|
|||
|
|
|
|||
|
|
return((float)r);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* tabulate logcombi(.,n) */
|
|||
|
|
float *makelogcombi_n(int n)
|
|||
|
|
{
|
|||
|
|
float *l;
|
|||
|
|
int k;
|
|||
|
|
|
|||
|
|
l = (float *)malloc((n+1)*sizeof(float));
|
|||
|
|
for (k=0;k<=n;k++) l[k]=logcombi(k,n);
|
|||
|
|
|
|||
|
|
return(l);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* tabulate logcombi(k,.) */
|
|||
|
|
float *makelogcombi_k(int k, int nmax)
|
|||
|
|
{
|
|||
|
|
float *l;
|
|||
|
|
int n;
|
|||
|
|
|
|||
|
|
l = (float *)malloc((nmax+1)*sizeof(float));
|
|||
|
|
for (n=0;n<=nmax;n++) l[n]=logcombi(k,n);
|
|||
|
|
|
|||
|
|
return(l);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
/* get a (sorted) random 7-uple of 0..n-1 */
|
|||
|
|
void random_p7(int *k, int n)
|
|||
|
|
{
|
|||
|
|
int i,j,j0,r;
|
|||
|
|
|
|||
|
|
for (i=0;i<7;i++) {
|
|||
|
|
r = (rand()>>3)%(n-i);
|
|||
|
|
for (j=0;j<i && r>=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() */
|
|||
|
|
//According to http://www.cplusplus.com/reference/clibrary/cstdlib/qsort/,
|
|||
|
|
//we should have: void qsort ( void * base, size_t num, size_t size, int ( * comparator ) ( const void *, const void * ) ); that means, for "qsort", the "comparator" has two constant void* type input parameters
|
|||
|
|
// static int compf(void *i, void *j)
|
|||
|
|
int compf(const void *i, const void *j)
|
|||
|
|
{
|
|||
|
|
float a,b;
|
|||
|
|
|
|||
|
|
a = *((float *)i);
|
|||
|
|
b = *((float *)j);
|
|||
|
|
return(a<b?-1:(a>b?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(float **F, Flist p1, Flist p2, float *e)
|
|||
|
|
void matcherrorn(float **F, const std::vector<float>& p1, const std::vector<float>& p2, float *e)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
double x,y,a,b,c,d; // Guoshen Yu, double precision is needed. When the two images are identical, the error under float precision is 0 => log(error)=-inf.
|
|||
|
|
|
|||
|
|
int pt_size = (p1.size())/2;
|
|||
|
|
|
|||
|
|
for (i = 0; i < pt_size; i++) {
|
|||
|
|
x = (double) p1[i*2];
|
|||
|
|
y = (double) p1[i*2+1];
|
|||
|
|
a = (double) F[1][1]*x+(double) F[1][2]*y+(double) F[1][3];
|
|||
|
|
b = (double) F[2][1]*x+(double) F[2][2]*y+(double) F[2][3];
|
|||
|
|
c = (double) F[3][1]*x+(double) F[3][2]*y+(double) F[3][3];
|
|||
|
|
d = (a*(double) p2[i*2]+b*(double) p2[i*2+1]+c);
|
|||
|
|
e[i*2] = (d*d)/(a*a+b*b);
|
|||
|
|
|
|||
|
|
e[i*2+1] = (float)i;
|
|||
|
|
}
|
|||
|
|
qsort(e, pt_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... 0<i<7 */
|
|||
|
|
/* */
|
|||
|
|
/* OUTPUT: */
|
|||
|
|
/* return the number of roots found, stored in z[] */
|
|||
|
|
/* the epipolar matrices are F1[i][j]+z[k]*F2[i][j], 1<=i,j<=3, 0<=k<m */
|
|||
|
|
|
|||
|
|
// int epipolar(float *m1, float *m2, int *k, float *z, float **F1, float **F2)
|
|||
|
|
int epipolar(std::vector<float>& m1, std::vector<float>& m2, int *k, float *z, float **F1, float **F2)
|
|||
|
|
{
|
|||
|
|
float a[4];
|
|||
|
|
int i,j,i2,i3;
|
|||
|
|
|
|||
|
|
typedef Eigen::MatrixXf Mat;
|
|||
|
|
Mat c(7,9);
|
|||
|
|
/* build 9xn matrix from point matches */
|
|||
|
|
for (i=0;i<7;i++) {
|
|||
|
|
c(i,0) = m1[k[i]*2 ]*m2[k[i]*2 ];
|
|||
|
|
c(i,1) = m1[k[i]*2+1]*m2[k[i]*2 ];
|
|||
|
|
c(i,2) = m2[k[i]*2 ];
|
|||
|
|
c(i,3) = m1[k[i]*2 ]*m2[k[i]*2+1];
|
|||
|
|
c(i,4) = m1[k[i]*2+1]*m2[k[i]*2+1];
|
|||
|
|
c(i,5) = m2[k[i]*2+1];
|
|||
|
|
c(i,6) = m1[k[i]*2 ];
|
|||
|
|
c(i,7) = m1[k[i]*2+1];
|
|||
|
|
c(i,8) = 1.;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// SVD
|
|||
|
|
Eigen::JacobiSVD<Mat> svd(c, Eigen::ComputeFullV);
|
|||
|
|
// look for the two smallest eigenvalue of c'c
|
|||
|
|
typedef Eigen::Matrix<float, 9, 1> Vec9;
|
|||
|
|
Vec9 F1Vec = svd.matrixV().col(c.cols()-1);
|
|||
|
|
Vec9 F2Vec = svd.matrixV().col(c.cols()-2);
|
|||
|
|
|
|||
|
|
/* build basis of solutions */
|
|||
|
|
int cpt = 0;
|
|||
|
|
for (i=1;i<=3;i++)
|
|||
|
|
for (j=1;j<=3;j++)
|
|||
|
|
{
|
|||
|
|
F1[i][j] = F1Vec(cpt);
|
|||
|
|
F2[i][j] = F2Vec(cpt);
|
|||
|
|
cpt++;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* build cubic polynomial P(x)=det(F1+xF2) */
|
|||
|
|
a[0] = a[1] = a[2] = a[3] = 0.;
|
|||
|
|
for (i=1;i<=3;i++) {
|
|||
|
|
i2 = i%3+1;
|
|||
|
|
i3 = i2%3+1;
|
|||
|
|
a[0] += F1[i][1]*F1[i2][2]*F1[i3][3];
|
|||
|
|
a[1] +=
|
|||
|
|
F2[i][1]*F1[i2][2]*F1[i3][3]+
|
|||
|
|
F1[i][1]*F2[i2][2]*F1[i3][3]+
|
|||
|
|
F1[i][1]*F1[i2][2]*F2[i3][3];
|
|||
|
|
a[2] +=
|
|||
|
|
F1[i][1]*F2[i2][2]*F2[i3][3]+
|
|||
|
|
F2[i][1]*F1[i2][2]*F2[i3][3]+
|
|||
|
|
F2[i][1]*F2[i2][2]*F1[i3][3];
|
|||
|
|
a[3] += F2[i][1]*F2[i2][2]*F2[i3][3];
|
|||
|
|
}
|
|||
|
|
for (i=1;i<=3;i++) {
|
|||
|
|
i2 = (i+1)%3+1;
|
|||
|
|
i3 = (i2+1)%3+1;
|
|||
|
|
a[0] -= F1[i][1]*F1[i2][2]*F1[i3][3];
|
|||
|
|
a[1] -=
|
|||
|
|
F2[i][1]*F1[i2][2]*F1[i3][3]+
|
|||
|
|
F1[i][1]*F2[i2][2]*F1[i3][3]+
|
|||
|
|
F1[i][1]*F1[i2][2]*F2[i3][3];
|
|||
|
|
a[2] -=
|
|||
|
|
F1[i][1]*F2[i2][2]*F2[i3][3]+
|
|||
|
|
F2[i][1]*F1[i2][2]*F2[i3][3]+
|
|||
|
|
F2[i][1]*F2[i2][2]*F1[i3][3];
|
|||
|
|
a[3] -= F2[i][1]*F2[i2][2]*F2[i3][3];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return(FindCubicRoots(a,z));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void divide_match(const std::vector<Match>& matches, std::vector<float>& p1, std::vector<float>& p2)
|
|||
|
|
{
|
|||
|
|
float x1, y1, x2, y2;
|
|||
|
|
|
|||
|
|
p1.clear();
|
|||
|
|
p2.clear();
|
|||
|
|
p1.reserve(2 * matches.size());
|
|||
|
|
p2.reserve(2 * matches.size());
|
|||
|
|
std::vector<Match>::const_iterator it=matches.begin();
|
|||
|
|
for(; it != matches.end(); ++it) {
|
|||
|
|
x1 = (*it).x1; y1 = (*it).y1;
|
|||
|
|
x2 = (*it).x2; y2 = (*it).y2;
|
|||
|
|
p1.push_back(x1); p1.push_back(y1);
|
|||
|
|
p2.push_back(x2); p2.push_back(y2);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// float stereomatch(int img_x, int img_y, int size_pt, float* p1, float* p2, float** f, float* index, int* t, int* verb, int* n_flag, int* mode, int* stop)
|
|||
|
|
// float stereomatch(const wxImage& u1, std::vector<float>& p1, std::vector<float>& p2, std::vector<SmallVector<float,3> >& f, std::vector<float>& index, int* t, int* verb, int* n_flag, int* mode, int* stop)
|
|||
|
|
//int main(int argc, char** argv)
|
|||
|
|
float orsa(int width, int height, std::vector<Match>& match, std::vector<float>& index, int t_value, int verb_value, int n_flag_value, int mode_value, int stop_value)
|
|||
|
|
{
|
|||
|
|
// int width = 0, height = 0;
|
|||
|
|
// int t_value, verb_value, n_flag_value, mode_value, stop_value;
|
|||
|
|
int *t, *verb, *n_flag, *mode, *stop;
|
|||
|
|
|
|||
|
|
t = (int*)malloc(sizeof(int)); // maximum number of ransac trials
|
|||
|
|
verb = (int*)malloc(sizeof(int)); //verbose
|
|||
|
|
n_flag = (int*)malloc(sizeof(int)); // in order NOT to reinitialize the random seed
|
|||
|
|
mode = (int*)malloc(sizeof(int)); // mode: 0=deterministic 1=ransac 2=optimized ransac (ORSA) 3=automatic
|
|||
|
|
stop = (int*)malloc(sizeof(int)); // stop as soon as the first meaningful correspondence is found
|
|||
|
|
|
|||
|
|
if(width <=0 || height <= 0) {
|
|||
|
|
std::cerr << "Wrong dimensions of image" << std::endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<float> p1(2*match.size()), p2(2*match.size()), p1_backup(2*match.size()), p2_backup(2*match.size());
|
|||
|
|
|
|||
|
|
divide_match(match, p1, p2);
|
|||
|
|
p1_backup = p1;
|
|||
|
|
p2_backup = p2;
|
|||
|
|
|
|||
|
|
libNumerics::matrix<libNumerics::flnum> f(3, 3);
|
|||
|
|
f = 0;
|
|||
|
|
index = std::vector<float>(match.size());
|
|||
|
|
// Guoshen Yu, 2010.09.23
|
|||
|
|
// index.clear();
|
|||
|
|
|
|||
|
|
if(t_value <= 0) {
|
|||
|
|
std::cerr << "t should be greater than 0" << std::endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
*t = t_value;
|
|||
|
|
|
|||
|
|
if(verb_value == 0) {
|
|||
|
|
free(verb);
|
|||
|
|
verb = NULL;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
*verb = verb_value;
|
|||
|
|
if(verb_value != 1 && verb_value != 0) {
|
|||
|
|
std::cerr << "verb can only be 0 or 1" << std::endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(n_flag_value == 0) {
|
|||
|
|
free(n_flag);
|
|||
|
|
n_flag = NULL;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
*n_flag = n_flag_value;
|
|||
|
|
if(n_flag_value != 1 && n_flag_value != 0) {
|
|||
|
|
std::cerr << "n_flag can only be 0 or 1" << std::endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(mode_value != 0 && mode_value != 1 && mode_value != 2 && mode_value != 3) {
|
|||
|
|
std::cerr << "mode can only be 0 or 1 or 2 or 3" << std::endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
*mode = mode_value;
|
|||
|
|
|
|||
|
|
if(stop_value == 0) {
|
|||
|
|
free(stop);
|
|||
|
|
stop = NULL;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
*stop = stop_value;
|
|||
|
|
if(stop_value != 1 && stop_value != 0) {
|
|||
|
|
std::cerr << "stop can only be 0 or 1" << std::endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
int i,j,i0,k[8],idk[8],*id,m,n,l,minicur=0,miniall=0,delete_index,nid;
|
|||
|
|
int niter,maxniter,better,cont,optimization;
|
|||
|
|
float **F1,**F2,**F,nx,ny,z[3],minepscur,minepsall,nfa;
|
|||
|
|
float norm,minlogalphacur,minlogalphaall,logalpha,logalpha0;
|
|||
|
|
float *e,*logcn,*logc7,loge0;
|
|||
|
|
|
|||
|
|
/* initialize random seed if necessary */
|
|||
|
|
// if (!n_flag) srand48( (long int) time (NULL) + (long int) getpid() );
|
|||
|
|
// if (!n_flag) srand( (long int) time (NULL) + (long int) getpid() );
|
|||
|
|
|
|||
|
|
// Guoshen Yu, 2010.09.21: remove getpid which does not exist under Windows
|
|||
|
|
if (!n_flag) srand( (long int) time (NULL) );
|
|||
|
|
|
|||
|
|
/* check sizes */
|
|||
|
|
if (p1.size() != p2.size() || p1.size() < 14) {
|
|||
|
|
fprintf(stderr, "Inconsistent sizes.\n");
|
|||
|
|
exit(EXIT_FAILURE); /* indicate failure.*/
|
|||
|
|
}
|
|||
|
|
n = p1.size()/2;
|
|||
|
|
|
|||
|
|
/* 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]));
|
|||
|
|
// Guoshen Yu, 2010.09.22, Windows version
|
|||
|
|
i = (int)(0.5+pow(10., (double)(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)width;
|
|||
|
|
ny = (float)height;
|
|||
|
|
norm = 1./(float)sqrt((double)(nx*ny));
|
|||
|
|
logalpha0 = (float)(log10(2.)+0.5*log10((double)((nx*nx+ny*ny)*norm*norm)));
|
|||
|
|
for (i=0;i<n;i++) {
|
|||
|
|
p1[i*2 ] = (p1[i*2 ]-0.5*nx)*norm;
|
|||
|
|
p1[i*2+1] = (p1[i*2+1]-0.5*ny)*norm;
|
|||
|
|
p2[i*2 ] = (p2[i*2 ]-0.5*nx)*norm;
|
|||
|
|
p2[i*2+1] = (p2[i*2+1]-0.5*ny)*norm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* allocate and initialize memory */
|
|||
|
|
F = matrix(1,3,1,3);
|
|||
|
|
F1 = matrix(1,3,1,3);
|
|||
|
|
F2 = matrix(1,3,1,3);
|
|||
|
|
|
|||
|
|
|
|||
|
|
//delete_index = (index?0:1);
|
|||
|
|
delete_index = 0;
|
|||
|
|
|
|||
|
|
e = (float *)malloc(2*n*sizeof(float));
|
|||
|
|
id = (int *)malloc(n*sizeof(int));
|
|||
|
|
for (i=0;i<n;i++) id[i]=i;
|
|||
|
|
nid = n;
|
|||
|
|
|
|||
|
|
maxniter = (*mode==0?*t:*t-(*t)/10);
|
|||
|
|
minlogalphaall = minepsall = 10000.;
|
|||
|
|
niter = optimization = 0;
|
|||
|
|
i0=0; k[0]=-1; k[7]=n;
|
|||
|
|
|
|||
|
|
/********** MAIN LOOP **********/
|
|||
|
|
do {
|
|||
|
|
|
|||
|
|
niter++;
|
|||
|
|
|
|||
|
|
/* build next list of points */
|
|||
|
|
if (*mode) random_p7(k,nid);
|
|||
|
|
else {
|
|||
|
|
k[i0]++;
|
|||
|
|
for (i=i0+1;i<=6;i++) k[i]=k[i-1]+1;
|
|||
|
|
}
|
|||
|
|
for (i=0;i<7;i++) idk[i]=id[k[i]];
|
|||
|
|
|
|||
|
|
/* find epipolar transform */
|
|||
|
|
m = epipolar(p1,p2,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;i<n;i++) {
|
|||
|
|
logalpha = logalpha0+0.5*(float)log10((double)e[i*2]);
|
|||
|
|
nfa = loge0+logalpha*(float)(i-6)+logcn[i+1]+logc7[i+1];
|
|||
|
|
if (nfa<minepscur) {
|
|||
|
|
minepscur = nfa;
|
|||
|
|
minicur = i;
|
|||
|
|
minlogalphacur = logalpha;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (minepscur<minepsall) {
|
|||
|
|
/* store best result so far */
|
|||
|
|
better = 1;
|
|||
|
|
minepsall = minepscur;
|
|||
|
|
minlogalphaall = minlogalphacur;
|
|||
|
|
miniall = minicur;
|
|||
|
|
// if (f)
|
|||
|
|
for (l=1;l<=3;l++)
|
|||
|
|
for (j=1;j<=3;j++)
|
|||
|
|
f(l-1, j-1) = F[l][j];
|
|||
|
|
|
|||
|
|
// Guoshen Yu, 2010.09.22
|
|||
|
|
// for (i=0;i<=minicur;i++)
|
|||
|
|
for (i=0;i<minicur;i++)
|
|||
|
|
{
|
|||
|
|
index[i] = e[i*2+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;
|
|||
|
|
|
|||
|
|
// Guoshen Yu, 2010.09.22
|
|||
|
|
// for (j=0;j<=miniall;j++)
|
|||
|
|
for (j=0;j<miniall;j++)
|
|||
|
|
id[j] = (int)(index[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<maxniter?1:0);
|
|||
|
|
|
|||
|
|
} while (cont);
|
|||
|
|
|
|||
|
|
|
|||
|
|
//erase "index", only get the index of the meaningful matchings
|
|||
|
|
index.erase(index.begin()+miniall+1, index.end());
|
|||
|
|
if (verb)
|
|||
|
|
printf("best matching found: %d points log(alpha)=%f (%d iterations)\n",
|
|||
|
|
miniall+1,minlogalphaall,niter);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
/* free memory */
|
|||
|
|
free(id);
|
|||
|
|
free(e);
|
|||
|
|
// if (delete_index) mw_delete_flist(index);
|
|||
|
|
free_matrix(F2,1,3,1,3);
|
|||
|
|
free_matrix(F1,1,3,1,3);
|
|||
|
|
free_matrix(F,1,3,1,3);
|
|||
|
|
free(logc7);
|
|||
|
|
free(logcn);
|
|||
|
|
|
|||
|
|
if(t) free(t);
|
|||
|
|
if(verb) free(verb);
|
|||
|
|
if(n_flag) free(n_flag);
|
|||
|
|
if(mode) free(mode);
|
|||
|
|
if(stop) free(stop);
|
|||
|
|
|
|||
|
|
// return 0;
|
|||
|
|
return(minepsall);
|
|||
|
|
}
|