1 | #include <math.h> |
---|
2 | #include <string> |
---|
3 | #include <vector> |
---|
4 | #include <float.h> |
---|
5 | #include <algorithm> |
---|
6 | #ifndef _WIN32 |
---|
7 | #include <ext/algorithm> |
---|
8 | #endif |
---|
9 | #include <iostream> |
---|
10 | |
---|
11 | #include "Ops.h" |
---|
12 | |
---|
13 | //#define DEBUG_RANSAC |
---|
14 | |
---|
15 | using namespace MatVec; |
---|
16 | using namespace std; |
---|
17 | using namespace Geometry; |
---|
18 | using namespace MultiViewGeom; |
---|
19 | typedef vector<pair<Point2D<double>, Point2D<double> > > MatchVec2Im; |
---|
20 | |
---|
21 | namespace HomogConsistF { |
---|
22 | |
---|
23 | // Linear version for use in RANSAC |
---|
24 | template <class T, class MatchT> |
---|
25 | bool Linear(Matrix<T> &H, MatchT &Matches, const MultiViewGeom::FMatrix<T> &FM) { |
---|
26 | Matrix<double> EigenVecs(3,3), FT(3,3); |
---|
27 | // Do FM^{T}*FM |
---|
28 | double sum; |
---|
29 | // TransposeT() |
---|
30 | for (unsigned int i=0;i<3;i++) |
---|
31 | for (unsigned int k=0;k<3;k++) |
---|
32 | { |
---|
33 | sum=FM(i,0)*FM(k,0); |
---|
34 | for (unsigned int j=1;j<3;j++) |
---|
35 | sum+=FM(i,j)*FM(k,j); |
---|
36 | |
---|
37 | FT(i,k)=sum; |
---|
38 | } |
---|
39 | double EigenVals[3]; |
---|
40 | MatrixOps::EigenValVec_Symmetric(FT, EigenVals, EigenVecs); |
---|
41 | Homg2DPoint<double> e2(EigenVecs(0,0), EigenVecs(1,0), EigenVecs(2,0)); |
---|
42 | double norm=1.0/sqrt(e2.x()+e2.y()+e2.w()); |
---|
43 | e2.x()*=norm; e2.y()*=norm; e2.w()*=norm; |
---|
44 | |
---|
45 | const T e2x=e2.x(), e2y=e2.y(), e2w=e2.w(); |
---|
46 | // Helps numerical stability in some near degenerate cases to at least have a value in a1,a2,a3 |
---|
47 | T a1=0.3, a2=0.3, a3=0.3; |
---|
48 | H(0,0)=e2y*FM(2,0)-e2w*FM(1,0)+e2x*a1; |
---|
49 | H(0,1)=e2y*FM(2,1)-e2w*FM(1,1)+e2x*a2; |
---|
50 | H(0,2)=e2y*FM(2,2)-e2w*FM(1,2)+e2x*a3; |
---|
51 | |
---|
52 | H(1,0)=e2w*FM(0,0)-e2x*FM(2,0)+e2y*a1; |
---|
53 | H(1,1)=e2w*FM(0,1)-e2x*FM(2,1)+e2y*a2; |
---|
54 | H(1,2)=e2w*FM(0,2)-e2x*FM(2,2)+e2y*a3; |
---|
55 | |
---|
56 | H(2,0)=e2x*FM(1,0)-e2y*FM(0,0)+e2w*a1; |
---|
57 | H(2,1)=e2x*FM(1,1)-e2y*FM(0,1)+e2w*a2; |
---|
58 | H(2,2)=e2x*FM(1,2)-e2y*FM(0,2)+e2w*a3; |
---|
59 | |
---|
60 | // Build a constraint matrix |
---|
61 | Matrix<T> A(Matches.size(), 3); |
---|
62 | double *b=new double[Matches.size()]; |
---|
63 | |
---|
64 | for (unsigned int i=0; i<Matches.size(); ++i) { |
---|
65 | const T ux=Matches[i].first.x(), uy=Matches[i].first.y(); |
---|
66 | const T u2x=Matches[i].second.x(), u2y=Matches[i].second.y(); |
---|
67 | const T u2w=1.0; |
---|
68 | |
---|
69 | const T theta2=-atan((u2w*e2y-e2w*u2y)/(u2w*e2x-e2w*u2x)); |
---|
70 | |
---|
71 | // Constraints on x |
---|
72 | A(i,0)=(-cos(theta2)*u2w*e2x+sin(theta2)*u2w*e2y-(-cos(theta2)*u2x+sin(theta2)*u2y)*e2w)*ux; |
---|
73 | A(i,1)=(-cos(theta2)*u2w*e2x+sin(theta2)*u2w*e2y-(-cos(theta2)*u2x+sin(theta2)*u2y)*e2w)*uy; |
---|
74 | A(i,2)=(-cos(theta2)*u2w*e2x+sin(theta2)*u2w*e2y-(-cos(theta2)*u2x+sin(theta2)*u2y)*e2w); |
---|
75 | b[i]=-((cos(theta2)*u2w*H(0,0)-sin(theta2)*u2w*H(1,0)+(-cos(theta2)*u2x+sin(theta2)*u2y)*H(2,0))*ux+(-cos(theta2)*u2x+sin(theta2)*u2y)*H(2,2)+(cos(theta2)*u2w*H(0,1)-sin(theta2)*u2w*H(1,1)+(-cos(theta2)*u2x+sin(theta2)*u2y)*H(2,1))*uy+cos(theta2)*u2w*H(0,2)-sin(theta2)*u2w*H(1,2)); |
---|
76 | |
---|
77 | } |
---|
78 | |
---|
79 | double x[3]; |
---|
80 | bool ok=LAPACK::LeastSquaresRankDeff(A,b,x); |
---|
81 | |
---|
82 | delete[] b; |
---|
83 | |
---|
84 | a1=x[0]; a2=x[1]; a3=x[2]; |
---|
85 | |
---|
86 | H(0,0)=H(0,0)-e2x*a1; |
---|
87 | H(0,1)=H(0,1)-e2x*a2; |
---|
88 | H(0,2)=H(0,2)-e2x*a3; |
---|
89 | |
---|
90 | H(1,0)=H(1,0)-e2y*a1; |
---|
91 | H(1,1)=H(1,1)-e2y*a2; |
---|
92 | H(1,2)=H(1,2)-e2y*a3; |
---|
93 | |
---|
94 | H(2,0)=H(2,0)-e2w*a1; |
---|
95 | H(2,1)=H(2,1)-e2w*a2; |
---|
96 | H(2,2)=H(2,2)-e2w*a3; |
---|
97 | |
---|
98 | return ok; |
---|
99 | } |
---|
100 | |
---|
101 | |
---|
102 | |
---|
103 | |
---|
104 | |
---|
105 | |
---|
106 | |
---|
107 | |
---|
108 | |
---|
109 | |
---|
110 | |
---|
111 | |
---|
112 | double GetResidual_RANSAC(const pair<Point2D<double>, Point2D<double> > &Match, |
---|
113 | const Matrix<double> &H) { |
---|
114 | |
---|
115 | const double px=Match.first[0], py=Match.first[1], pw=1.0; |
---|
116 | double tx=H(0,0)*px+H(0,1)*py+H(0,2)*pw; |
---|
117 | double ty=H(1,0)*px+H(1,1)*py+H(1,2)*pw; |
---|
118 | double tz=H(2,0)*px+H(2,1)*py+H(2,2)*pw; |
---|
119 | Point2D<double> NP(tx/tz,ty/tz); |
---|
120 | double xdiff=NP.x()-Match.second.x(), ydiff=NP.y()-Match.second.y(); |
---|
121 | return xdiff*xdiff+ydiff*ydiff; |
---|
122 | } |
---|
123 | |
---|
124 | // The function itself |
---|
125 | void RANSAC(const vector<pair<Point2D<double>, Point2D<double> > > &Matches, |
---|
126 | vector<pair<Point2D<double>, Point2D<double> > > &OutlierFree, |
---|
127 | Matrix<double> &ReturnH, const FMatrix<double> &FM) { |
---|
128 | |
---|
129 | // Copy out of the iterator and into a vector so can normalise without destroying callers copy. |
---|
130 | unsigned int i; |
---|
131 | vector<Matrix<double> > Hs; |
---|
132 | // I know - I know - this is just in this simplified version |
---|
133 | OutlierFree.clear(); |
---|
134 | |
---|
135 | vector<pair<Point2D<double>, Point2D<double> > >::size_type numm=Matches.size(); |
---|
136 | |
---|
137 | if (numm<10) { |
---|
138 | cerr << "Planar homography RANSAC estimate (consistant with fundamental matrix) must be given at least 10 point matches to work!. So, go away and don't come back until you've got some more!\n" << endl; |
---|
139 | exit(1); |
---|
140 | } |
---|
141 | |
---|
142 | unsigned int minsample=10; |
---|
143 | // LMedS style calculation of number of sub-samples |
---|
144 | unsigned int NumberSubSamples=400; |
---|
145 | |
---|
146 | #ifdef DEBUG_RANSAC |
---|
147 | cerr << "Calculating Planar Homography matrix using LMedS:" << endl; |
---|
148 | cerr << "Number matches=" << numm << endl; |
---|
149 | cerr << "Using " << NumberSubSamples << " 4 point subsamples" << endl; |
---|
150 | #endif |
---|
151 | |
---|
152 | // Pre-allocate match vector for the four point algorithm |
---|
153 | vector<pair<Point2D<double>, Point2D<double> > > PassThrough(minsample); |
---|
154 | |
---|
155 | // So, run the 3 point algorithm on NumberSubSamples subsamples |
---|
156 | for (i=0;i<NumberSubSamples;i++) |
---|
157 | { |
---|
158 | // They always insist on removing the most useful functions in the windows STL. B******S |
---|
159 | #ifndef _WIN32 |
---|
160 | // Randomly select minsample points |
---|
161 | random_sample_n(Matches.begin(), Matches.end(), PassThrough.begin(), minsample); |
---|
162 | #else |
---|
163 | // This version is a bit naff since duplicates can occur |
---|
164 | unsigned int picked=0; |
---|
165 | while (picked<minsample) { |
---|
166 | int off=rand()%(int)numm; |
---|
167 | PassThrough[picked++]=Matches[off]; |
---|
168 | } |
---|
169 | #endif |
---|
170 | |
---|
171 | // Run the minimal algorithm |
---|
172 | Matrix<double> H(3,3); |
---|
173 | if (Linear(H, PassThrough, FM)) |
---|
174 | Hs.push_back(H); |
---|
175 | } |
---|
176 | |
---|
177 | // Now, need to determine the median of the squared residuals, for each image |
---|
178 | double *residuals=new double[numm], *bestr=new double[numm]; |
---|
179 | double minhuber=DBL_MAX, bestmaxerr=0.0; |
---|
180 | double med; |
---|
181 | int residual; |
---|
182 | vector<Matrix<double> >::const_iterator Hs_iter; |
---|
183 | MatchVec2Im::const_iterator iter; |
---|
184 | Matrix<double> BestH(3,3); |
---|
185 | |
---|
186 | for (Hs_iter=Hs.begin(); Hs_iter!=Hs.end(); ++Hs_iter) |
---|
187 | { |
---|
188 | const Matrix<double> &H=*Hs_iter; |
---|
189 | residual=0; |
---|
190 | |
---|
191 | // First run through all matches and evaluate the residual |
---|
192 | for (iter=Matches.begin();iter!=Matches.end();++iter) |
---|
193 | // evaluate d(m, Hm')^2 + d(m', H^T m)^2 |
---|
194 | residuals[residual++]=GetResidual_RANSAC(*iter, H); |
---|
195 | |
---|
196 | // Now, find the median of the results. |
---|
197 | if (residual>0) |
---|
198 | { |
---|
199 | med=MiscMath::VectorMedian(residuals, residuals+residual); |
---|
200 | |
---|
201 | // Get confidence limit for Huber function |
---|
202 | double robuststddev=1.4826*(1.0+(5.0/(numm-minsample)))*sqrt(med); |
---|
203 | double maxerr=3.84*robuststddev*robuststddev; |
---|
204 | |
---|
205 | // Now, go through residuals and sum |
---|
206 | double hubersum=0.0; |
---|
207 | for (int i=0;i<residual;++i) { |
---|
208 | if (residuals[i]<maxerr) |
---|
209 | hubersum+=residuals[i]; |
---|
210 | else |
---|
211 | hubersum+=maxerr; |
---|
212 | } |
---|
213 | |
---|
214 | if (hubersum<minhuber) |
---|
215 | { |
---|
216 | BestH=H; minhuber=hubersum; bestmaxerr=maxerr; |
---|
217 | copy(residuals, residuals+residual, bestr); |
---|
218 | } |
---|
219 | } |
---|
220 | } |
---|
221 | Hs.clear(); |
---|
222 | |
---|
223 | // Now, discard all false matches |
---|
224 | #ifdef DEBUG_RANSAC |
---|
225 | cerr << "Outlier threshold (95%) maxerr=" << bestmaxerr << endl; |
---|
226 | #endif |
---|
227 | |
---|
228 | // Work through all matches and discard if residual squared is less than just |
---|
229 | // calculated robust value. |
---|
230 | unsigned int off, outliers=0; |
---|
231 | for (off=0;off<numm;++off) |
---|
232 | { |
---|
233 | if (bestr[off]>bestmaxerr) |
---|
234 | ++outliers; |
---|
235 | else |
---|
236 | OutlierFree.push_back(Matches[off]); |
---|
237 | } |
---|
238 | |
---|
239 | ReturnH=BestH; |
---|
240 | |
---|
241 | #ifdef DEBUG_RANSAC |
---|
242 | cerr << endl << "Completed - found " << outliers << " outliers in " << numm << " points" << endl; |
---|
243 | #endif |
---|
244 | |
---|
245 | delete[] residuals; |
---|
246 | delete[] bestr; |
---|
247 | } |
---|
248 | |
---|
249 | #undef DEBUG_RANSAC |
---|
250 | |
---|
251 | } |
---|
252 | |
---|
253 | namespace MatrixOps { |
---|
254 | |
---|
255 | extern "C" { |
---|
256 | void dspevd_(char *, char *, int *, double *, double *, double *, int *, double *, int *, int *, int *, int *); |
---|
257 | } |
---|
258 | |
---|
259 | /** |
---|
260 | Finds the eigenvalues and eigenvectors of a symmetric matrix using LAPACK routines. Returns eigenvalues and eigenvectors in ascending order (lowest first). |
---|
261 | {\bf Errors:} |
---|
262 | \begin{itemize} |
---|
263 | \item {\em 1:} Error in EigenValVec_Symmetric: Supplied matrix is not symmetric - This is returned whenever the supplied matrix is not symmetric. |
---|
264 | \item {\em 2:} Error in EigenValVec_Symmetric: Eigenvalue and Eigenvector arrays are not the correct size - If the returning eigenvalues/eigenvector arrays are incorrectly sized this is returned.. |
---|
265 | */ |
---|
266 | void EigenValVec_Symmetric(const Matrix<double> &A, double *EigenVals, Matrix<double> &EigenVecs) |
---|
267 | { |
---|
268 | // First, check if it is symmetric |
---|
269 | unsigned int i,j; |
---|
270 | size_t N=A.num_rows(); |
---|
271 | bool Symm=true; |
---|
272 | |
---|
273 | if (N!=A.num_cols()) |
---|
274 | Symm=false; |
---|
275 | else |
---|
276 | { |
---|
277 | for (i=0;i<N-1;i++) |
---|
278 | for (j=0;j<i;j++) |
---|
279 | if (A(i,j)!=A(j,i)) |
---|
280 | Symm=false; |
---|
281 | } |
---|
282 | |
---|
283 | if (Symm==false) { |
---|
284 | throw 1; |
---|
285 | } |
---|
286 | |
---|
287 | double *A_Packed=new double[(N*(N+1))/2]; |
---|
288 | unsigned int indx=0; |
---|
289 | // Put into packed storage |
---|
290 | for (i=0;i<N;i++) |
---|
291 | for (j=0;j<=i;j++) |
---|
292 | A_Packed[indx++]=A(i,j); |
---|
293 | |
---|
294 | int rinfo=0; |
---|
295 | int LWork=(int)(1+6*N+N*N); |
---|
296 | double *Work=new double[LWork]; |
---|
297 | int LIWork=(int)(3+5*N); |
---|
298 | int *IWork=new int[LIWork]; |
---|
299 | int N_=(int)N; // Has to be an int for passing to fortran code |
---|
300 | |
---|
301 | EigenVecs.resize(N,N,false); |
---|
302 | |
---|
303 | dspevd_("V", "U", &N_, A_Packed, &EigenVals[0], &EigenVecs(0,0), &N_, Work, &LWork, IWork, &LIWork, &rinfo); |
---|
304 | |
---|
305 | delete[] A_Packed; |
---|
306 | delete[] Work; |
---|
307 | delete[] IWork; |
---|
308 | |
---|
309 | assert(rinfo==0); |
---|
310 | return; |
---|
311 | |
---|
312 | } |
---|
313 | |
---|
314 | // The LAPACK library functions |
---|
315 | extern "C" { |
---|
316 | int dgetrf_(int *m, int *n, double *a, int *lda, int *ipiv, int *info); |
---|
317 | int dgetri_(int *n, double *a, int *lda, int *ipiv, double *work, int *lwork, int *info); |
---|
318 | } |
---|
319 | |
---|
320 | bool inverse(const MatVec::Matrix<double> &A, MatVec::Matrix<double> &AInv) { |
---|
321 | |
---|
322 | int N = (int)A.num_rows(); |
---|
323 | if (A.num_rows()!=A.num_cols() || N<1) |
---|
324 | return false; |
---|
325 | |
---|
326 | // Going to use AInv as our workspace |
---|
327 | AInv=A; |
---|
328 | |
---|
329 | // Set up storage |
---|
330 | int *ipiv = new int[N]; |
---|
331 | int info; |
---|
332 | |
---|
333 | // Call for the LU factorisation |
---|
334 | dgetrf_(&N, &N, &(AInv(0,0)), &N, ipiv, &info); |
---|
335 | |
---|
336 | if (info>0) { |
---|
337 | std::cerr << "info=" << info << std::endl; |
---|
338 | delete[] ipiv; |
---|
339 | return false; |
---|
340 | } |
---|
341 | |
---|
342 | // Set up workspace |
---|
343 | int lwork = N; |
---|
344 | double *workd = new double[lwork]; |
---|
345 | |
---|
346 | // Now, call for inverse |
---|
347 | dgetri_(&N, &(AInv(0,0)), &N, ipiv, workd, &lwork, &info); |
---|
348 | |
---|
349 | delete[] ipiv; |
---|
350 | delete[] workd; |
---|
351 | |
---|
352 | // If got a singular matrix |
---|
353 | if (info>0) |
---|
354 | return false; |
---|
355 | |
---|
356 | return true; |
---|
357 | } |
---|
358 | |
---|
359 | |
---|
360 | } |
---|
361 | |
---|
362 | |
---|
363 | |
---|
364 | |
---|
365 | namespace MiscMath { |
---|
366 | |
---|
367 | double selectnth_heap(unsigned long m, double *ArrBegin, double *ArrEnd) { |
---|
368 | typedef double DataT; |
---|
369 | |
---|
370 | unsigned long i,j,k,n=(unsigned long)(distance(ArrBegin, ArrEnd)); |
---|
371 | DataT swap; |
---|
372 | m=n-m+1; // Since want to find nth lowest value, not nth highest value |
---|
373 | |
---|
374 | // const DataT *arr=ArrBegin-1; |
---|
375 | DataT *heap=new DataT[m+2]; |
---|
376 | |
---|
377 | copy(ArrBegin, ArrBegin+m, heap); |
---|
378 | sort(heap, heap+m); |
---|
379 | --heap; |
---|
380 | |
---|
381 | for (i=m+1;i<=n;++i) { |
---|
382 | if (ArrBegin[i-1]>heap[1]) { |
---|
383 | heap[1]=ArrBegin[i-1]; |
---|
384 | for (j=1;;) { |
---|
385 | k=j << 1; |
---|
386 | if (k>m) break; |
---|
387 | if (k!=m && heap[k] > heap[k+1]) ++k; |
---|
388 | if (heap[j] <= heap[k]) break; |
---|
389 | swap=heap[k]; |
---|
390 | heap[k]=heap[j]; |
---|
391 | heap[j]=swap; |
---|
392 | j=k; |
---|
393 | } |
---|
394 | } |
---|
395 | } |
---|
396 | |
---|
397 | DataT rval=heap[1]; |
---|
398 | ++heap; |
---|
399 | delete[] heap; |
---|
400 | |
---|
401 | return rval; |
---|
402 | } |
---|
403 | |
---|
404 | double VectorMedian(double *VecBegin, double *VecEnd) { |
---|
405 | if (VecBegin==VecEnd) |
---|
406 | return 0.0; |
---|
407 | |
---|
408 | double median; |
---|
409 | |
---|
410 | unsigned int size=(unsigned int)(distance(VecBegin, VecEnd)); |
---|
411 | |
---|
412 | if (size % 2 == 0) |
---|
413 | { |
---|
414 | // Don't use pedants if enough items in the iterator to get away with it |
---|
415 | if (size>100) |
---|
416 | median=MiscMath::selectnth_heap(size/2, VecBegin, VecEnd); |
---|
417 | else |
---|
418 | median= |
---|
419 | (MiscMath::selectnth_heap(size/2, VecBegin, VecEnd)+ |
---|
420 | MiscMath::selectnth_heap((size/2)+1, VecBegin, VecEnd))/2.0; |
---|
421 | } |
---|
422 | else |
---|
423 | // Odd case |
---|
424 | median=MiscMath::selectnth_heap(((size+1)/2)-1, VecBegin, VecEnd); |
---|
425 | |
---|
426 | return median; |
---|
427 | } |
---|
428 | |
---|
429 | } |
---|
430 | |
---|
431 | namespace LAPACK { |
---|
432 | |
---|
433 | extern "C" { |
---|
434 | int dgelsy_(int *, int *, int *, double *, int *, double *, int *, int *, double *, int *, double *, int *, int *); |
---|
435 | } |
---|
436 | |
---|
437 | bool LeastSquaresRankDeff(Matrix<double>& A, const double *B, double *X) { |
---|
438 | int m=(int)A.num_rows(), n=(int)A.num_cols(); |
---|
439 | int info=1, one=1; |
---|
440 | int *jpvt=new int[n]; |
---|
441 | int rank; |
---|
442 | double rcond=1e-14; |
---|
443 | |
---|
444 | int mn=m; |
---|
445 | if (n<m) |
---|
446 | mn=n; |
---|
447 | int lwork=mn+3*n+1; |
---|
448 | if (2*mn+1>lwork) |
---|
449 | lwork=2*mn+1; |
---|
450 | double *Work=new double[lwork]; |
---|
451 | |
---|
452 | // copy B into temp holder since X is wrong dimensions |
---|
453 | double *Temp=new double[m]; |
---|
454 | for (int i=0;i<m;++i) |
---|
455 | Temp[i]=B[i]; |
---|
456 | |
---|
457 | dgelsy_(&m, &n, &one, &(A(0,0)), &m, &Temp[0], &m, &jpvt[0], &rcond, &rank, &Work[0], &lwork, &info); |
---|
458 | |
---|
459 | if (info<0) { |
---|
460 | // cerr << "Error from linear least square solver (LAPACK routines)" << endl; |
---|
461 | return false; |
---|
462 | } |
---|
463 | |
---|
464 | for (int i=0;i<n;++i) |
---|
465 | X[i]=Temp[i]; |
---|
466 | |
---|
467 | delete[] jpvt; |
---|
468 | delete[] Work; |
---|
469 | delete[] Temp; |
---|
470 | return true; |
---|
471 | } |
---|
472 | |
---|
473 | } |
---|