1 /*
2 IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
3
4 By downloading, copying, installing or using the software you agree to this license.
5 If you do not agree to this license, do not download, install,
6 copy or use the software.
7
8
9 BSD 3-Clause License
10
11 Copyright (C) 2014, Olexa Bilaniuk, Hamid Bazargani & Robert Laganiere, all rights reserved.
12
13 Redistribution and use in source and binary forms, with or without modification,
14 are permitted provided that the following conditions are met:
15
16 * Redistribution's of source code must retain the above copyright notice,
17 this list of conditions and the following disclaimer.
18
19 * Redistribution's in binary form must reproduce the above copyright notice,
20 this list of conditions and the following disclaimer in the documentation
21 and/or other materials provided with the distribution.
22
23 * The name of the copyright holders may not be used to endorse or promote products
24 derived from this software without specific prior written permission.
25
26 This software is provided by the copyright holders and contributors "as is" and
27 any express or implied warranties, including, but not limited to, the implied
28 warranties of merchantability and fitness for a particular purpose are disclaimed.
29 In no event shall the Intel Corporation or contributors be liable for any direct,
30 indirect, incidental, special, exemplary, or consequential damages
31 (including, but not limited to, procurement of substitute goods or services;
32 loss of use, data, or profits; or business interruption) however caused
33 and on any theory of liability, whether in contract, strict liability,
34 or tort (including negligence or otherwise) arising in any way out of
35 the use of this software, even if advised of the possibility of such damage.
36 */
37
38 /**
39 * Bilaniuk, Olexa, Hamid Bazargani, and Robert Laganiere. "Fast Target
40 * Recognition on Mobile Devices: Revisiting Gaussian Elimination for the
41 * Estimation of Planar Homographies." In Computer Vision and Pattern
42 * Recognition Workshops (CVPRW), 2014 IEEE Conference on, pp. 119-125.
43 * IEEE, 2014.
44 */
45
46 /* Includes */
47 #include <precomp.hpp>
48 #include <opencv2/core.hpp>
49 #include <stdlib.h>
50 #include <stdio.h>
51 #include <stdint.h>
52 #include <string.h>
53 #include <stddef.h>
54 #include <limits.h>
55 #include <float.h>
56 #include <math.h>
57 #include <vector>
58 #include "rho.h"
59
60
61
62
63
64 /* For the sake of cv:: namespace ONLY: */
65 namespace cv{/* For C support, replace with extern "C" { */
66
67
68 /* Constants */
69 const int MEM_ALIGN = 32;
70 const size_t HSIZE = (3*3*sizeof(float));
71 const double MIN_DELTA_CHNG = 0.1;
72 // const double CHI_STAT = 2.706;
73 const double CHI_SQ = 1.645;
74 // const double RLO = 0.25;
75 // const double RHI = 0.75;
76 const int MAXLEVMARQITERS = 100;
77 const int SMPL_SIZE = 4; /* 4 points required per model */
78 const int SPRT_T_M = 25; /* Guessing 25 match evlauations / 1 model generation */
79 const int SPRT_M_S = 1; /* 1 model per sample */
80 const double SPRT_EPSILON = 0.1; /* No explanation */
81 const double SPRT_DELTA = 0.01; /* No explanation */
82 const double LM_GAIN_LO = 0.25; /* See sacLMGain(). */
83 const double LM_GAIN_HI = 0.75; /* See sacLMGain(). */
84
85
86 /* Data Structures */
87
88 /**
89 * Base Struct for RHO algorithm.
90 *
91 * A RHO estimator has initialization, finalization, capacity, seeding and
92 * homography-estimation APIs that must be implemented.
93 */
94
95 struct RHO_HEST{
96 /* This is a virtual base class; It should have a virtual destructor. */
~RHO_HESTcv::RHO_HEST97 virtual ~RHO_HEST(){}
98
99 /* External Interface Methods */
100
101 /**
102 * Initialization work.
103 *
104 * @return 0 if initialization is unsuccessful; non-zero otherwise.
105 */
106
initializecv::RHO_HEST107 virtual inline int initialize(void){return 1;}
108
109
110 /**
111 * Finalization work.
112 */
113
finalizecv::RHO_HEST114 virtual inline void finalize(void){}
115
116 /**
117 * Ensure that the estimator context's internal table for the non-randomness
118 * criterion is at least of the given size, and uses the given beta. The table
119 * should be larger than the maximum number of matches fed into the estimator.
120 *
121 * A value of N of 0 requests deallocation of the table.
122 *
123 * @param [in] N If 0, deallocate internal table. If > 0, ensure that the
124 * internal table is of at least this size, reallocating if
125 * necessary.
126 * @param [in] beta The beta-factor to use within the table.
127 * @return 0 if unsuccessful; non-zero otherwise.
128 */
129
ensureCapacitycv::RHO_HEST130 virtual inline int ensureCapacity(unsigned N, double beta){
131 (void)N;
132 (void)beta;
133
134 return 1;
135 }
136
137
138 /**
139 * Generates a random double uniformly distributed in the range [0, 1).
140 *
141 * The default implementation uses the xorshift128+ algorithm from
142 * Sebastiano Vigna. Further scramblings of Marsaglia's xorshift generators.
143 * CoRR, abs/1402.6246, 2014.
144 * http://vigna.di.unimi.it/ftp/papers/xorshiftplus.pdf
145 *
146 * Source roughly as given in
147 * http://en.wikipedia.org/wiki/Xorshift#Xorshift.2B
148 */
149
fastRandomcv::RHO_HEST150 virtual inline double fastRandom(void){
151 uint64_t x = prng.s[0];
152 uint64_t y = prng.s[1];
153 x ^= x << 23; // a
154 x ^= x >> 17; // b
155 x ^= y ^ (y >> 26); // c
156 prng.s[0] = y;
157 prng.s[1] = x;
158 uint64_t s = x + y;
159
160 return s * 5.421010862427522e-20;/* 2^-64 */
161 }
162
163
164 /**
165 * Seeds the context's PRNG.
166 *
167 * @param [in] seed A 64-bit unsigned integer seed.
168 */
169
fastSeedcv::RHO_HEST170 virtual inline void fastSeed(uint64_t seed){
171 int i;
172
173 prng.s[0] = seed;
174 prng.s[1] = ~seed;/* Guarantees one of the elements will be non-zero. */
175
176 /**
177 * Escape from zero-land (see xorshift128+ paper). Approximately 20
178 * iterations required according to the graph.
179 */
180
181 for(i=0;i<20;i++){
182 fastRandom();
183 }
184 }
185
186
187 /**
188 * Estimates the homography using the given context, matches and parameters to
189 * PROSAC.
190 *
191 * @param [in] src The pointer to the source points of the matches.
192 * Cannot be NULL.
193 * @param [in] dst The pointer to the destination points of the matches.
194 * Cannot be NULL.
195 * @param [out] inl The pointer to the output mask of inlier matches.
196 * May be NULL.
197 * @param [in] N The number of matches.
198 * @param [in] maxD The maximum distance.
199 * @param [in] maxI The maximum number of PROSAC iterations.
200 * @param [in] rConvg The RANSAC convergence parameter.
201 * @param [in] cfd The required confidence in the solution.
202 * @param [in] minInl The minimum required number of inliers.
203 * @param [in] beta The beta-parameter for the non-randomness criterion.
204 * @param [in] flags A union of flags to control the estimation.
205 * @param [in] guessH An extrinsic guess at the solution H, or NULL if
206 * none provided.
207 * @param [out] finalH The final estimation of H, or the zero matrix if
208 * the minimum number of inliers was not met.
209 * Cannot be NULL.
210 * @return The number of inliers if the minimum number of
211 * inliers for acceptance was reached; 0 otherwise.
212 */
213
214 virtual unsigned rhoHest(const float* src, /* Source points */
215 const float* dst, /* Destination points */
216 char* inl, /* Inlier mask */
217 unsigned N, /* = src.length = dst.length = inl.length */
218 float maxD, /* Works: 3.0 */
219 unsigned maxI, /* Works: 2000 */
220 unsigned rConvg, /* Works: 2000 */
221 double cfd, /* Works: 0.995 */
222 unsigned minInl, /* Minimum: 4 */
223 double beta, /* Works: 0.35 */
224 unsigned flags, /* Works: 0 */
225 const float* guessH, /* Extrinsic guess, NULL if none provided */
226 float* finalH) = 0; /* Final result. */
227
228
229
230 /* PRNG XORshift128+ */
231 struct{
232 uint64_t s[2]; /* PRNG state */
233 } prng;
234 };
235
236
237
238 /**
239 * Generic C implementation of RHO algorithm.
240 */
241
242 struct RHO_HEST_REFC : RHO_HEST{
243 /**
244 * Virtual Arguments.
245 *
246 * Exactly the same as at function call, except:
247 * - minInl is enforced to be >= 4.
248 */
249
250 struct{
251 const float* src;
252 const float* dst;
253 char* inl;
254 unsigned N;
255 float maxD;
256 unsigned maxI;
257 unsigned rConvg;
258 double cfd;
259 unsigned minInl;
260 double beta;
261 unsigned flags;
262 const float* guessH;
263 float* finalH;
264 } arg;
265
266 /* PROSAC Control */
267 struct{
268 unsigned i; /* Iteration Number */
269 unsigned phNum; /* Phase Number */
270 unsigned phEndI; /* Phase End Iteration */
271 double phEndFpI; /* Phase floating-point End Iteration */
272 unsigned phMax; /* Termination phase number */
273 unsigned phNumInl; /* Number of inliers for termination phase */
274 unsigned numModels; /* Number of models tested */
275 unsigned* smpl; /* Sample of match indexes */
276 } ctrl;
277
278 /* Current model being tested */
279 struct{
280 float* pkdPts; /* Packed points */
281 float* H; /* Homography */
282 char* inl; /* Mask of inliers */
283 unsigned numInl; /* Number of inliers */
284 } curr;
285
286 /* Best model (so far) */
287 struct{
288 float* H; /* Homography */
289 char* inl; /* Mask of inliers */
290 unsigned numInl; /* Number of inliers */
291 } best;
292
293 /* Non-randomness criterion */
294 struct{
295 std::vector<unsigned> tbl; /* Non-Randomness: Table */
296 unsigned size; /* Non-Randomness: Size */
297 double beta; /* Non-Randomness: Beta */
298 } nr;
299
300 /* SPRT Evaluator */
301 struct{
302 double t_M; /* t_M */
303 double m_S; /* m_S */
304 double epsilon; /* Epsilon */
305 double delta; /* delta */
306 double A; /* SPRT Threshold */
307 unsigned Ntested; /* Number of points tested */
308 unsigned Ntestedtotal; /* Number of points tested in total */
309 int good; /* Good/bad flag */
310 double lambdaAccept; /* Accept multiplier */
311 double lambdaReject; /* Reject multiplier */
312 } eval;
313
314 /* Levenberg-Marquardt Refinement */
315 struct{
316 float (* JtJ)[8]; /* JtJ matrix */
317 float (* tmp1)[8]; /* Temporary 1 */
318 float* Jte; /* Jte vector */
319 } lm;
320
321 /* Memory Management */
322 struct{
323 cv::Mat perObj;
324 cv::Mat perRun;
325 } mem;
326
327 /* Initialized? */
328 int initialized;
329
330
331 /* Empty constructors and destructors */
332 public:
333 RHO_HEST_REFC();
334 private: /* Forbid copying. */
335 RHO_HEST_REFC(const RHO_HEST_REFC&);
336 public:
337 ~RHO_HEST_REFC();
338
339 /* Methods to implement external interface */
340 inline int initialize(void);
341 inline void finalize(void);
342 inline int ensureCapacity(unsigned N, double beta);
343 unsigned rhoHest(const float* src, /* Source points */
344 const float* dst, /* Destination points */
345 char* inl, /* Inlier mask */
346 unsigned N, /* = src.length = dst.length = inl.length */
347 float maxD, /* Works: 3.0 */
348 unsigned maxI, /* Works: 2000 */
349 unsigned rConvg, /* Works: 2000 */
350 double cfd, /* Works: 0.995 */
351 unsigned minInl, /* Minimum: 4 */
352 double beta, /* Works: 0.35 */
353 unsigned flags, /* Works: 0 */
354 const float* guessH, /* Extrinsic guess, NULL if none provided */
355 float* finalH); /* Final result. */
356
357
358
359 /* Methods to implement internals */
360 inline void allocatePerObj(void);
361 inline void allocatePerRun(void);
362 inline void deallocatePerRun(void);
363 inline void deallocatePerObj(void);
364 inline int initRun(void);
365 inline void finiRun(void);
366 inline int haveExtrinsicGuess(void);
367 inline int hypothesize(void);
368 inline int verify(void);
369 inline int isNREnabled(void);
370 inline int isRefineEnabled(void);
371 inline int isFinalRefineEnabled(void);
372 inline int PROSACPhaseEndReached(void);
373 inline void PROSACGoToNextPhase(void);
374 inline void getPROSACSample(void);
375 inline void rndSmpl(unsigned sampleSize,
376 unsigned* currentSample,
377 unsigned dataSetSize);
378 inline int isSampleDegenerate(void);
379 inline void generateModel(void);
380 inline int isModelDegenerate(void);
381 inline void evaluateModelSPRT(void);
382 inline void updateSPRT(void);
383 inline void designSPRTTest(void);
384 inline int isBestModel(void);
385 inline int isBestModelGoodEnough(void);
386 inline void saveBestModel(void);
387 inline void nStarOptimize(void);
388 inline void updateBounds(void);
389 inline void outputModel(void);
390 inline void outputZeroH(void);
391 inline int canRefine(void);
392 inline void refine(void);
393 };
394
395
396
397
398 /**
399 * Prototypes for purely-computational code.
400 */
401
402 static inline void sacInitNonRand (double beta,
403 unsigned start,
404 unsigned N,
405 unsigned* nonRandMinInl);
406 static inline double sacInitPEndFpI (const unsigned ransacConvg,
407 const unsigned n,
408 const unsigned s);
409 static inline unsigned sacCalcIterBound (double confidence,
410 double inlierRate,
411 unsigned sampleSize,
412 unsigned maxIterBound);
413 static inline void hFuncRefC (float* packedPoints, float* H);
414 static inline void sacCalcJacobianErrors(const float* H,
415 const float* src,
416 const float* dst,
417 const char* inl,
418 unsigned N,
419 float (* JtJ)[8],
420 float* Jte,
421 float* Sp);
422 static inline float sacLMGain (const float* dH,
423 const float* Jte,
424 const float S,
425 const float newS,
426 const float lambda);
427 static inline int sacChol8x8Damped (const float (*A)[8],
428 float lambda,
429 float (*L)[8]);
430 static inline void sacTRInv8x8 (const float (*L)[8],
431 float (*M)[8]);
432 static inline void sacTRISolve8x8 (const float (*L)[8],
433 const float* Jte,
434 float* dH);
435 static inline void sacSub8x1 (float* Hout,
436 const float* H,
437 const float* dH);
438
439
440
441 /* Functions */
442
443 /**
444 * External access to context constructor.
445 *
446 * @return A pointer to the context if successful; NULL if an error occured.
447 */
448
rhoInit(void)449 Ptr<RHO_HEST> rhoInit(void){
450 /* Select an optimized implementation of RHO here. */
451
452 #if 1
453 /**
454 * For now, only the generic C implementation is available. In the future,
455 * SSE2/AVX/AVX2/FMA/NEON versions may be added, and they will be selected
456 * depending on cv::checkHardwareSupport()'s return values.
457 */
458
459 Ptr<RHO_HEST> p = Ptr<RHO_HEST>(new RHO_HEST_REFC);
460 #endif
461
462 /* Initialize it. */
463 if(p){
464 if(!p->initialize()){
465 p.release();
466 }
467 }
468
469 /* Return it. */
470 return p;
471 }
472
473
474 /**
475 * External access to non-randomness table resize.
476 */
477
rhoEnsureCapacity(Ptr<RHO_HEST> p,unsigned N,double beta)478 int rhoEnsureCapacity(Ptr<RHO_HEST> p, unsigned N, double beta){
479 return p->ensureCapacity(N, beta);
480 }
481
482
483 /**
484 * Seeds the internal PRNG with the given seed.
485 */
486
rhoSeed(Ptr<RHO_HEST> p,uint64_t seed)487 void rhoSeed(Ptr<RHO_HEST> p, uint64_t seed){
488 p->fastSeed(seed);
489 }
490
491
492 /**
493 * Estimates the homography using the given context, matches and parameters to
494 * PROSAC.
495 *
496 * @param [in/out] p The context to use for homography estimation. Must
497 * be already initialized. Cannot be NULL.
498 * @param [in] src The pointer to the source points of the matches.
499 * Must be aligned to 4 bytes. Cannot be NULL.
500 * @param [in] dst The pointer to the destination points of the matches.
501 * Must be aligned to 16 bytes. Cannot be NULL.
502 * @param [out] inl The pointer to the output mask of inlier matches.
503 * Must be aligned to 16 bytes. May be NULL.
504 * @param [in] N The number of matches.
505 * @param [in] maxD The maximum distance.
506 * @param [in] maxI The maximum number of PROSAC iterations.
507 * @param [in] rConvg The RANSAC convergence parameter.
508 * @param [in] cfd The required confidence in the solution.
509 * @param [in] minInl The minimum required number of inliers.
510 * @param [in] beta The beta-parameter for the non-randomness criterion.
511 * @param [in] flags A union of flags to control the estimation.
512 * @param [in] guessH An extrinsic guess at the solution H, or NULL if
513 * none provided.
514 * @param [out] finalH The final estimation of H, or the zero matrix if
515 * the minimum number of inliers was not met.
516 * Cannot be NULL.
517 * @return The number of inliers if the minimum number of
518 * inliers for acceptance was reached; 0 otherwise.
519 */
520
rhoHest(Ptr<RHO_HEST> p,const float * src,const float * dst,char * inl,unsigned N,float maxD,unsigned maxI,unsigned rConvg,double cfd,unsigned minInl,double beta,unsigned flags,const float * guessH,float * finalH)521 unsigned rhoHest(Ptr<RHO_HEST> p, /* Homography estimation context. */
522 const float* src, /* Source points */
523 const float* dst, /* Destination points */
524 char* inl, /* Inlier mask */
525 unsigned N, /* = src.length = dst.length = inl.length */
526 float maxD, /* Works: 3.0 */
527 unsigned maxI, /* Works: 2000 */
528 unsigned rConvg, /* Works: 2000 */
529 double cfd, /* Works: 0.995 */
530 unsigned minInl, /* Minimum: 4 */
531 double beta, /* Works: 0.35 */
532 unsigned flags, /* Works: 0 */
533 const float* guessH, /* Extrinsic guess, NULL if none provided */
534 float* finalH){ /* Final result. */
535 return p->rhoHest(src, dst, inl, N, maxD, maxI, rConvg, cfd, minInl, beta,
536 flags, guessH, finalH);
537 }
538
539
540
541
542
543
544
545
546
547
548
549
550 /*********************** RHO_HEST_REFC implementation **********************/
551
552 /**
553 * Constructor for RHO_HEST_REFC.
554 *
555 * Does nothing. True initialization is done by initialize().
556 */
557
RHO_HEST_REFC()558 RHO_HEST_REFC::RHO_HEST_REFC() : initialized(0){
559
560 }
561
562 /**
563 * Private copy constructor for RHO_HEST_REFC. Disabled.
564 */
565
RHO_HEST_REFC(const RHO_HEST_REFC &)566 RHO_HEST_REFC::RHO_HEST_REFC(const RHO_HEST_REFC&) : initialized(0){
567
568 }
569
570 /**
571 * Destructor for RHO_HEST_REFC.
572 */
573
~RHO_HEST_REFC()574 RHO_HEST_REFC::~RHO_HEST_REFC(){
575 if(initialized){
576 finalize();
577 }
578 }
579
580
581
582 /**
583 * Initialize the estimator context, by allocating the aligned buffers
584 * internally needed.
585 *
586 * Currently there are 5 per-estimator buffers:
587 * - The buffer of m indexes representing a sample
588 * - The buffer of 16 floats representing m matches (x,y) -> (X,Y).
589 * - The buffer for the current homography
590 * - The buffer for the best-so-far homography
591 * - Optionally, the non-randomness criterion table
592 *
593 * Returns 0 if unsuccessful and non-0 otherwise.
594 */
595
initialize(void)596 inline int RHO_HEST_REFC::initialize(void){
597 initialized = 0;
598
599
600 allocatePerObj();
601
602 curr.inl = NULL;
603 curr.numInl = 0;
604
605 best.inl = NULL;
606 best.numInl = 0;
607
608 nr.size = 0;
609 nr.beta = 0.0;
610
611
612 fastSeed((uint64_t)~0);
613
614
615 int areAllAllocsSuccessful = !mem.perObj.empty();
616
617 if(!areAllAllocsSuccessful){
618 finalize();
619 }else{
620 initialized = 1;
621 }
622
623 return areAllAllocsSuccessful;
624 }
625
626 /**
627 * Finalize.
628 *
629 * Finalize the estimator context, by freeing the aligned buffers used
630 * internally.
631 */
632
finalize(void)633 inline void RHO_HEST_REFC::finalize(void){
634 if(initialized){
635 deallocatePerObj();
636
637 initialized = 0;
638 }
639 }
640
641 /**
642 * Ensure that the estimator context's internal table for non-randomness
643 * criterion is at least of the given size, and uses the given beta. The table
644 * should be larger than the maximum number of matches fed into the estimator.
645 *
646 * A value of N of 0 requests deallocation of the table.
647 *
648 * @param [in] N If 0, deallocate internal table. If > 0, ensure that the
649 * internal table is of at least this size, reallocating if
650 * necessary.
651 * @param [in] beta The beta-factor to use within the table.
652 * @return 0 if unsuccessful; non-zero otherwise.
653 *
654 * Reads: nr.*
655 * Writes: nr.*
656 */
657
ensureCapacity(unsigned N,double beta)658 inline int RHO_HEST_REFC::ensureCapacity(unsigned N, double beta){
659 if(N == 0){
660 /* Clear. */
661 nr.tbl.clear();
662 nr.size = 0;
663 }else if(nr.beta != beta){
664 /* Beta changed. Redo all the work. */
665 nr.tbl.resize(N);
666 nr.beta = beta;
667 sacInitNonRand(nr.beta, 0, N, &nr.tbl[0]);
668 nr.size = N;
669 }else if(N > nr.size){
670 /* Work is partially done. Do rest of it. */
671 nr.tbl.resize(N);
672 sacInitNonRand(nr.beta, nr.size, N, &nr.tbl[nr.size]);
673 nr.size = N;
674 }else{
675 /* Work is already done. Do nothing. */
676 }
677
678 return 1;
679 }
680
681
682 /**
683 * Estimates the homography using the given context, matches and parameters to
684 * PROSAC.
685 *
686 * @param [in] src The pointer to the source points of the matches.
687 * Must be aligned to 4 bytes. Cannot be NULL.
688 * @param [in] dst The pointer to the destination points of the matches.
689 * Must be aligned to 4 bytes. Cannot be NULL.
690 * @param [out] inl The pointer to the output mask of inlier matches.
691 * Must be aligned to 4 bytes. May be NULL.
692 * @param [in] N The number of matches.
693 * @param [in] maxD The maximum distance.
694 * @param [in] maxI The maximum number of PROSAC iterations.
695 * @param [in] rConvg The RANSAC convergence parameter.
696 * @param [in] cfd The required confidence in the solution.
697 * @param [in] minInl The minimum required number of inliers.
698 * @param [in] beta The beta-parameter for the non-randomness criterion.
699 * @param [in] flags A union of flags to control the estimation.
700 * @param [in] guessH An extrinsic guess at the solution H, or NULL if
701 * none provided.
702 * @param [out] finalH The final estimation of H, or the zero matrix if
703 * the minimum number of inliers was not met.
704 * Cannot be NULL.
705 * @return The number of inliers if the minimum number of
706 * inliers for acceptance was reached; 0 otherwise.
707 */
708
rhoHest(const float * src,const float * dst,char * inl,unsigned N,float maxD,unsigned maxI,unsigned rConvg,double cfd,unsigned minInl,double beta,unsigned flags,const float * guessH,float * finalH)709 unsigned RHO_HEST_REFC::rhoHest(const float* src, /* Source points */
710 const float* dst, /* Destination points */
711 char* inl, /* Inlier mask */
712 unsigned N, /* = src.length = dst.length = inl.length */
713 float maxD, /* Works: 3.0 */
714 unsigned maxI, /* Works: 2000 */
715 unsigned rConvg, /* Works: 2000 */
716 double cfd, /* Works: 0.995 */
717 unsigned minInl, /* Minimum: 4 */
718 double beta, /* Works: 0.35 */
719 unsigned flags, /* Works: 0 */
720 const float* guessH, /* Extrinsic guess, NULL if none provided */
721 float* finalH){ /* Final result. */
722
723 /**
724 * Setup
725 */
726
727 arg.src = src;
728 arg.dst = dst;
729 arg.inl = inl;
730 arg.N = N;
731 arg.maxD = maxD;
732 arg.maxI = maxI;
733 arg.rConvg = rConvg;
734 arg.cfd = cfd;
735 arg.minInl = minInl;
736 arg.beta = beta;
737 arg.flags = flags;
738 arg.guessH = guessH;
739 arg.finalH = finalH;
740 if(!initRun()){
741 outputZeroH();
742 finiRun();
743 return 0;
744 }
745
746 /**
747 * Extrinsic Guess
748 */
749
750 if(haveExtrinsicGuess()){
751 verify();
752 }
753
754
755 /**
756 * PROSAC Loop
757 */
758
759 for(ctrl.i=0; ctrl.i < arg.maxI || ctrl.i < 100; ctrl.i++){
760 hypothesize() && verify();
761 }
762
763
764 /**
765 * Teardown
766 */
767
768 if(isFinalRefineEnabled() && canRefine()){
769 refine();
770 }
771
772 outputModel();
773 finiRun();
774 return isBestModelGoodEnough() ? best.numInl : 0;
775 }
776
777
778 /**
779 * Allocate per-object dynamic storage.
780 *
781 * This includes aligned, fixed-size internal buffers, but excludes any buffers
782 * whose size cannot be determined ahead-of-time (before the number of matches
783 * is known).
784 *
785 * All buffer memory is allocated in one single shot, and all pointers are
786 * initialized.
787 */
788
allocatePerObj(void)789 inline void RHO_HEST_REFC::allocatePerObj(void){
790 /* We have known sizes */
791 size_t ctrl_smpl_sz = SMPL_SIZE*sizeof(*ctrl.smpl);
792 size_t curr_pkdPts_sz = SMPL_SIZE*2*2*sizeof(*curr.pkdPts);
793 size_t curr_H_sz = HSIZE;
794 size_t best_H_sz = HSIZE;
795 size_t lm_JtJ_sz = 8*8*sizeof(float);
796 size_t lm_tmp1_sz = 8*8*sizeof(float);
797 size_t lm_Jte_sz = 1*8*sizeof(float);
798
799 /* We compute offsets */
800 size_t total = 0;
801 #define MK_OFFSET(v) \
802 size_t v ## _of = total; \
803 total = alignSize(v ## _of + v ## _sz, MEM_ALIGN)
804
805 MK_OFFSET(ctrl_smpl);
806 MK_OFFSET(curr_pkdPts);
807 MK_OFFSET(curr_H);
808 MK_OFFSET(best_H);
809 MK_OFFSET(lm_JtJ);
810 MK_OFFSET(lm_tmp1);
811 MK_OFFSET(lm_Jte);
812
813 #undef MK_OFFSET
814
815 /* Allocate dynamic memory managed by cv::Mat */
816 mem.perObj.create(1, (int)(total + MEM_ALIGN), CV_8UC1);
817
818 /* Extract aligned pointer */
819 unsigned char* ptr = alignPtr(mem.perObj.data, MEM_ALIGN);
820
821 /* Assign pointers */
822 ctrl.smpl = (unsigned*) (ptr + ctrl_smpl_of);
823 curr.pkdPts = (float*) (ptr + curr_pkdPts_of);
824 curr.H = (float*) (ptr + curr_H_of);
825 best.H = (float*) (ptr + best_H_of);
826 lm.JtJ = (float(*)[8])(ptr + lm_JtJ_of);
827 lm.tmp1 = (float(*)[8])(ptr + lm_tmp1_of);
828 lm.Jte = (float*) (ptr + lm_Jte_of);
829 }
830
831
832 /**
833 * Allocate per-run dynamic storage.
834 *
835 * This includes storage that is proportional to the number of points, such as
836 * the inlier mask.
837 */
838
allocatePerRun(void)839 inline void RHO_HEST_REFC::allocatePerRun(void){
840 /* We have known sizes */
841 size_t best_inl_sz = arg.N;
842 size_t curr_inl_sz = arg.N;
843
844 /* We compute offsets */
845 size_t total = 0;
846 #define MK_OFFSET(v) \
847 size_t v ## _of = total; \
848 total = alignSize(v ## _of + v ## _sz, MEM_ALIGN)
849
850 MK_OFFSET(best_inl);
851 MK_OFFSET(curr_inl);
852
853 #undef MK_OFFSET
854
855 /* Allocate dynamic memory managed by cv::Mat */
856 mem.perRun.create(1, (int)(total + MEM_ALIGN), CV_8UC1);
857
858 /* Extract aligned pointer */
859 unsigned char* ptr = alignPtr(mem.perRun.data, MEM_ALIGN);
860
861 /* Assign pointers */
862 best.inl = (char*)(ptr + best_inl_of);
863 curr.inl = (char*)(ptr + curr_inl_of);
864 }
865
866
867 /**
868 * Deallocate per-run dynamic storage.
869 *
870 * Undoes the work by allocatePerRun().
871 */
872
deallocatePerRun(void)873 inline void RHO_HEST_REFC::deallocatePerRun(void){
874 best.inl = NULL;
875 curr.inl = NULL;
876
877 mem.perRun.release();
878 }
879
880
881 /**
882 * Deallocate per-object dynamic storage.
883 *
884 * Undoes the work by allocatePerObj().
885 */
886
deallocatePerObj(void)887 inline void RHO_HEST_REFC::deallocatePerObj(void){
888 ctrl.smpl = NULL;
889 curr.pkdPts = NULL;
890 curr.H = NULL;
891 best.H = NULL;
892 lm.JtJ = NULL;
893 lm.tmp1 = NULL;
894 lm.Jte = NULL;
895
896 mem.perObj.release();
897 }
898
899
900 /**
901 * Initialize SAC for a run given its arguments.
902 *
903 * Performs sanity-checks and memory allocations. Also initializes the state.
904 *
905 * @returns 0 if per-run initialization failed at any point; non-zero
906 * otherwise.
907 *
908 * Reads: arg.*, nr.*
909 * Writes: curr.*, best.*, ctrl.*, eval.*
910 */
911
initRun(void)912 inline int RHO_HEST_REFC::initRun(void){
913 /**
914 * Sanitize arguments.
915 *
916 * Runs zeroth because these are easy-to-check errors and unambiguously
917 * mean something or other.
918 */
919
920 if(!arg.src || !arg.dst){
921 /* Arguments src or dst are insane, must be != NULL */
922 return 0;
923 }
924 if(arg.N < (unsigned)SMPL_SIZE){
925 /* Argument N is insane, must be >= 4. */
926 return 0;
927 }
928 if(arg.maxD < 0){
929 /* Argument maxD is insane, must be >= 0. */
930 return 0;
931 }
932 if(arg.cfd < 0 || arg.cfd > 1){
933 /* Argument cfd is insane, must be in [0, 1]. */
934 return 0;
935 }
936 /* Clamp minInl to 4 or higher. */
937 arg.minInl = arg.minInl < (unsigned)SMPL_SIZE ? SMPL_SIZE : arg.minInl;
938 if(isNREnabled() && (arg.beta <= 0 || arg.beta >= 1)){
939 /* Argument beta is insane, must be in (0, 1). */
940 return 0;
941 }
942 if(!arg.finalH){
943 /* Argument finalH is insane, must be != NULL */
944 return 0;
945 }
946
947 /**
948 * Optional NR setup.
949 *
950 * Runs first because it is decoupled from most other things (*) and if it
951 * fails, it is easy to recover from.
952 *
953 * (*) The only things this code depends on is the flags argument, the nr.*
954 * substruct and the sanity-checked N and beta arguments from above.
955 */
956
957 if(isNREnabled() && !ensureCapacity(arg.N, arg.beta)){
958 return 0;
959 }
960
961 /**
962 * Inlier mask alloc.
963 *
964 * Runs second because we want to quit as fast as possible if we can't even
965 * allocate the two masks.
966 */
967
968 allocatePerRun();
969
970 memset(best.inl, 0, arg.N);
971 memset(curr.inl, 0, arg.N);
972
973 /**
974 * Reset scalar per-run state.
975 *
976 * Runs third because there's no point in resetting/calculating a large
977 * number of fields if something in the above junk failed.
978 */
979
980 ctrl.i = 0;
981 ctrl.phNum = SMPL_SIZE;
982 ctrl.phEndI = 1;
983 ctrl.phEndFpI = sacInitPEndFpI(arg.rConvg, arg.N, SMPL_SIZE);
984 ctrl.phMax = arg.N;
985 ctrl.phNumInl = 0;
986 ctrl.numModels = 0;
987
988 if(haveExtrinsicGuess()){
989 memcpy(curr.H, arg.guessH, HSIZE);
990 }else{
991 memset(curr.H, 0, HSIZE);
992 }
993 curr.numInl = 0;
994
995 memset(best.H, 0, HSIZE);
996 best.numInl = 0;
997
998 eval.Ntested = 0;
999 eval.Ntestedtotal = 0;
1000 eval.good = 1;
1001 eval.t_M = SPRT_T_M;
1002 eval.m_S = SPRT_M_S;
1003 eval.epsilon = SPRT_EPSILON;
1004 eval.delta = SPRT_DELTA;
1005 designSPRTTest();
1006
1007 return 1;
1008 }
1009
1010 /**
1011 * Finalize SAC run.
1012 *
1013 * Deallocates per-run allocatable resources. Currently this consists only of
1014 * the best and current inlier masks, which are equal in size to p->arg.N
1015 * bytes.
1016 *
1017 * Reads: arg.bestInl, curr.inl, best.inl
1018 * Writes: curr.inl, best.inl
1019 */
1020
finiRun(void)1021 inline void RHO_HEST_REFC::finiRun(void){
1022 deallocatePerRun();
1023 }
1024
1025 /**
1026 * Hypothesize a model.
1027 *
1028 * Selects randomly a sample (within the rules of PROSAC) and generates a
1029 * new current model, and applies degeneracy tests to it.
1030 *
1031 * @returns 0 if hypothesized model could be rejected early as degenerate, and
1032 * non-zero otherwise.
1033 */
1034
hypothesize(void)1035 inline int RHO_HEST_REFC::hypothesize(void){
1036 if(PROSACPhaseEndReached()){
1037 PROSACGoToNextPhase();
1038 }
1039
1040 getPROSACSample();
1041 if(isSampleDegenerate()){
1042 return 0;
1043 }
1044
1045 generateModel();
1046 if(isModelDegenerate()){
1047 return 0;
1048 }
1049
1050 return 1;
1051 }
1052
1053 /**
1054 * Verify the hypothesized model.
1055 *
1056 * Given the current model, evaluate its quality. If it is better than
1057 * everything before, save as new best model (and possibly refine it).
1058 *
1059 * Returns 1.
1060 */
1061
verify(void)1062 inline int RHO_HEST_REFC::verify(void){
1063 evaluateModelSPRT();
1064 updateSPRT();
1065
1066 if(isBestModel()){
1067 saveBestModel();
1068
1069 if(isRefineEnabled() && canRefine()){
1070 refine();
1071 }
1072
1073 updateBounds();
1074
1075 if(isNREnabled()){
1076 nStarOptimize();
1077 }
1078 }
1079
1080 return 1;
1081 }
1082
1083 /**
1084 * Check whether extrinsic guess was provided or not.
1085 *
1086 * @return Zero if no extrinsic guess was provided; non-zero otherwiseEE.
1087 */
1088
haveExtrinsicGuess(void)1089 inline int RHO_HEST_REFC::haveExtrinsicGuess(void){
1090 return !!arg.guessH;
1091 }
1092
1093 /**
1094 * Check whether non-randomness criterion is enabled.
1095 *
1096 * @return Zero if non-randomness criterion disabled; non-zero if not.
1097 */
1098
isNREnabled(void)1099 inline int RHO_HEST_REFC::isNREnabled(void){
1100 return arg.flags & RHO_FLAG_ENABLE_NR;
1101 }
1102
1103 /**
1104 * Check whether best-model-so-far refinement is enabled.
1105 *
1106 * @return Zero if best-model-so-far refinement disabled; non-zero if not.
1107 */
1108
isRefineEnabled(void)1109 inline int RHO_HEST_REFC::isRefineEnabled(void){
1110 return arg.flags & RHO_FLAG_ENABLE_REFINEMENT;
1111 }
1112
1113 /**
1114 * Check whether final-model refinement is enabled.
1115 *
1116 * @return Zero if final-model refinement disabled; non-zero if not.
1117 */
1118
isFinalRefineEnabled(void)1119 inline int RHO_HEST_REFC::isFinalRefineEnabled(void){
1120 return arg.flags & RHO_FLAG_ENABLE_FINAL_REFINEMENT;
1121 }
1122
1123 /**
1124 * Computes whether the end of the current PROSAC phase has been reached. At
1125 * PROSAC phase phNum, only matches [0, phNum) are sampled from.
1126 *
1127 * Reads (direct): ctrl.i, ctrl.phEndI, ctrl.phNum, ctrl.phMax
1128 * Reads (callees): None.
1129 * Writes (direct): None.
1130 * Writes (callees): None.
1131 */
1132
PROSACPhaseEndReached(void)1133 inline int RHO_HEST_REFC::PROSACPhaseEndReached(void){
1134 return ctrl.i >= ctrl.phEndI && ctrl.phNum < ctrl.phMax;
1135 }
1136
1137 /**
1138 * Updates unconditionally the necessary fields to move to the next PROSAC
1139 * stage.
1140 *
1141 * Not idempotent.
1142 *
1143 * Reads (direct): ctrl.phNum, ctrl.phEndFpI, ctrl.phEndI
1144 * Reads (callees): None.
1145 * Writes (direct): ctrl.phNum, ctrl.phEndFpI, ctrl.phEndI
1146 * Writes (callees): None.
1147 */
1148
PROSACGoToNextPhase(void)1149 inline void RHO_HEST_REFC::PROSACGoToNextPhase(void){
1150 double next;
1151
1152 ctrl.phNum++;
1153 next = (ctrl.phEndFpI * ctrl.phNum)/(ctrl.phNum - SMPL_SIZE);
1154 ctrl.phEndI += (unsigned)ceil(next - ctrl.phEndFpI);
1155 ctrl.phEndFpI = next;
1156 }
1157
1158 /**
1159 * Get a sample according to PROSAC rules. Namely:
1160 * - If we're past the phase end interation, select randomly 4 out of the first
1161 * phNum matches.
1162 * - Otherwise, select match phNum-1 and select randomly the 3 others out of
1163 * the first phNum-1 matches.
1164 *
1165 * Reads (direct): ctrl.i, ctrl.phEndI, ctrl.phNum
1166 * Reads (callees): prng.s
1167 * Writes (direct): ctrl.smpl
1168 * Writes (callees): prng.s
1169 */
1170
getPROSACSample(void)1171 inline void RHO_HEST_REFC::getPROSACSample(void){
1172 if(ctrl.i > ctrl.phEndI){
1173 /* FIXME: Dubious. Review. */
1174 rndSmpl(4, ctrl.smpl, ctrl.phNum);/* Used to be phMax */
1175 }else{
1176 rndSmpl(3, ctrl.smpl, ctrl.phNum-1);
1177 ctrl.smpl[3] = ctrl.phNum-1;
1178 }
1179 }
1180
1181 /**
1182 * Choose, without repetition, sampleSize integers in the range [0, numDataPoints).
1183 *
1184 * Reads (direct): None.
1185 * Reads (callees): prng.s
1186 * Writes (direct): None.
1187 * Writes (callees): prng.s
1188 */
1189
rndSmpl(unsigned sampleSize,unsigned * currentSample,unsigned dataSetSize)1190 inline void RHO_HEST_REFC::rndSmpl(unsigned sampleSize,
1191 unsigned* currentSample,
1192 unsigned dataSetSize){
1193 /**
1194 * If sampleSize is very close to dataSetSize, we use selection sampling.
1195 * Otherwise we use the naive sampling technique wherein we select random
1196 * indexes until sampleSize of them are distinct.
1197 */
1198
1199 if(sampleSize*2>dataSetSize){
1200 /**
1201 * Selection Sampling:
1202 *
1203 * Algorithm S (Selection sampling technique). To select n records at random from a set of N, where 0 < n ≤ N.
1204 * S1. [Initialize.] Set t ← 0, m ← 0. (During this algorithm, m represents the number of records selected so far,
1205 * and t is the total number of input records that we have dealt with.)
1206 * S2. [Generate U.] Generate a random number U, uniformly distributed between zero and one.
1207 * S3. [Test.] If (N – t)U ≥ n – m, go to step S5.
1208 * S4. [Select.] Select the next record for the sample, and increase m and t by 1. If m < n, go to step S2;
1209 * otherwise the sample is complete and the algorithm terminates.
1210 * S5. [Skip.] Skip the next record (do not include it in the sample), increase t by 1, and go back to step S2.
1211 *
1212 * Replaced m with i and t with j in the below code.
1213 */
1214
1215 unsigned i=0,j=0;
1216
1217 for(i=0;i<sampleSize;j++){
1218 double U=fastRandom();
1219 if((dataSetSize-j)*U < (sampleSize-i)){
1220 currentSample[i++]=j;
1221 }
1222 }
1223 }else{
1224 /**
1225 * Naive sampling technique. Generate indexes until sampleSize of them are distinct.
1226 */
1227
1228 unsigned i, j;
1229 for(i=0;i<sampleSize;i++){
1230 int inList;
1231
1232 do{
1233 currentSample[i] = (unsigned)(dataSetSize*fastRandom());
1234
1235 inList=0;
1236 for(j=0;j<i;j++){
1237 if(currentSample[i] == currentSample[j]){
1238 inList=1;
1239 break;
1240 }
1241 }
1242 }while(inList);
1243 }
1244 }
1245 }
1246
1247 /**
1248 * Checks whether the *sample* is degenerate prior to model generation.
1249 * - First, the extremely cheap numerical degeneracy test is run, which weeds
1250 * out bad samples to the optimized GE implementation.
1251 * - Second, the geometrical degeneracy test is run, which weeds out most other
1252 * bad samples.
1253 *
1254 * Reads (direct): ctrl.smpl, arg.src, arg.dst
1255 * Reads (callees): None.
1256 * Writes (direct): curr.pkdPts
1257 * Writes (callees): None.
1258 */
1259
isSampleDegenerate(void)1260 inline int RHO_HEST_REFC::isSampleDegenerate(void){
1261 unsigned i0 = ctrl.smpl[0], i1 = ctrl.smpl[1], i2 = ctrl.smpl[2], i3 = ctrl.smpl[3];
1262 typedef struct{float x,y;} MyPt2f;
1263 MyPt2f* pkdPts = (MyPt2f*)curr.pkdPts, *src = (MyPt2f*)arg.src, *dst = (MyPt2f*)arg.dst;
1264
1265 /**
1266 * Pack the matches selected by the SAC algorithm.
1267 * Must be packed points[0:7] = {srcx0, srcy0, srcx1, srcy1, srcx2, srcy2, srcx3, srcy3}
1268 * points[8:15] = {dstx0, dsty0, dstx1, dsty1, dstx2, dsty2, dstx3, dsty3}
1269 * Gather 4 points into the vector
1270 */
1271
1272 pkdPts[0] = src[i0];
1273 pkdPts[1] = src[i1];
1274 pkdPts[2] = src[i2];
1275 pkdPts[3] = src[i3];
1276 pkdPts[4] = dst[i0];
1277 pkdPts[5] = dst[i1];
1278 pkdPts[6] = dst[i2];
1279 pkdPts[7] = dst[i3];
1280
1281 /**
1282 * If the matches' source points have common x and y coordinates, abort.
1283 */
1284
1285 if(pkdPts[0].x == pkdPts[1].x || pkdPts[1].x == pkdPts[2].x ||
1286 pkdPts[2].x == pkdPts[3].x || pkdPts[0].x == pkdPts[2].x ||
1287 pkdPts[1].x == pkdPts[3].x || pkdPts[0].x == pkdPts[3].x ||
1288 pkdPts[0].y == pkdPts[1].y || pkdPts[1].y == pkdPts[2].y ||
1289 pkdPts[2].y == pkdPts[3].y || pkdPts[0].y == pkdPts[2].y ||
1290 pkdPts[1].y == pkdPts[3].y || pkdPts[0].y == pkdPts[3].y){
1291 return 1;
1292 }
1293
1294 /* If the matches do not satisfy the strong geometric constraint, abort. */
1295 /* (0 x 1) * 2 */
1296 float cross0s0 = pkdPts[0].y-pkdPts[1].y;
1297 float cross0s1 = pkdPts[1].x-pkdPts[0].x;
1298 float cross0s2 = pkdPts[0].x*pkdPts[1].y-pkdPts[0].y*pkdPts[1].x;
1299 float dots0 = cross0s0*pkdPts[2].x + cross0s1*pkdPts[2].y + cross0s2;
1300 float cross0d0 = pkdPts[4].y-pkdPts[5].y;
1301 float cross0d1 = pkdPts[5].x-pkdPts[4].x;
1302 float cross0d2 = pkdPts[4].x*pkdPts[5].y-pkdPts[4].y*pkdPts[5].x;
1303 float dotd0 = cross0d0*pkdPts[6].x + cross0d1*pkdPts[6].y + cross0d2;
1304 if(((int)dots0^(int)dotd0) < 0){
1305 return 1;
1306 }
1307 /* (0 x 1) * 3 */
1308 float cross1s0 = cross0s0;
1309 float cross1s1 = cross0s1;
1310 float cross1s2 = cross0s2;
1311 float dots1 = cross1s0*pkdPts[3].x + cross1s1*pkdPts[3].y + cross1s2;
1312 float cross1d0 = cross0d0;
1313 float cross1d1 = cross0d1;
1314 float cross1d2 = cross0d2;
1315 float dotd1 = cross1d0*pkdPts[7].x + cross1d1*pkdPts[7].y + cross1d2;
1316 if(((int)dots1^(int)dotd1) < 0){
1317 return 1;
1318 }
1319 /* (2 x 3) * 0 */
1320 float cross2s0 = pkdPts[2].y-pkdPts[3].y;
1321 float cross2s1 = pkdPts[3].x-pkdPts[2].x;
1322 float cross2s2 = pkdPts[2].x*pkdPts[3].y-pkdPts[2].y*pkdPts[3].x;
1323 float dots2 = cross2s0*pkdPts[0].x + cross2s1*pkdPts[0].y + cross2s2;
1324 float cross2d0 = pkdPts[6].y-pkdPts[7].y;
1325 float cross2d1 = pkdPts[7].x-pkdPts[6].x;
1326 float cross2d2 = pkdPts[6].x*pkdPts[7].y-pkdPts[6].y*pkdPts[7].x;
1327 float dotd2 = cross2d0*pkdPts[4].x + cross2d1*pkdPts[4].y + cross2d2;
1328 if(((int)dots2^(int)dotd2) < 0){
1329 return 1;
1330 }
1331 /* (2 x 3) * 1 */
1332 float cross3s0 = cross2s0;
1333 float cross3s1 = cross2s1;
1334 float cross3s2 = cross2s2;
1335 float dots3 = cross3s0*pkdPts[1].x + cross3s1*pkdPts[1].y + cross3s2;
1336 float cross3d0 = cross2d0;
1337 float cross3d1 = cross2d1;
1338 float cross3d2 = cross2d2;
1339 float dotd3 = cross3d0*pkdPts[5].x + cross3d1*pkdPts[5].y + cross3d2;
1340 if(((int)dots3^(int)dotd3) < 0){
1341 return 1;
1342 }
1343
1344 /* Otherwise, accept */
1345 return 0;
1346 }
1347
1348 /**
1349 * Compute homography of matches in gathered, packed sample and output the
1350 * current homography.
1351 *
1352 * Reads (direct): None.
1353 * Reads (callees): curr.pkdPts
1354 * Writes (direct): None.
1355 * Writes (callees): curr.H
1356 */
1357
generateModel(void)1358 inline void RHO_HEST_REFC::generateModel(void){
1359 hFuncRefC(curr.pkdPts, curr.H);
1360 }
1361
1362 /**
1363 * Checks whether the model is itself degenerate.
1364 * - One test: All elements of the homography are added, and if the result is
1365 * NaN the homography is rejected.
1366 *
1367 * Reads (direct): curr.H
1368 * Reads (callees): None.
1369 * Writes (direct): None.
1370 * Writes (callees): None.
1371 */
1372
isModelDegenerate(void)1373 inline int RHO_HEST_REFC::isModelDegenerate(void){
1374 int degenerate;
1375 float* H = curr.H;
1376 float f=H[0]+H[1]+H[2]+H[3]+H[4]+H[5]+H[6]+H[7];
1377
1378 /* degenerate = isnan(f); */
1379 /* degenerate = f!=f;// Only NaN is not equal to itself. */
1380 degenerate = cvIsNaN(f);
1381 /* degenerate = 0; */
1382
1383
1384 return degenerate;
1385 }
1386
1387 /**
1388 * Evaluates the current model using SPRT for early exiting.
1389 *
1390 * Reads (direct): arg.maxD, arg.src, arg.dst, arg.N, curr.inl, curr.H,
1391 * ctrl.numModels, eval.Ntestedtotal, eval.lambdaAccept,
1392 * eval.lambdaReject, eval.A
1393 * Reads (callees): None.
1394 * Writes (direct): ctrl.numModels, curr.numInl, eval.Ntested, eval.good,
1395 * eval.Ntestedtotal
1396 * Writes (callees): None.
1397 */
1398
evaluateModelSPRT(void)1399 inline void RHO_HEST_REFC::evaluateModelSPRT(void){
1400 unsigned i;
1401 unsigned isInlier;
1402 double lambda = 1.0;
1403 float distSq = arg.maxD*arg.maxD;
1404 const float* src = arg.src;
1405 const float* dst = arg.dst;
1406 char* inl = curr.inl;
1407 const float* H = curr.H;
1408
1409
1410 ctrl.numModels++;
1411
1412 curr.numInl = 0;
1413 eval.Ntested = 0;
1414 eval.good = 1;
1415
1416
1417 /* SCALAR */
1418 for(i=0;i<arg.N && eval.good;i++){
1419 /* Backproject */
1420 float x=src[i*2],y=src[i*2+1];
1421 float X=dst[i*2],Y=dst[i*2+1];
1422
1423 float reprojX=H[0]*x+H[1]*y+H[2]; /* ( X_1 ) ( H_11 H_12 H_13 ) (x_1) */
1424 float reprojY=H[3]*x+H[4]*y+H[5]; /* ( X_2 ) = ( H_21 H_22 H_23 ) (x_2) */
1425 float reprojZ=H[6]*x+H[7]*y+1.0f; /* ( X_3 ) ( H_31 H_32 H_33=1.0 ) (x_3 = 1.0) */
1426
1427 /* reproj is in homogeneous coordinates. To bring back to "regular" coordinates, divide by Z. */
1428 reprojX/=reprojZ;
1429 reprojY/=reprojZ;
1430
1431 /* Compute distance */
1432 reprojX-=X;
1433 reprojY-=Y;
1434 reprojX*=reprojX;
1435 reprojY*=reprojY;
1436 float reprojDist = reprojX+reprojY;
1437
1438 /* ... */
1439 isInlier = reprojDist <= distSq;
1440 curr.numInl += isInlier;
1441 *inl++ = (char)isInlier;
1442
1443
1444 /* SPRT */
1445 lambda *= isInlier ? eval.lambdaAccept : eval.lambdaReject;
1446 eval.good = lambda <= eval.A;
1447 /* If !good, the threshold A was exceeded, so we're rejecting */
1448 }
1449
1450
1451 eval.Ntested = i;
1452 eval.Ntestedtotal += i;
1453 }
1454
1455 /**
1456 * Update either the delta or epsilon SPRT parameters, depending on the events
1457 * that transpired in the previous evaluation.
1458 *
1459 * Reads (direct): eval.good, curr.numInl, arg.N, eval.Ntested, eval.delta
1460 * Reads (callees): eval.delta, eval.epsilon, eval.t_M, eval.m_S
1461 * Writes (direct): eval.epsilon, eval.delta
1462 * Writes (callees): eval.A, eval.lambdaReject, eval.lambdaAccept
1463 */
1464
updateSPRT(void)1465 inline void RHO_HEST_REFC::updateSPRT(void){
1466 if(eval.good){
1467 if(isBestModel()){
1468 eval.epsilon = (double)curr.numInl/arg.N;
1469 designSPRTTest();
1470 }
1471 }else{
1472 double newDelta = (double)curr.numInl/eval.Ntested;
1473
1474 if(newDelta > 0){
1475 double relChange = fabs(eval.delta - newDelta)/ eval.delta;
1476 if(relChange > MIN_DELTA_CHNG){
1477 eval.delta = newDelta;
1478 designSPRTTest();
1479 }
1480 }
1481 }
1482 }
1483
1484 /**
1485 * Numerically compute threshold A from the estimated delta, epsilon, t_M and
1486 * m_S values.
1487 *
1488 * Epsilon: Denotes the probability that a randomly chosen data point is
1489 * consistent with a good model.
1490 * Delta: Denotes the probability that a randomly chosen data point is
1491 * consistent with a bad model.
1492 * t_M: Time needed to instantiate a model hypotheses given a sample.
1493 * (Computing model parameters from a sample takes the same time
1494 * as verification of t_M data points)
1495 * m_S: The number of models that are verified per sample.
1496 */
1497
sacDesignSPRTTest(double delta,double epsilon,double t_M,double m_S)1498 static inline double sacDesignSPRTTest(double delta, double epsilon, double t_M, double m_S){
1499 double An, C, K, prevAn;
1500 unsigned i;
1501
1502 /**
1503 * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005
1504 * Eq (2)
1505 */
1506
1507 C = (1-delta) * log((1-delta)/(1-epsilon)) +
1508 delta * log( delta / epsilon );
1509
1510 /**
1511 * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005
1512 * Eq (6)
1513 * K = K_1/K_2 + 1 = (t_M*C)/m_S + 1
1514 */
1515
1516 K = t_M*C/m_S + 1;
1517
1518 /**
1519 * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005
1520 * Paragraph below Eq (6)
1521 *
1522 * A* = lim_{n -> infty} A_n, where
1523 * A_0 = K1/K2 + 1 and
1524 * A_{n+1} = K1/K2 + 1 + log(A_n)
1525 * The series converges fast, typically within four iterations.
1526 */
1527
1528 An = K;
1529 i = 0;
1530
1531 do{
1532 prevAn = An;
1533 An = K + log(An);
1534 }while((An-prevAn > 1.5e-8) && (++i < 10));
1535
1536 /**
1537 * Return A = An_stopping, with n_stopping < 10
1538 */
1539
1540 return An;
1541 }
1542
1543 /**
1544 * Design the SPRT test. Shorthand for
1545 * A = sprt(delta, epsilon, t_M, m_S);
1546 *
1547 * Idempotent.
1548 *
1549 * Reads (direct): eval.delta, eval.epsilon, eval.t_M, eval.m_S
1550 * Reads (callees): None.
1551 * Writes (direct): eval.A, eval.lambdaReject, eval.lambdaAccept.
1552 * Writes (callees): None.
1553 */
1554
designSPRTTest(void)1555 inline void RHO_HEST_REFC::designSPRTTest(void){
1556 eval.A = sacDesignSPRTTest(eval.delta, eval.epsilon, eval.t_M, eval.m_S);
1557 eval.lambdaReject = ((1.0 - eval.delta) / (1.0 - eval.epsilon));
1558 eval.lambdaAccept = (( eval.delta ) / ( eval.epsilon ));
1559 }
1560
1561 /**
1562 * Return whether the current model is the best model so far.
1563 *
1564 * @return Non-zero if this is the model with the most inliers seen so far;
1565 * 0 otherwise.
1566 *
1567 * Reads (direct): curr.numInl, best.numInl
1568 * Reads (callees): None.
1569 * Writes (direct): None.
1570 * Writes (callees): None.
1571 */
1572
isBestModel(void)1573 inline int RHO_HEST_REFC::isBestModel(void){
1574 return curr.numInl > best.numInl;
1575 }
1576
1577 /**
1578 * Returns whether the current-best model is good enough to be an
1579 * acceptable best model, by checking whether it meets the minimum
1580 * number of inliers.
1581 *
1582 * @return Non-zero if the current model is "good enough" to save;
1583 * 0 otherwise.
1584 *
1585 * Reads (direct): best.numInl, arg.minInl
1586 * Reads (callees): None.
1587 * Writes (direct): None.
1588 * Writes (callees): None.
1589 */
1590
isBestModelGoodEnough(void)1591 inline int RHO_HEST_REFC::isBestModelGoodEnough(void){
1592 return best.numInl >= arg.minInl;
1593 }
1594
1595 /**
1596 * Make current model new best model by swapping the homography, inlier mask
1597 * and count of inliers between the current and best models.
1598 *
1599 * Reads (direct): curr.H, curr.inl, curr.numInl,
1600 * best.H, best.inl, best.numInl
1601 * Reads (callees): None.
1602 * Writes (direct): curr.H, curr.inl, curr.numInl,
1603 * best.H, best.inl, best.numInl
1604 * Writes (callees): None.
1605 */
1606
saveBestModel(void)1607 inline void RHO_HEST_REFC::saveBestModel(void){
1608 float* H = curr.H;
1609 char* inl = curr.inl;
1610 unsigned numInl = curr.numInl;
1611
1612 curr.H = best.H;
1613 curr.inl = best.inl;
1614 curr.numInl = best.numInl;
1615
1616 best.H = H;
1617 best.inl = inl;
1618 best.numInl = numInl;
1619 }
1620
1621 /**
1622 * Compute NR table entries [start, N) for given beta.
1623 */
1624
sacInitNonRand(double beta,unsigned start,unsigned N,unsigned * nonRandMinInl)1625 static inline void sacInitNonRand(double beta,
1626 unsigned start,
1627 unsigned N,
1628 unsigned* nonRandMinInl){
1629 unsigned n = SMPL_SIZE+1 > start ? SMPL_SIZE+1 : start;
1630 double beta_beta1_sq_chi = sqrt(beta*(1.0-beta)) * CHI_SQ;
1631
1632 for(; n < N; n++){
1633 double mu = n * beta;
1634 double sigma = sqrt((double)n)* beta_beta1_sq_chi;
1635 unsigned i_min = (unsigned)ceil(SMPL_SIZE + mu + sigma);
1636
1637 nonRandMinInl[n] = i_min;
1638 }
1639 }
1640
1641 /**
1642 * Optimize the stopping criterion to account for the non-randomness criterion
1643 * of PROSAC.
1644 *
1645 * Reads (direct): arg.N, best.numInl, nr.tbl, arg.inl, ctrl.phMax,
1646 * ctrl.phNumInl, arg.cfd, arg.maxI
1647 * Reads (callees): None.
1648 * Writes (direct): arg.maxI, ctrl.phMax, ctrl.phNumInl
1649 * Writes (callees): None.
1650 */
1651
nStarOptimize(void)1652 inline void RHO_HEST_REFC::nStarOptimize(void){
1653 unsigned min_sample_length = 10*2; /*(N * INLIERS_RATIO) */
1654 unsigned best_n = arg.N;
1655 unsigned test_n = best_n;
1656 unsigned bestNumInl = best.numInl;
1657 unsigned testNumInl = bestNumInl;
1658
1659 for(;test_n > min_sample_length && testNumInl;test_n--){
1660 if(testNumInl*best_n > bestNumInl*test_n){
1661 if(testNumInl < nr.tbl[test_n]){
1662 break;
1663 }
1664 best_n = test_n;
1665 bestNumInl = testNumInl;
1666 }
1667 testNumInl -= !!best.inl[test_n-1];
1668 }
1669
1670 if(bestNumInl*ctrl.phMax > ctrl.phNumInl*best_n){
1671 ctrl.phMax = best_n;
1672 ctrl.phNumInl = bestNumInl;
1673 arg.maxI = sacCalcIterBound(arg.cfd,
1674 (double)ctrl.phNumInl/ctrl.phMax,
1675 SMPL_SIZE,
1676 arg.maxI);
1677 }
1678 }
1679
1680 /**
1681 * Classic RANSAC iteration bound based on largest # of inliers.
1682 *
1683 * Reads (direct): arg.maxI, arg.cfd, best.numInl, arg.N
1684 * Reads (callees): None.
1685 * Writes (direct): arg.maxI
1686 * Writes (callees): None.
1687 */
1688
updateBounds(void)1689 inline void RHO_HEST_REFC::updateBounds(void){
1690 arg.maxI = sacCalcIterBound(arg.cfd,
1691 (double)best.numInl/arg.N,
1692 SMPL_SIZE,
1693 arg.maxI);
1694 }
1695
1696 /**
1697 * Ouput the best model so far to the output argument.
1698 *
1699 * Reads (direct): arg.finalH, best.H, arg.inl, best.inl, arg.N
1700 * Reads (callees): arg.finalH, arg.inl, arg.N
1701 * Writes (direct): arg.finalH, arg.inl
1702 * Writes (callees): arg.finalH, arg.inl
1703 */
1704
outputModel(void)1705 inline void RHO_HEST_REFC::outputModel(void){
1706 if(isBestModelGoodEnough()){
1707 memcpy(arg.finalH, best.H, HSIZE);
1708 if(arg.inl){
1709 memcpy(arg.inl, best.inl, arg.N);
1710 }
1711 }else{
1712 outputZeroH();
1713 }
1714 }
1715
1716 /**
1717 * Ouput a zeroed H to the output argument.
1718 *
1719 * Reads (direct): arg.finalH, arg.inl, arg.N
1720 * Reads (callees): None.
1721 * Writes (direct): arg.finalH, arg.inl
1722 * Writes (callees): None.
1723 */
1724
outputZeroH(void)1725 inline void RHO_HEST_REFC::outputZeroH(void){
1726 if(arg.finalH){
1727 memset(arg.finalH, 0, HSIZE);
1728 }
1729 if(arg.inl){
1730 memset(arg.inl, 0, arg.N);
1731 }
1732 }
1733
1734 /**
1735 * Compute the real-valued number of samples per phase, given the RANSAC convergence speed,
1736 * data set size and sample size.
1737 */
1738
sacInitPEndFpI(const unsigned ransacConvg,const unsigned n,const unsigned s)1739 static inline double sacInitPEndFpI(const unsigned ransacConvg,
1740 const unsigned n,
1741 const unsigned s){
1742 double numer=1, denom=1;
1743
1744 unsigned i;
1745 for(i=0;i<s;i++){
1746 numer *= s-i;
1747 denom *= n-i;
1748 }
1749
1750 return ransacConvg*numer/denom;
1751 }
1752
1753 /**
1754 * Estimate the number of iterations required based on the requested confidence,
1755 * proportion of inliers in the best model so far and sample size.
1756 *
1757 * Clamp return value at maxIterationBound.
1758 */
1759
sacCalcIterBound(double confidence,double inlierRate,unsigned sampleSize,unsigned maxIterBound)1760 static inline unsigned sacCalcIterBound(double confidence,
1761 double inlierRate,
1762 unsigned sampleSize,
1763 unsigned maxIterBound){
1764 unsigned retVal;
1765
1766 /**
1767 * Formula chosen from http://en.wikipedia.org/wiki/RANSAC#The_parameters :
1768 *
1769 * \[ k = \frac{\log{(1-confidence)}}{\log{(1-inlierRate**sampleSize)}} \]
1770 */
1771
1772 double atLeastOneOutlierProbability = 1.-pow(inlierRate, (double)sampleSize);
1773
1774 /**
1775 * There are two special cases: When argument to log() is 0 and when it is 1.
1776 * Each has a special meaning.
1777 */
1778
1779 if(atLeastOneOutlierProbability>=1.){
1780 /**
1781 * A certainty of picking at least one outlier means that we will need
1782 * an infinite amount of iterations in order to find a correct solution.
1783 */
1784
1785 retVal = maxIterBound;
1786 }else if(atLeastOneOutlierProbability<=0.){
1787 /**
1788 * The certainty of NOT picking an outlier means that only 1 iteration
1789 * is needed to find a solution.
1790 */
1791
1792 retVal = 1;
1793 }else{
1794 /**
1795 * Since 1-confidence (the probability of the model being based on at
1796 * least one outlier in the data) is equal to
1797 * (1-inlierRate**sampleSize)**numIterations (the probability of picking
1798 * at least one outlier in numIterations samples), we can isolate
1799 * numIterations (the return value) into
1800 */
1801
1802 retVal = (unsigned)ceil(log(1.-confidence)/log(atLeastOneOutlierProbability));
1803 }
1804
1805 /**
1806 * Clamp to maxIterationBound.
1807 */
1808
1809 return retVal <= maxIterBound ? retVal : maxIterBound;
1810 }
1811
1812
1813 /**
1814 * Given 4 matches, computes the homography that relates them using Gaussian
1815 * Elimination. The row operations are as given in the paper.
1816 *
1817 * TODO: Clean this up. The code is hideous, and might even conceal sign bugs
1818 * (specifically relating to whether the last column should be negated,
1819 * or not).
1820 */
1821
hFuncRefC(float * packedPoints,float * H)1822 static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates) points followed by
1823 destination (four x,y float coordinates) points, aligned by 32 bytes */
1824 float* H){ /* Homography (three 16-byte aligned rows of 3 floats) */
1825 float x0=*packedPoints++;
1826 float y0=*packedPoints++;
1827 float x1=*packedPoints++;
1828 float y1=*packedPoints++;
1829 float x2=*packedPoints++;
1830 float y2=*packedPoints++;
1831 float x3=*packedPoints++;
1832 float y3=*packedPoints++;
1833 float X0=*packedPoints++;
1834 float Y0=*packedPoints++;
1835 float X1=*packedPoints++;
1836 float Y1=*packedPoints++;
1837 float X2=*packedPoints++;
1838 float Y2=*packedPoints++;
1839 float X3=*packedPoints++;
1840 float Y3=*packedPoints++;
1841
1842 float x0X0=x0*X0, x1X1=x1*X1, x2X2=x2*X2, x3X3=x3*X3;
1843 float x0Y0=x0*Y0, x1Y1=x1*Y1, x2Y2=x2*Y2, x3Y3=x3*Y3;
1844 float y0X0=y0*X0, y1X1=y1*X1, y2X2=y2*X2, y3X3=y3*X3;
1845 float y0Y0=y0*Y0, y1Y1=y1*Y1, y2Y2=y2*Y2, y3Y3=y3*Y3;
1846
1847
1848 /**
1849 * [0] [1] Hidden Prec
1850 * x0 y0 1 x1
1851 * x1 y1 1 x1
1852 * x2 y2 1 x1
1853 * x3 y3 1 x1
1854 *
1855 * Eliminate ones in column 2 and 5.
1856 * R(0)-=R(2)
1857 * R(1)-=R(2)
1858 * R(3)-=R(2)
1859 *
1860 * [0] [1] Hidden Prec
1861 * x0-x2 y0-y2 0 x1+1
1862 * x1-x2 y1-y2 0 x1+1
1863 * x2 y2 1 x1
1864 * x3-x2 y3-y2 0 x1+1
1865 *
1866 * Eliminate column 0 of rows 1 and 3
1867 * R(1)=(x0-x2)*R(1)-(x1-x2)*R(0), y1'=(y1-y2)(x0-x2)-(x1-x2)(y0-y2)
1868 * R(3)=(x0-x2)*R(3)-(x3-x2)*R(0), y3'=(y3-y2)(x0-x2)-(x3-x2)(y0-y2)
1869 *
1870 * [0] [1] Hidden Prec
1871 * x0-x2 y0-y2 0 x1+1
1872 * 0 y1' 0 x2+3
1873 * x2 y2 1 x1
1874 * 0 y3' 0 x2+3
1875 *
1876 * Eliminate column 1 of rows 0 and 3
1877 * R(3)=y1'*R(3)-y3'*R(1)
1878 * R(0)=y1'*R(0)-(y0-y2)*R(1)
1879 *
1880 * [0] [1] Hidden Prec
1881 * x0' 0 0 x3+5
1882 * 0 y1' 0 x2+3
1883 * x2 y2 1 x1
1884 * 0 0 0 x4+7
1885 *
1886 * Eliminate columns 0 and 1 of row 2
1887 * R(0)/=x0'
1888 * R(1)/=y1'
1889 * R(2)-= (x2*R(0) + y2*R(1))
1890 *
1891 * [0] [1] Hidden Prec
1892 * 1 0 0 x6+10
1893 * 0 1 0 x4+6
1894 * 0 0 1 x4+7
1895 * 0 0 0 x4+7
1896 */
1897
1898 /**
1899 * Eliminate ones in column 2 and 5.
1900 * R(0)-=R(2)
1901 * R(1)-=R(2)
1902 * R(3)-=R(2)
1903 */
1904
1905 /*float minor[4][2] = {{x0-x2,y0-y2},
1906 {x1-x2,y1-y2},
1907 {x2 ,y2 },
1908 {x3-x2,y3-y2}};*/
1909 /*float major[8][3] = {{x2X2-x0X0,y2X2-y0X0,(X0-X2)},
1910 {x2X2-x1X1,y2X2-y1X1,(X1-X2)},
1911 {-x2X2 ,-y2X2 ,(X2 )},
1912 {x2X2-x3X3,y2X2-y3X3,(X3-X2)},
1913 {x2Y2-x0Y0,y2Y2-y0Y0,(Y0-Y2)},
1914 {x2Y2-x1Y1,y2Y2-y1Y1,(Y1-Y2)},
1915 {-x2Y2 ,-y2Y2 ,(Y2 )},
1916 {x2Y2-x3Y3,y2Y2-y3Y3,(Y3-Y2)}};*/
1917 float minor[2][4] = {{x0-x2,x1-x2,x2 ,x3-x2},
1918 {y0-y2,y1-y2,y2 ,y3-y2}};
1919 float major[3][8] = {{x2X2-x0X0,x2X2-x1X1,-x2X2 ,x2X2-x3X3,x2Y2-x0Y0,x2Y2-x1Y1,-x2Y2 ,x2Y2-x3Y3},
1920 {y2X2-y0X0,y2X2-y1X1,-y2X2 ,y2X2-y3X3,y2Y2-y0Y0,y2Y2-y1Y1,-y2Y2 ,y2Y2-y3Y3},
1921 { (X0-X2) , (X1-X2) , (X2 ) , (X3-X2) , (Y0-Y2) , (Y1-Y2) , (Y2 ) , (Y3-Y2) }};
1922
1923 /**
1924 * int i;
1925 * for(i=0;i<8;i++) major[2][i]=-major[2][i];
1926 * Eliminate column 0 of rows 1 and 3
1927 * R(1)=(x0-x2)*R(1)-(x1-x2)*R(0), y1'=(y1-y2)(x0-x2)-(x1-x2)(y0-y2)
1928 * R(3)=(x0-x2)*R(3)-(x3-x2)*R(0), y3'=(y3-y2)(x0-x2)-(x3-x2)(y0-y2)
1929 */
1930
1931 float scalar1=minor[0][0], scalar2=minor[0][1];
1932 minor[1][1]=minor[1][1]*scalar1-minor[1][0]*scalar2;
1933
1934 major[0][1]=major[0][1]*scalar1-major[0][0]*scalar2;
1935 major[1][1]=major[1][1]*scalar1-major[1][0]*scalar2;
1936 major[2][1]=major[2][1]*scalar1-major[2][0]*scalar2;
1937
1938 major[0][5]=major[0][5]*scalar1-major[0][4]*scalar2;
1939 major[1][5]=major[1][5]*scalar1-major[1][4]*scalar2;
1940 major[2][5]=major[2][5]*scalar1-major[2][4]*scalar2;
1941
1942 scalar2=minor[0][3];
1943 minor[1][3]=minor[1][3]*scalar1-minor[1][0]*scalar2;
1944
1945 major[0][3]=major[0][3]*scalar1-major[0][0]*scalar2;
1946 major[1][3]=major[1][3]*scalar1-major[1][0]*scalar2;
1947 major[2][3]=major[2][3]*scalar1-major[2][0]*scalar2;
1948
1949 major[0][7]=major[0][7]*scalar1-major[0][4]*scalar2;
1950 major[1][7]=major[1][7]*scalar1-major[1][4]*scalar2;
1951 major[2][7]=major[2][7]*scalar1-major[2][4]*scalar2;
1952
1953 /**
1954 * Eliminate column 1 of rows 0 and 3
1955 * R(3)=y1'*R(3)-y3'*R(1)
1956 * R(0)=y1'*R(0)-(y0-y2)*R(1)
1957 */
1958
1959 scalar1=minor[1][1];scalar2=minor[1][3];
1960 major[0][3]=major[0][3]*scalar1-major[0][1]*scalar2;
1961 major[1][3]=major[1][3]*scalar1-major[1][1]*scalar2;
1962 major[2][3]=major[2][3]*scalar1-major[2][1]*scalar2;
1963
1964 major[0][7]=major[0][7]*scalar1-major[0][5]*scalar2;
1965 major[1][7]=major[1][7]*scalar1-major[1][5]*scalar2;
1966 major[2][7]=major[2][7]*scalar1-major[2][5]*scalar2;
1967
1968 scalar2=minor[1][0];
1969 minor[0][0]=minor[0][0]*scalar1-minor[0][1]*scalar2;
1970
1971 major[0][0]=major[0][0]*scalar1-major[0][1]*scalar2;
1972 major[1][0]=major[1][0]*scalar1-major[1][1]*scalar2;
1973 major[2][0]=major[2][0]*scalar1-major[2][1]*scalar2;
1974
1975 major[0][4]=major[0][4]*scalar1-major[0][5]*scalar2;
1976 major[1][4]=major[1][4]*scalar1-major[1][5]*scalar2;
1977 major[2][4]=major[2][4]*scalar1-major[2][5]*scalar2;
1978
1979 /**
1980 * Eliminate columns 0 and 1 of row 2
1981 * R(0)/=x0'
1982 * R(1)/=y1'
1983 * R(2)-= (x2*R(0) + y2*R(1))
1984 */
1985
1986 scalar1=1.0f/minor[0][0];
1987 major[0][0]*=scalar1;
1988 major[1][0]*=scalar1;
1989 major[2][0]*=scalar1;
1990 major[0][4]*=scalar1;
1991 major[1][4]*=scalar1;
1992 major[2][4]*=scalar1;
1993
1994 scalar1=1.0f/minor[1][1];
1995 major[0][1]*=scalar1;
1996 major[1][1]*=scalar1;
1997 major[2][1]*=scalar1;
1998 major[0][5]*=scalar1;
1999 major[1][5]*=scalar1;
2000 major[2][5]*=scalar1;
2001
2002
2003 scalar1=minor[0][2];scalar2=minor[1][2];
2004 major[0][2]-=major[0][0]*scalar1+major[0][1]*scalar2;
2005 major[1][2]-=major[1][0]*scalar1+major[1][1]*scalar2;
2006 major[2][2]-=major[2][0]*scalar1+major[2][1]*scalar2;
2007
2008 major[0][6]-=major[0][4]*scalar1+major[0][5]*scalar2;
2009 major[1][6]-=major[1][4]*scalar1+major[1][5]*scalar2;
2010 major[2][6]-=major[2][4]*scalar1+major[2][5]*scalar2;
2011
2012 /* Only major matters now. R(3) and R(7) correspond to the hollowed-out rows. */
2013 scalar1=major[0][7];
2014 major[1][7]/=scalar1;
2015 major[2][7]/=scalar1;
2016
2017 scalar1=major[0][0];major[1][0]-=scalar1*major[1][7];major[2][0]-=scalar1*major[2][7];
2018 scalar1=major[0][1];major[1][1]-=scalar1*major[1][7];major[2][1]-=scalar1*major[2][7];
2019 scalar1=major[0][2];major[1][2]-=scalar1*major[1][7];major[2][2]-=scalar1*major[2][7];
2020 scalar1=major[0][3];major[1][3]-=scalar1*major[1][7];major[2][3]-=scalar1*major[2][7];
2021 scalar1=major[0][4];major[1][4]-=scalar1*major[1][7];major[2][4]-=scalar1*major[2][7];
2022 scalar1=major[0][5];major[1][5]-=scalar1*major[1][7];major[2][5]-=scalar1*major[2][7];
2023 scalar1=major[0][6];major[1][6]-=scalar1*major[1][7];major[2][6]-=scalar1*major[2][7];
2024
2025
2026 /* One column left (Two in fact, but the last one is the homography) */
2027 scalar1=major[1][3];
2028
2029 major[2][3]/=scalar1;
2030 scalar1=major[1][0];major[2][0]-=scalar1*major[2][3];
2031 scalar1=major[1][1];major[2][1]-=scalar1*major[2][3];
2032 scalar1=major[1][2];major[2][2]-=scalar1*major[2][3];
2033 scalar1=major[1][4];major[2][4]-=scalar1*major[2][3];
2034 scalar1=major[1][5];major[2][5]-=scalar1*major[2][3];
2035 scalar1=major[1][6];major[2][6]-=scalar1*major[2][3];
2036 scalar1=major[1][7];major[2][7]-=scalar1*major[2][3];
2037
2038
2039 /* Homography is done. */
2040 H[0]=major[2][0];
2041 H[1]=major[2][1];
2042 H[2]=major[2][2];
2043
2044 H[3]=major[2][4];
2045 H[4]=major[2][5];
2046 H[5]=major[2][6];
2047
2048 H[6]=major[2][7];
2049 H[7]=major[2][3];
2050 H[8]=1.0;
2051 }
2052
2053
2054 /**
2055 * Returns whether refinement is possible.
2056 *
2057 * NB This is separate from whether it is *enabled*.
2058 *
2059 * @return 0 if refinement isn't possible, non-zero otherwise.
2060 *
2061 * Reads (direct): best.numInl
2062 * Reads (callees): None.
2063 * Writes (direct): None.
2064 * Writes (callees): None.
2065 */
2066
canRefine(void)2067 inline int RHO_HEST_REFC::canRefine(void){
2068 /**
2069 * If we only have 4 matches, GE's result is already optimal and cannot
2070 * be refined any further.
2071 */
2072
2073 return best.numInl > (unsigned)SMPL_SIZE;
2074 }
2075
2076
2077 /**
2078 * Refines the best-so-far homography (p->best.H).
2079 *
2080 * Reads (direct): best.H, arg.src, arg.dst, best.inl, arg.N, lm.JtJ,
2081 * lm.Jte, lm.tmp1
2082 * Reads (callees): None.
2083 * Writes (direct): best.H, lm.JtJ, lm.Jte, lm.tmp1
2084 * Writes (callees): None.
2085 */
2086
refine(void)2087 inline void RHO_HEST_REFC::refine(void){
2088 int i;
2089 float S, newS; /* Sum of squared errors */
2090 float gain; /* Gain-parameter. */
2091 float L = 100.0f;/* Lambda of LevMarq */
2092 float dH[8], newH[8];
2093
2094 /**
2095 * Iteratively refine the homography.
2096 */
2097 /* Find initial conditions */
2098 sacCalcJacobianErrors(best.H, arg.src, arg.dst, best.inl, arg.N,
2099 lm.JtJ, lm.Jte, &S);
2100
2101 /*Levenberg-Marquardt Loop.*/
2102 for(i=0;i<MAXLEVMARQITERS;i++){
2103 /**
2104 * Attempt a step given current state
2105 * - Jacobian-x-Jacobian (JtJ)
2106 * - Jacobian-x-error (Jte)
2107 * - Sum of squared errors (S)
2108 * and current parameter
2109 * - Lambda (L)
2110 * .
2111 *
2112 * This is done by solving the system of equations
2113 * Ax = b
2114 * where A (JtJ) and b (Jte) are sourced from our current state, and
2115 * the solution x becomes a step (dH) that is applied to best.H in
2116 * order to compute a candidate homography (newH).
2117 *
2118 * The system above is solved by Cholesky decomposition of a
2119 * sufficently-damped JtJ into a lower-triangular matrix (and its
2120 * transpose), whose inverse is then computed. This inverse (and its
2121 * transpose) then multiply Jte in order to find dH.
2122 */
2123
2124 while(!sacChol8x8Damped(lm.JtJ, L, lm.tmp1)){
2125 L *= 2.0f;
2126 }
2127 sacTRInv8x8 (lm.tmp1, lm.tmp1);
2128 sacTRISolve8x8(lm.tmp1, lm.Jte, dH);
2129 sacSub8x1 (newH, best.H, dH);
2130 sacCalcJacobianErrors(newH, arg.src, arg.dst, best.inl, arg.N,
2131 NULL, NULL, &newS);
2132 gain = sacLMGain(dH, lm.Jte, S, newS, L);
2133 /*printf("Lambda: %12.6f S: %12.6f newS: %12.6f Gain: %12.6f\n",
2134 L, S, newS, gain);*/
2135
2136 /**
2137 * If the gain is positive (i.e., the new Sum of Square Errors (newS)
2138 * corresponding to newH is lower than the previous one (S) ), save
2139 * the current state and accept the new step dH.
2140 *
2141 * If the gain is below LM_GAIN_LO, damp more (increase L), even if the
2142 * gain was positive. If the gain is above LM_GAIN_HI, damp less
2143 * (decrease L). Otherwise the gain is left unchanged.
2144 */
2145
2146 if(gain < LM_GAIN_LO){
2147 L *= 8;
2148 if(L>1000.0f/FLT_EPSILON){
2149 break;/* FIXME: Most naive termination criterion imaginable. */
2150 }
2151 }else if(gain > LM_GAIN_HI){
2152 L *= 0.5;
2153 }
2154
2155 if(gain > 0){
2156 S = newS;
2157 memcpy(best.H, newH, sizeof(newH));
2158 sacCalcJacobianErrors(best.H, arg.src, arg.dst, best.inl, arg.N,
2159 lm.JtJ, lm.Jte, &S);
2160 }
2161 }
2162 }
2163
2164
2165 /**
2166 * Compute directly the JtJ, Jte and sum-of-squared-error for a given
2167 * homography and set of inliers.
2168 *
2169 * This is possible because the product of J and its transpose as well as with
2170 * the error and the sum-of-squared-error can all be computed additively
2171 * (match-by-match), as one would intuitively expect; All matches make
2172 * contributions to the error independently of each other.
2173 *
2174 * What this allows is a constant-space implementation of Lev-Marq that can
2175 * nevertheless be vectorized if need be.
2176 */
2177
sacCalcJacobianErrors(const float * H,const float * src,const float * dst,const char * inl,unsigned N,float (* JtJ)[8],float * Jte,float * Sp)2178 static inline void sacCalcJacobianErrors(const float* H,
2179 const float* src,
2180 const float* dst,
2181 const char* inl,
2182 unsigned N,
2183 float (* JtJ)[8],
2184 float* Jte,
2185 float* Sp){
2186 unsigned i;
2187 float S;
2188
2189 /* Zero out JtJ, Jte and S */
2190 if(JtJ){memset(JtJ, 0, 8*8*sizeof(float));}
2191 if(Jte){memset(Jte, 0, 8*1*sizeof(float));}
2192 S = 0.0f;
2193
2194 /* Additively compute JtJ and Jte */
2195 for(i=0;i<N;i++){
2196 /* Skip outliers */
2197 if(!inl[i]){
2198 continue;
2199 }
2200
2201 /**
2202 * Otherwise, compute additively the upper triangular matrix JtJ and
2203 * the Jtd vector within the following formula:
2204 *
2205 * LaTeX:
2206 * (J^{T}J + \lambda \diag( J^{T}J )) \beta = J^{T}[ y - f(\Beta) ]
2207 * Simplified ASCII:
2208 * (JtJ + L*diag(JtJ)) beta = Jt e, where e (error) is y-f(Beta).
2209 *
2210 * For this we need to calculate
2211 * 1) The 2D error (e) of the homography on the current point i
2212 * using the current parameters Beta.
2213 * 2) The derivatives (J) of the error on the current point i under
2214 * perturbations of the current parameters Beta.
2215 * Accumulate products of the error times the derivative to Jte, and
2216 * products of the derivatives to JtJ.
2217 */
2218
2219 /* Compute Squared Error */
2220 float x = src[2*i+0];
2221 float y = src[2*i+1];
2222 float X = dst[2*i+0];
2223 float Y = dst[2*i+1];
2224 float W = (H[6]*x + H[7]*y + 1.0f);
2225 float iW = fabs(W) > FLT_EPSILON ? 1.0f/W : 0;
2226
2227 float reprojX = (H[0]*x + H[1]*y + H[2]) * iW;
2228 float reprojY = (H[3]*x + H[4]*y + H[5]) * iW;
2229
2230 float eX = reprojX - X;
2231 float eY = reprojY - Y;
2232 float e = eX*eX + eY*eY;
2233 S += e;
2234
2235 /* Compute Jacobian */
2236 if(JtJ || Jte){
2237 float dxh11 = x * iW;
2238 float dxh12 = y * iW;
2239 float dxh13 = iW;
2240 /*float dxh21 = 0.0f;*/
2241 /*float dxh22 = 0.0f;*/
2242 /*float dxh23 = 0.0f;*/
2243 float dxh31 = -reprojX*x * iW;
2244 float dxh32 = -reprojX*y * iW;
2245
2246 /*float dyh11 = 0.0f;*/
2247 /*float dyh12 = 0.0f;*/
2248 /*float dyh13 = 0.0f;*/
2249 float dyh21 = x * iW;
2250 float dyh22 = y * iW;
2251 float dyh23 = iW;
2252 float dyh31 = -reprojY*x * iW;
2253 float dyh32 = -reprojY*y * iW;
2254
2255 /* Update Jte: X Y */
2256 if(Jte){
2257 Jte[0] += eX *dxh11 ;/* +0 */
2258 Jte[1] += eX *dxh12 ;/* +0 */
2259 Jte[2] += eX *dxh13 ;/* +0 */
2260 Jte[3] += eY *dyh21;/* 0+ */
2261 Jte[4] += eY *dyh22;/* 0+ */
2262 Jte[5] += eY *dyh23;/* 0+ */
2263 Jte[6] += eX *dxh31 + eY *dyh31;/* + */
2264 Jte[7] += eX *dxh32 + eY *dyh32;/* + */
2265 }
2266
2267 /* Update JtJ: X Y */
2268 if(JtJ){
2269 JtJ[0][0] += dxh11*dxh11 ;/* +0 */
2270
2271 JtJ[1][0] += dxh11*dxh12 ;/* +0 */
2272 JtJ[1][1] += dxh12*dxh12 ;/* +0 */
2273
2274 JtJ[2][0] += dxh11*dxh13 ;/* +0 */
2275 JtJ[2][1] += dxh12*dxh13 ;/* +0 */
2276 JtJ[2][2] += dxh13*dxh13 ;/* +0 */
2277
2278 /*JtJ[3][0] += ; 0+0 */
2279 /*JtJ[3][1] += ; 0+0 */
2280 /*JtJ[3][2] += ; 0+0 */
2281 JtJ[3][3] += dyh21*dyh21;/* 0+ */
2282
2283 /*JtJ[4][0] += ; 0+0 */
2284 /*JtJ[4][1] += ; 0+0 */
2285 /*JtJ[4][2] += ; 0+0 */
2286 JtJ[4][3] += dyh21*dyh22;/* 0+ */
2287 JtJ[4][4] += dyh22*dyh22;/* 0+ */
2288
2289 /*JtJ[5][0] += ; 0+0 */
2290 /*JtJ[5][1] += ; 0+0 */
2291 /*JtJ[5][2] += ; 0+0 */
2292 JtJ[5][3] += dyh21*dyh23;/* 0+ */
2293 JtJ[5][4] += dyh22*dyh23;/* 0+ */
2294 JtJ[5][5] += dyh23*dyh23;/* 0+ */
2295
2296 JtJ[6][0] += dxh11*dxh31 ;/* +0 */
2297 JtJ[6][1] += dxh12*dxh31 ;/* +0 */
2298 JtJ[6][2] += dxh13*dxh31 ;/* +0 */
2299 JtJ[6][3] += dyh21*dyh31;/* 0+ */
2300 JtJ[6][4] += dyh22*dyh31;/* 0+ */
2301 JtJ[6][5] += dyh23*dyh31;/* 0+ */
2302 JtJ[6][6] += dxh31*dxh31 + dyh31*dyh31;/* + */
2303
2304 JtJ[7][0] += dxh11*dxh32 ;/* +0 */
2305 JtJ[7][1] += dxh12*dxh32 ;/* +0 */
2306 JtJ[7][2] += dxh13*dxh32 ;/* +0 */
2307 JtJ[7][3] += dyh21*dyh32;/* 0+ */
2308 JtJ[7][4] += dyh22*dyh32;/* 0+ */
2309 JtJ[7][5] += dyh23*dyh32;/* 0+ */
2310 JtJ[7][6] += dxh31*dxh32 + dyh31*dyh32;/* + */
2311 JtJ[7][7] += dxh32*dxh32 + dyh32*dyh32;/* + */
2312 }
2313 }
2314 }
2315
2316 if(Sp){*Sp = S;}
2317 }
2318
2319
2320 /**
2321 * Compute the Levenberg-Marquardt "gain" obtained by the given step dH.
2322 *
2323 * Drawn from http://www2.imm.dtu.dk/documents/ftp/tr99/tr05_99.pdf.
2324 */
2325
sacLMGain(const float * dH,const float * Jte,const float S,const float newS,const float lambda)2326 static inline float sacLMGain(const float* dH,
2327 const float* Jte,
2328 const float S,
2329 const float newS,
2330 const float lambda){
2331 float dS = S-newS;
2332 float dL = 0;
2333 int i;
2334
2335 /* Compute h^t h... */
2336 for(i=0;i<8;i++){
2337 dL += dH[i]*dH[i];
2338 }
2339 /* Compute mu * h^t h... */
2340 dL *= lambda;
2341 /* Subtract h^t F'... */
2342 for(i=0;i<8;i++){
2343 dL += dH[i]*Jte[i];/* += as opposed to -=, since dH we compute is
2344 opposite sign. */
2345 }
2346 /* Multiply by 1/2... */
2347 dL *= 0.5;
2348
2349 /* Return gain as S-newS / L0 - LH. */
2350 return fabs(dL) < FLT_EPSILON ? dS : dS / dL;
2351 }
2352
2353
2354 /**
2355 * Cholesky decomposition on 8x8 real positive-definite matrix defined by its
2356 * lower-triangular half. Outputs L, the lower triangular part of the
2357 * decomposition.
2358 *
2359 * A and L can overlap fully (in-place) or not at all, but may not partially
2360 * overlap.
2361 *
2362 * For damping, the diagonal elements are scaled by 1.0 + lambda.
2363 *
2364 * Returns zero if decomposition unsuccessful, and non-zero otherwise.
2365 *
2366 * Source: http://en.wikipedia.org/wiki/Cholesky_decomposition#
2367 * The_Cholesky.E2.80.93Banachiewicz_and_Cholesky.E2.80.93Crout_algorithms
2368 */
2369
sacChol8x8Damped(const float (* A)[8],float lambda,float (* L)[8])2370 static inline int sacChol8x8Damped(const float (*A)[8],
2371 float lambda,
2372 float (*L)[8]){
2373 const register int N = 8;
2374 int i, j, k;
2375 float lambdap1 = lambda + 1.0f;
2376 float x;
2377
2378 for(i=0;i<N;i++){/* Row */
2379 /* Pre-diagonal elements */
2380 for(j=0;j<i;j++){
2381 x = A[i][j]; /* Aij */
2382 for(k=0;k<j;k++){
2383 x -= L[i][k] * L[j][k];/* - Sum_{k=0..j-1} Lik*Ljk */
2384 }
2385 L[i][j] = x / L[j][j]; /* Lij = ... / Ljj */
2386 }
2387
2388 /* Diagonal element */
2389 {j = i;
2390 x = A[j][j] * lambdap1; /* Ajj */
2391 for(k=0;k<j;k++){
2392 x -= L[j][k] * L[j][k];/* - Sum_{k=0..j-1} Ljk^2 */
2393 }
2394 if(x<0){
2395 return 0;
2396 }
2397 L[j][j] = sqrtf(x); /* Ljj = sqrt( ... ) */
2398 }
2399 }
2400
2401 return 1;
2402 }
2403
2404
2405 /**
2406 * Invert lower-triangular 8x8 matrix L into lower-triangular matrix M.
2407 *
2408 * L and M can overlap fully (in-place) or not at all, but may not partially
2409 * overlap.
2410 *
2411 * Uses formulation from
2412 * http://www.cs.berkeley.edu/~knight/knight_math221_poster.pdf
2413 * , adjusted for the fact that A^T^-1 = A^-1^T. Thus:
2414 *
2415 * U11 U12 U11^-1 -U11^-1*U12*U22^-1
2416 * ->
2417 * 0 U22 0 U22^-1
2418 *
2419 * Becomes
2420 *
2421 * L11 0 L11^-1 0
2422 * ->
2423 * L21 L22 -L22^-1*L21*L11^-1 L22^-1
2424 *
2425 * Since
2426 *
2427 * ( -L11^T^-1*L21^T*L22^T^-1 )^T = -L22^T^-1^T*L21^T^T*L11^T^-1^T
2428 * = -L22^T^T^-1*L21^T^T*L11^T^T^-1
2429 * = -L22^-1*L21*L11^-1
2430 */
2431
sacTRInv8x8(const float (* L)[8],float (* M)[8])2432 static inline void sacTRInv8x8(const float (*L)[8],
2433 float (*M)[8]){
2434 float s[2][2], t[2][2];
2435 float u[4][4], v[4][4];
2436
2437 /*
2438 L00 0 0 0 0 0 0 0
2439 L10 L11 0 0 0 0 0 0
2440 L20 L21 L22 0 0 0 0 0
2441 L30 L31 L32 L33 0 0 0 0
2442 L40 L41 L42 L43 L44 0 0 0
2443 L50 L51 L52 L53 L54 L55 0 0
2444 L60 L61 L62 L63 L64 L65 L66 0
2445 L70 L71 L72 L73 L74 L75 L76 L77
2446 */
2447
2448 /* Invert 4*2 1x1 matrices; Starts recursion. */
2449 M[0][0] = 1.0f/L[0][0];
2450 M[1][1] = 1.0f/L[1][1];
2451 M[2][2] = 1.0f/L[2][2];
2452 M[3][3] = 1.0f/L[3][3];
2453 M[4][4] = 1.0f/L[4][4];
2454 M[5][5] = 1.0f/L[5][5];
2455 M[6][6] = 1.0f/L[6][6];
2456 M[7][7] = 1.0f/L[7][7];
2457
2458 /*
2459 M00 0 0 0 0 0 0 0
2460 L10 M11 0 0 0 0 0 0
2461 L20 L21 M22 0 0 0 0 0
2462 L30 L31 L32 M33 0 0 0 0
2463 L40 L41 L42 L43 M44 0 0 0
2464 L50 L51 L52 L53 L54 M55 0 0
2465 L60 L61 L62 L63 L64 L65 M66 0
2466 L70 L71 L72 L73 L74 L75 L76 M77
2467 */
2468
2469 /* 4*2 Matrix products of 1x1 matrices */
2470 M[1][0] = -M[1][1]*L[1][0]*M[0][0];
2471 M[3][2] = -M[3][3]*L[3][2]*M[2][2];
2472 M[5][4] = -M[5][5]*L[5][4]*M[4][4];
2473 M[7][6] = -M[7][7]*L[7][6]*M[6][6];
2474
2475 /*
2476 M00 0 0 0 0 0 0 0
2477 M10 M11 0 0 0 0 0 0
2478 L20 L21 M22 0 0 0 0 0
2479 L30 L31 M32 M33 0 0 0 0
2480 L40 L41 L42 L43 M44 0 0 0
2481 L50 L51 L52 L53 M54 M55 0 0
2482 L60 L61 L62 L63 L64 L65 M66 0
2483 L70 L71 L72 L73 L74 L75 M76 M77
2484 */
2485
2486 /* 2*2 Matrix products of 2x2 matrices */
2487
2488 /*
2489 (M22 0 ) (L20 L21) (M00 0 )
2490 - (M32 M33) x (L30 L31) x (M10 M11)
2491 */
2492
2493 s[0][0] = M[2][2]*L[2][0];
2494 s[0][1] = M[2][2]*L[2][1];
2495 s[1][0] = M[3][2]*L[2][0]+M[3][3]*L[3][0];
2496 s[1][1] = M[3][2]*L[2][1]+M[3][3]*L[3][1];
2497
2498 t[0][0] = s[0][0]*M[0][0]+s[0][1]*M[1][0];
2499 t[0][1] = s[0][1]*M[1][1];
2500 t[1][0] = s[1][0]*M[0][0]+s[1][1]*M[1][0];
2501 t[1][1] = s[1][1]*M[1][1];
2502
2503 M[2][0] = -t[0][0];
2504 M[2][1] = -t[0][1];
2505 M[3][0] = -t[1][0];
2506 M[3][1] = -t[1][1];
2507
2508 /*
2509 (M66 0 ) (L64 L65) (M44 0 )
2510 - (L76 M77) x (L74 L75) x (M54 M55)
2511 */
2512
2513 s[0][0] = M[6][6]*L[6][4];
2514 s[0][1] = M[6][6]*L[6][5];
2515 s[1][0] = M[7][6]*L[6][4]+M[7][7]*L[7][4];
2516 s[1][1] = M[7][6]*L[6][5]+M[7][7]*L[7][5];
2517
2518 t[0][0] = s[0][0]*M[4][4]+s[0][1]*M[5][4];
2519 t[0][1] = s[0][1]*M[5][5];
2520 t[1][0] = s[1][0]*M[4][4]+s[1][1]*M[5][4];
2521 t[1][1] = s[1][1]*M[5][5];
2522
2523 M[6][4] = -t[0][0];
2524 M[6][5] = -t[0][1];
2525 M[7][4] = -t[1][0];
2526 M[7][5] = -t[1][1];
2527
2528 /*
2529 M00 0 0 0 0 0 0 0
2530 M10 M11 0 0 0 0 0 0
2531 M20 M21 M22 0 0 0 0 0
2532 M30 M31 M32 M33 0 0 0 0
2533 L40 L41 L42 L43 M44 0 0 0
2534 L50 L51 L52 L53 M54 M55 0 0
2535 L60 L61 L62 L63 M64 M65 M66 0
2536 L70 L71 L72 L73 M74 M75 M76 M77
2537 */
2538
2539 /* 1*2 Matrix products of 4x4 matrices */
2540
2541 /*
2542 (M44 0 0 0 ) (L40 L41 L42 L43) (M00 0 0 0 )
2543 (M54 M55 0 0 ) (L50 L51 L52 L53) (M10 M11 0 0 )
2544 (M64 M65 M66 0 ) (L60 L61 L62 L63) (M20 M21 M22 0 )
2545 - (M74 M75 M76 M77) x (L70 L71 L72 L73) x (M30 M31 M32 M33)
2546 */
2547
2548 u[0][0] = M[4][4]*L[4][0];
2549 u[0][1] = M[4][4]*L[4][1];
2550 u[0][2] = M[4][4]*L[4][2];
2551 u[0][3] = M[4][4]*L[4][3];
2552 u[1][0] = M[5][4]*L[4][0]+M[5][5]*L[5][0];
2553 u[1][1] = M[5][4]*L[4][1]+M[5][5]*L[5][1];
2554 u[1][2] = M[5][4]*L[4][2]+M[5][5]*L[5][2];
2555 u[1][3] = M[5][4]*L[4][3]+M[5][5]*L[5][3];
2556 u[2][0] = M[6][4]*L[4][0]+M[6][5]*L[5][0]+M[6][6]*L[6][0];
2557 u[2][1] = M[6][4]*L[4][1]+M[6][5]*L[5][1]+M[6][6]*L[6][1];
2558 u[2][2] = M[6][4]*L[4][2]+M[6][5]*L[5][2]+M[6][6]*L[6][2];
2559 u[2][3] = M[6][4]*L[4][3]+M[6][5]*L[5][3]+M[6][6]*L[6][3];
2560 u[3][0] = M[7][4]*L[4][0]+M[7][5]*L[5][0]+M[7][6]*L[6][0]+M[7][7]*L[7][0];
2561 u[3][1] = M[7][4]*L[4][1]+M[7][5]*L[5][1]+M[7][6]*L[6][1]+M[7][7]*L[7][1];
2562 u[3][2] = M[7][4]*L[4][2]+M[7][5]*L[5][2]+M[7][6]*L[6][2]+M[7][7]*L[7][2];
2563 u[3][3] = M[7][4]*L[4][3]+M[7][5]*L[5][3]+M[7][6]*L[6][3]+M[7][7]*L[7][3];
2564
2565 v[0][0] = u[0][0]*M[0][0]+u[0][1]*M[1][0]+u[0][2]*M[2][0]+u[0][3]*M[3][0];
2566 v[0][1] = u[0][1]*M[1][1]+u[0][2]*M[2][1]+u[0][3]*M[3][1];
2567 v[0][2] = u[0][2]*M[2][2]+u[0][3]*M[3][2];
2568 v[0][3] = u[0][3]*M[3][3];
2569 v[1][0] = u[1][0]*M[0][0]+u[1][1]*M[1][0]+u[1][2]*M[2][0]+u[1][3]*M[3][0];
2570 v[1][1] = u[1][1]*M[1][1]+u[1][2]*M[2][1]+u[1][3]*M[3][1];
2571 v[1][2] = u[1][2]*M[2][2]+u[1][3]*M[3][2];
2572 v[1][3] = u[1][3]*M[3][3];
2573 v[2][0] = u[2][0]*M[0][0]+u[2][1]*M[1][0]+u[2][2]*M[2][0]+u[2][3]*M[3][0];
2574 v[2][1] = u[2][1]*M[1][1]+u[2][2]*M[2][1]+u[2][3]*M[3][1];
2575 v[2][2] = u[2][2]*M[2][2]+u[2][3]*M[3][2];
2576 v[2][3] = u[2][3]*M[3][3];
2577 v[3][0] = u[3][0]*M[0][0]+u[3][1]*M[1][0]+u[3][2]*M[2][0]+u[3][3]*M[3][0];
2578 v[3][1] = u[3][1]*M[1][1]+u[3][2]*M[2][1]+u[3][3]*M[3][1];
2579 v[3][2] = u[3][2]*M[2][2]+u[3][3]*M[3][2];
2580 v[3][3] = u[3][3]*M[3][3];
2581
2582 M[4][0] = -v[0][0];
2583 M[4][1] = -v[0][1];
2584 M[4][2] = -v[0][2];
2585 M[4][3] = -v[0][3];
2586 M[5][0] = -v[1][0];
2587 M[5][1] = -v[1][1];
2588 M[5][2] = -v[1][2];
2589 M[5][3] = -v[1][3];
2590 M[6][0] = -v[2][0];
2591 M[6][1] = -v[2][1];
2592 M[6][2] = -v[2][2];
2593 M[6][3] = -v[2][3];
2594 M[7][0] = -v[3][0];
2595 M[7][1] = -v[3][1];
2596 M[7][2] = -v[3][2];
2597 M[7][3] = -v[3][3];
2598
2599 /*
2600 M00 0 0 0 0 0 0 0
2601 M10 M11 0 0 0 0 0 0
2602 M20 M21 M22 0 0 0 0 0
2603 M30 M31 M32 M33 0 0 0 0
2604 M40 M41 M42 M43 M44 0 0 0
2605 M50 M51 M52 M53 M54 M55 0 0
2606 M60 M61 M62 M63 M64 M65 M66 0
2607 M70 M71 M72 M73 M74 M75 M76 M77
2608 */
2609 }
2610
2611
2612 /**
2613 * Solves dH = inv(JtJ) Jte. The argument lower-triangular matrix is the
2614 * inverse of L as produced by the Cholesky decomposition LL^T of the matrix
2615 * JtJ; Thus the operation performed here is a left-multiplication of a vector
2616 * by two triangular matrices. The math is below:
2617 *
2618 * JtJ = LL^T
2619 * Linv = L^-1
2620 * (JtJ)^-1 = (LL^T)^-1
2621 * = (L^T^-1)(Linv)
2622 * = (Linv^T)(Linv)
2623 * dH = ((JtJ)^-1) (Jte)
2624 * = (Linv^T)(Linv) (Jte)
2625 *
2626 * where J is nx8, Jt is 8xn, JtJ is 8x8 PD, e is nx1, Jte is 8x1, L is lower
2627 * triangular 8x8 and dH is 8x1.
2628 */
2629
sacTRISolve8x8(const float (* L)[8],const float * Jte,float * dH)2630 static inline void sacTRISolve8x8(const float (*L)[8],
2631 const float* Jte,
2632 float* dH){
2633 float t[8];
2634
2635 t[0] = L[0][0]*Jte[0];
2636 t[1] = L[1][0]*Jte[0]+L[1][1]*Jte[1];
2637 t[2] = L[2][0]*Jte[0]+L[2][1]*Jte[1]+L[2][2]*Jte[2];
2638 t[3] = L[3][0]*Jte[0]+L[3][1]*Jte[1]+L[3][2]*Jte[2]+L[3][3]*Jte[3];
2639 t[4] = L[4][0]*Jte[0]+L[4][1]*Jte[1]+L[4][2]*Jte[2]+L[4][3]*Jte[3]+L[4][4]*Jte[4];
2640 t[5] = L[5][0]*Jte[0]+L[5][1]*Jte[1]+L[5][2]*Jte[2]+L[5][3]*Jte[3]+L[5][4]*Jte[4]+L[5][5]*Jte[5];
2641 t[6] = L[6][0]*Jte[0]+L[6][1]*Jte[1]+L[6][2]*Jte[2]+L[6][3]*Jte[3]+L[6][4]*Jte[4]+L[6][5]*Jte[5]+L[6][6]*Jte[6];
2642 t[7] = L[7][0]*Jte[0]+L[7][1]*Jte[1]+L[7][2]*Jte[2]+L[7][3]*Jte[3]+L[7][4]*Jte[4]+L[7][5]*Jte[5]+L[7][6]*Jte[6]+L[7][7]*Jte[7];
2643
2644
2645 dH[0] = L[0][0]*t[0]+L[1][0]*t[1]+L[2][0]*t[2]+L[3][0]*t[3]+L[4][0]*t[4]+L[5][0]*t[5]+L[6][0]*t[6]+L[7][0]*t[7];
2646 dH[1] = L[1][1]*t[1]+L[2][1]*t[2]+L[3][1]*t[3]+L[4][1]*t[4]+L[5][1]*t[5]+L[6][1]*t[6]+L[7][1]*t[7];
2647 dH[2] = L[2][2]*t[2]+L[3][2]*t[3]+L[4][2]*t[4]+L[5][2]*t[5]+L[6][2]*t[6]+L[7][2]*t[7];
2648 dH[3] = L[3][3]*t[3]+L[4][3]*t[4]+L[5][3]*t[5]+L[6][3]*t[6]+L[7][3]*t[7];
2649 dH[4] = L[4][4]*t[4]+L[5][4]*t[5]+L[6][4]*t[6]+L[7][4]*t[7];
2650 dH[5] = L[5][5]*t[5]+L[6][5]*t[6]+L[7][5]*t[7];
2651 dH[6] = L[6][6]*t[6]+L[7][6]*t[7];
2652 dH[7] = L[7][7]*t[7];
2653 }
2654
2655
2656 /**
2657 * Subtract dH from H.
2658 */
2659
sacSub8x1(float * Hout,const float * H,const float * dH)2660 static inline void sacSub8x1(float* Hout, const float* H, const float* dH){
2661 Hout[0] = H[0] - dH[0];
2662 Hout[1] = H[1] - dH[1];
2663 Hout[2] = H[2] - dH[2];
2664 Hout[3] = H[3] - dH[3];
2665 Hout[4] = H[4] - dH[4];
2666 Hout[5] = H[5] - dH[5];
2667 Hout[6] = H[6] - dH[6];
2668 Hout[7] = H[7] - dH[7];
2669 }
2670
2671
2672 /* End namespace cv */
2673 }
2674