diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 4f405afc667cf861e9aa6ac8d96ad34c005904cd..397b87a0efd6a52bff0c6586b160fe12b7e16d11 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -53,7 +53,8 @@ namespace cv //! type of the robust estimation algorithm enum { LMEDS = 4, //!< least-median algorithm - RANSAC = 8 //!< RANSAC algorithm + RANSAC = 8, //!< RANSAC algorithm + RHO = 16 //!< RHO algorithm }; enum { SOLVEPNP_ITERATIVE = 0, diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index c700ece70d05ea678cd2f0c4c4149dc86aa70810..07cf68a0fe132ed7bc20acdf879185ded09528aa 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -41,6 +41,10 @@ //M*/ #include "precomp.hpp" +#include "rhorefc.h" +#if CV_SSE2 +#include "rhosse2.h" +#endif #include namespace cv @@ -273,6 +277,73 @@ public: } + +namespace cv{ +static bool createAndRunRHORegistrator(double confidence, int maxIters, double ransacReprojThreshold, int npoints, InputArray _src, InputArray _dst, OutputArray _H, OutputArray _tempMask){ + Mat src = _src.getMat(); + Mat dst = _dst.getMat(); + Mat tempMask = _tempMask.getMat(); + bool result; + + /* Run RHO. Needs cleanup or separate function to invoke. */ + Mat tmpH = Mat(3, 3, CV_32FC1); + tempMask = Mat(npoints, 1, CV_8U); + double beta = 0.35;/* 0.35 is a value that often works. */ + +#if CV_SSE2 && 0 + if(useOptimized()){ + RHO_HEST_SSE2 p; + rhoSSE2Init(&p); + rhoSSE2EnsureCapacity(&p, npoints, beta); + result = !!rhoSSE2(&p, + (const float*)src.data, + (const float*)dst.data, + (char*) tempMask.data, + npoints, + ransacReprojThreshold, + maxIters, + maxIters, + confidence, + 4, + beta, + RHO_FLAG_ENABLE_NR, + NULL, + (float*)tmpH.data); + rhoSSE2Fini(&p); + }else +#endif + { + RHO_HEST_REFC p; + rhoRefCInit(&p); + rhoRefCEnsureCapacity(&p, npoints, beta); + result = !!rhoRefC(&p, + (const float*)src.data, + (const float*)dst.data, + (char*) tempMask.data, + npoints, + ransacReprojThreshold, + maxIters, + maxIters, + confidence, + 4, + beta, + RHO_FLAG_ENABLE_NR, + NULL, + (float*)tmpH.data); + rhoRefCFini(&p); + } + tmpH.convertTo(_H, CV_64FC1); + + /* Maps non-zero maks elems to 1, for the sake of the testcase. */ + for(int k=0;krun(src, dst, H, tempMask); else if( method == LMEDS ) result = createLMeDSPointSetRegistrator(cb, 4, confidence, maxIters)->run(src, dst, H, tempMask); - else + else if( method == RHO ){ + result = createAndRunRHORegistrator(confidence, maxIters, ransacReprojThreshold, npoints, src, dst, H, tempMask); + }else CV_Error(Error::StsBadArg, "Unknown estimation method"); - if( result && npoints > 4 ) + if( result && npoints > 4 && method != RHO) { compressPoints( src.ptr(), tempMask.ptr(), 1, npoints ); npoints = compressPoints( dst.ptr(), tempMask.ptr(), 1, npoints ); diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cda70f4162e7f817c62420d89ff589cb7100af5b --- /dev/null +++ b/modules/calib3d/src/rhorefc.cpp @@ -0,0 +1,2042 @@ +/* + IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + + By downloading, copying, installing or using the software you agree to this license. + If you do not agree to this license, do not download, install, + copy or use the software. + + + BSD 3-Clause License + + Copyright (C) 2014, Olexa Bilaniuk, Hamid Bazargani & Robert Laganiere, all rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistribution's of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistribution's in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * The name of the copyright holders may not be used to endorse or promote products + derived from this software without specific prior written permission. + + This software is provided by the copyright holders and contributors "as is" and + any express or implied warranties, including, but not limited to, the implied + warranties of merchantability and fitness for a particular purpose are disclaimed. + In no event shall the Intel Corporation or contributors be liable for any direct, + indirect, incidental, special, exemplary, or consequential damages + (including, but not limited to, procurement of substitute goods or services; + loss of use, data, or profits; or business interruption) however caused + and on any theory of liability, whether in contract, strict liability, + or tort (including negligence or otherwise) arising in any way out of + the use of this software, even if advised of the possibility of such damage. +*/ + +/** + * Bilaniuk, Olexa, Hamid Bazargani, and Robert Laganiere. "Fast Target + * Recognition on Mobile Devices: Revisiting Gaussian Elimination for the + * Estimation of Planar Homographies." In Computer Vision and Pattern + * Recognition Workshops (CVPRW), 2014 IEEE Conference on, pp. 119-125. + * IEEE, 2014. + */ + +/* Includes */ +#include +#include +#include +#include +#include +#include +#include +#include +#include "rhorefc.h" + + + +/* Defines */ +#define MEM_ALIGN 32 +#define HSIZE (3*3*sizeof(float)) +#define MIN_DELTA_CHNG 0.1 +#define REL_CHNG(a, b) (fabs((a) - (b))/(a)) +#define CHNG_SIGNIFICANT(a, b) (REL_CHNG(a, b) > MIN_DELTA_CHNG) +#define CHI_STAT 2.706 +#define CHI_SQ 1.645 +#define RLO 0.25 +#define RHI 0.75 +#define MAXLEVMARQITERS 10 +#define m 4 /* 4 points required per model */ +#define SPRT_T_M 25 /* Guessing 25 match evlauations / 1 model generation */ +#define SPRT_M_S 1 /* 1 model per sample */ +#define SPRT_EPSILON 0.1 /* No explanation */ +#define SPRT_DELTA 0.01 /* No explanation */ + + + +/* For the sake of cv:: namespace ONLY: */ +#ifdef __cplusplus +namespace cv{/* For C support, replace with extern "C" { */ +#endif + + +/* Data Structures */ + + + +/* Prototypes */ +static inline void* almalloc(size_t nBytes); +static inline void alfree(void* ptr); + +static inline int sacInitRun(RHO_HEST_REFC* p); +static inline void sacFiniRun(RHO_HEST_REFC* p); +static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p); +static inline int sacHypothesize(RHO_HEST_REFC* p); +static inline int sacVerify(RHO_HEST_REFC* p); +static inline int sacIsNREnabled(RHO_HEST_REFC* p); +static inline int sacIsRefineEnabled(RHO_HEST_REFC* p); +static inline int sacIsFinalRefineEnabled(RHO_HEST_REFC* p); +static inline int sacPROSACPhaseEndReached(RHO_HEST_REFC* p); +static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p); +static inline void sacGetPROSACSample(RHO_HEST_REFC* p); +static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p); +static inline void sacGenerateModel(RHO_HEST_REFC* p); +static inline int sacIsModelDegenerate(RHO_HEST_REFC* p); +static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p); +static inline void sacUpdateSPRT(RHO_HEST_REFC* p); +static inline void sacDesignSPRTTest(RHO_HEST_REFC* p); +static inline int sacIsBestModel(RHO_HEST_REFC* p); +static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p); +static inline void sacSaveBestModel(RHO_HEST_REFC* p); +static inline void sacInitNonRand(double beta, + unsigned start, + unsigned N, + unsigned* nonRandMinInl); +static inline void sacNStarOptimize(RHO_HEST_REFC* p); +static inline void sacUpdateBounds(RHO_HEST_REFC* p); +static inline void sacOutputModel(RHO_HEST_REFC* p); +static inline void sacOutputZeroH(RHO_HEST_REFC* p); + +static inline double sacInitPEndFpI(const unsigned ransacConvg, + const unsigned n, + const unsigned s); +static inline void sacRndSmpl(unsigned sampleSize, + unsigned* currentSample, + unsigned dataSetSize); +static inline double sacRandom(void); +static inline unsigned sacCalcIterBound(double confidence, + double inlierRate, + unsigned sampleSize, + unsigned maxIterBound); +static inline void hFuncRefC(float* packedPoints, float* H); +static inline int sacCanRefine(RHO_HEST_REFC* p); +static inline void sacRefine(RHO_HEST_REFC* p); +static inline void sacCalcJtMats(float (* restrict JtJ)[8], + float* restrict Jte, + float* restrict Sp, + const float* restrict H, + const float* restrict src, + const float* restrict dst, + const char* restrict inl, + unsigned N); +static inline void sacChol8x8 (const float (*A)[8], + float (*L)[8]); +static inline void sacTRInv8x8(const float (*L)[8], + float (*M)[8]); +static inline void sacTRISolve8x8(const float (*L)[8], + const float* Jte, + float* dH); +static inline void sacScaleDiag8x8(const float (*A)[8], + float lambda, + float (*B)[8]); +static inline void sacSub8x1(float* Hout, const float* H, const float* dH); + + + +/* Functions */ + +/** + * Initialize the estimator context, by allocating the aligned buffers + * internally needed. + * + * Currently there are 5 per-estimator buffers: + * - The buffer of m indexes representing a sample + * - The buffer of 16 floats representing m matches (x,y) -> (X,Y). + * - The buffer for the current homography + * - The buffer for the best-so-far homography + * - Optionally, the non-randomness criterion table + * + * @param [in/out] p The uninitialized estimator context to initialize. + * @return 0 if successful; non-zero if an error occured. + */ + +int rhoRefCInit(RHO_HEST_REFC* p){ + memset(p, 0, sizeof(*p)); + + p->ctrl.smpl = (unsigned*)almalloc(m*sizeof(*p->ctrl.smpl)); + + p->curr.pkdPts = (float*) almalloc(m*2*2*sizeof(*p->curr.pkdPts)); + p->curr.H = (float*) almalloc(HSIZE); + p->curr.inl = NULL; + p->curr.numInl = 0; + + p->best.H = (float*) almalloc(HSIZE); + p->best.inl = NULL; + p->best.numInl = 0; + + p->nr.tbl = NULL;/* By default this table is not computed. */ + p->nr.size = 0; + p->nr.beta = 0.0; + + + int areAllAllocsSuccessful = p->ctrl.smpl && + p->curr.H && + p->best.H && + p->curr.pkdPts; + + if(!areAllAllocsSuccessful){ + rhoRefCFini(p); + } + + return areAllAllocsSuccessful; +} + + +/** + * Ensure that the estimator context's internal table for non-randomness + * criterion is at least of the given size, and uses the given beta. The table + * should be larger than the maximum number of matches fed into the estimator. + * + * A value of N of 0 requests deallocation of the table. + * + * @param [in] p The initialized estimator context + * @param [in] N If 0, deallocate internal table. If > 0, ensure that the + * internal table is of at least this size, reallocating if + * necessary. + * @param [in] beta The beta-factor to use within the table. + * @return 1 if successful; 0 if an error occured. + * + * Reads: nr.* + * Writes: nr.* + */ + +int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta){ + unsigned* tmp; + + + if(N == 0){ + /* Deallocate table */ + alfree(p->nr.tbl); + p->nr.tbl = NULL; + p->nr.size = 0; + }else{ + /* Ensure table at least as big as N and made for correct beta. */ + if(p->nr.tbl && p->nr.beta == beta && p->nr.size >= N){ + /* Table already correctly set up */ + }else{ + if(p->nr.size < N){ + /* Reallocate table because it is too small. */ + tmp = (unsigned*)almalloc(N*sizeof(unsigned)); + if(!tmp){ + return 0; + } + + /* Must recalculate in whole or part. */ + if(p->nr.beta != beta){ + /* Beta changed; recalculate in whole. */ + sacInitNonRand(beta, 0, N, tmp); + alfree(p->nr.tbl); + }else{ + /* Beta did not change; Copy over any work already done. */ + memcpy(tmp, p->nr.tbl, p->nr.size*sizeof(unsigned)); + sacInitNonRand(beta, p->nr.size, N, tmp); + alfree(p->nr.tbl); + } + + p->nr.tbl = tmp; + p->nr.size = N; + p->nr.beta = beta; + }else{ + /* Might recalculate in whole, or not at all. */ + if(p->nr.beta != beta){ + /* Beta changed; recalculate in whole. */ + sacInitNonRand(beta, 0, p->nr.size, p->nr.tbl); + p->nr.beta = beta; + }else{ + /* Beta did not change; Table was already big enough. Do nothing. */ + /* Besides, this is unreachable. */ + } + } + } + } + + return 1; +} + + +/** + * Finalize the estimator context, by freeing the aligned buffers used + * internally. + * + * @param [in] p The initialized estimator context to finalize. + */ + +void rhoRefCFini(RHO_HEST_REFC* p){ + alfree(p->ctrl.smpl); + alfree(p->curr.H); + alfree(p->best.H); + alfree(p->curr.pkdPts); + alfree(p->nr.tbl); + + memset(p, 0, sizeof(*p)); +} + + +/** + * Estimates the homography using the given context, matches and parameters to + * PROSAC. + * + * @param [in/out] p The context to use for homography estimation. Must + * be already initialized. Cannot be NULL. + * @param [in] src The pointer to the source points of the matches. + * Must be aligned to 4 bytes. Cannot be NULL. + * @param [in] dst The pointer to the destination points of the matches. + * Must be aligned to 16 bytes. Cannot be NULL. + * @param [out] inl The pointer to the output mask of inlier matches. + * Must be aligned to 16 bytes. May be NULL. + * @param [in] N The number of matches. + * @param [in] maxD The maximum distance. + * @param [in] maxI The maximum number of PROSAC iterations. + * @param [in] rConvg The RANSAC convergence parameter. + * @param [in] cfd The required confidence in the solution. + * @param [in] minInl The minimum required number of inliers. + * @param [in] beta The beta-parameter for the non-randomness criterion. + * @param [in] flags A union of flags to control the estimation. + * @param [in] guessH An extrinsic guess at the solution H, or NULL if + * none provided. + * @param [out] finalH The final estimation of H, or the zero matrix if + * the minimum number of inliers was not met. + * Cannot be NULL. + * @return The number of inliers if the minimum number of + * inliers for acceptance was reached; 0 otherwise. + */ + +unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation context. */ + const float* restrict src, /* Source points */ + const float* restrict dst, /* Destination points */ + char* restrict inl, /* Inlier mask */ + unsigned N, /* = src.length = dst.length = inl.length */ + float maxD, /* Works: 3.0 */ + unsigned maxI, /* Works: 2000 */ + unsigned rConvg, /* Works: 2000 */ + double cfd, /* Works: 0.995 */ + unsigned minInl, /* Minimum: 4 */ + double beta, /* Works: 0.35 */ + unsigned flags, /* Works: 0 */ + const float* guessH, /* Extrinsic guess, NULL if none provided */ + float* finalH){ /* Final result. */ + + /** + * Setup + */ + + p->arg.src = src; + p->arg.dst = dst; + p->arg.inl = inl; + p->arg.N = N; + p->arg.maxD = maxD; + p->arg.maxI = maxI; + p->arg.rConvg = rConvg; + p->arg.cfd = cfd; + p->arg.minInl = minInl; + p->arg.beta = beta; + p->arg.flags = flags; + p->arg.guessH = guessH; + p->arg.finalH = finalH; + if(!sacInitRun(p)){ + sacOutputZeroH(p); + sacFiniRun(p); + return 0; + } + + /** + * Extrinsic Guess + */ + + if(sacHaveExtrinsicGuess(p)){ + sacVerify(p); + } + + + /** + * PROSAC Loop + */ + + for(p->ctrl.i=0; p->ctrl.i < p->arg.maxI; p->ctrl.i++){ + sacHypothesize(p) && sacVerify(p); + } + + + /** + * Teardown + */ + + if(sacIsFinalRefineEnabled(p) && sacCanRefine(p)){ + sacRefine(p); + } + + sacOutputModel(p); + sacFiniRun(p); + return sacIsBestModelGoodEnough(p) ? p->best.numInl : 0; +} + + +/** + * Allocate memory aligned to a boundary of MEMALIGN. + */ + +static inline void* almalloc(size_t nBytes){ + if(nBytes){ + unsigned char* ptr = (unsigned char*)malloc(MEM_ALIGN + nBytes); + if(ptr){ + unsigned char* adj = (unsigned char*)(((intptr_t)(ptr+MEM_ALIGN))&((intptr_t)(-MEM_ALIGN))); + ptrdiff_t diff = adj - ptr; + adj[-1] = diff - 1; + return adj; + } + } + + return NULL; +} + +/** + * Free aligned memory. + * + * If argument is NULL, do nothing in accordance with free() semantics. + */ + +static inline void alfree(void* ptr){ + if(ptr){ + unsigned char* cptr = (unsigned char*)ptr; + free(cptr - (ptrdiff_t)cptr[-1] - 1); + } +} + + +/** + * Initialize SAC for a run given its arguments. + * + * Performs sanity-checks and memory allocations. Also initializes the state. + * + * @returns 0 if per-run initialization failed at any point; non-zero + * otherwise. + * + * Reads: arg.*, nr.* + * Writes: curr.*, best.*, ctrl.*, eval.* + */ + +static inline int sacInitRun(RHO_HEST_REFC* p){ + /** + * Sanitize arguments. + * + * Runs zeroth because these are easy-to-check errors and unambiguously + * mean something or other. + */ + + if(!p->arg.src || !p->arg.dst){ + /* Arguments src or dst are insane, must be != NULL */ + return 0; + } + if(p->arg.N < m){ + /* Argument N is insane, must be >= 4. */ + return 0; + } + if(p->arg.maxD < 0){ + /* Argument maxD is insane, must be >= 0. */ + return 0; + } + if(p->arg.cfd < 0 || p->arg.cfd > 1){ + /* Argument cfd is insane, must be in [0, 1]. */ + return 0; + } + /* Clamp minInl to 4 or higher. */ + p->arg.minInl = p->arg.minInl < m ? m : p->arg.minInl; + if(sacIsNREnabled(p) && (p->arg.beta <= 0 || p->arg.beta >= 1)){ + /* Argument beta is insane, must be in (0, 1). */ + return 0; + } + if(!p->arg.finalH){ + /* Argument finalH is insane, must be != NULL */ + return 0; + } + + /** + * Optional NR setup. + * + * Runs first because it is decoupled from most other things (*) and if it + * fails, it is easy to recover from. + * + * (*) The only things this code depends on is the flags argument, the nr.* + * substruct and the sanity-checked N and beta arguments from above. + */ + + if(sacIsNREnabled(p) && !rhoRefCEnsureCapacity(p, p->arg.N, p->arg.beta)){ + return 0; + } + + /** + * Inlier mask alloc. + * + * Runs second because we want to quit as fast as possible if we can't even + * allocate the up tp two masks. + * + * If the calling software wants an output mask, use buffer provided. If + * not, allocate one anyways internally. + */ + + p->best.inl = p->arg.inl ? p->arg.inl : (char*)almalloc(p->arg.N); + p->curr.inl = (char*)almalloc(p->arg.N); + + if(!p->curr.inl || !p->best.inl){ + return 0; + } + + /** + * Reset scalar per-run state. + * + * Runs third because there's no point in resetting/calculating a large + * number of fields if something in the above junk failed. + */ + + p->ctrl.i = 0; + p->ctrl.phNum = m; + p->ctrl.phEndI = 1; + p->ctrl.phEndFpI = sacInitPEndFpI(p->arg.rConvg, p->arg.N, m); + p->ctrl.phMax = p->arg.N; + p->ctrl.phNumInl = 0; + p->ctrl.numModels = 0; + + if(sacHaveExtrinsicGuess(p)){ + memcpy(p->curr.H, p->arg.guessH, HSIZE); + }else{ + memset(p->curr.H, 0, HSIZE); + } + p->curr.numInl = 0; + + memset(p->best.H, 0, HSIZE); + p->best.numInl = 0; + + p->eval.Ntested = 0; + p->eval.Ntestedtotal = 0; + p->eval.good = 1; + p->eval.t_M = SPRT_T_M; + p->eval.m_S = SPRT_M_S; + p->eval.epsilon = SPRT_EPSILON; + p->eval.delta = SPRT_DELTA; + sacDesignSPRTTest(p); + + return 1; +} + +/** + * Finalize SAC run. + * + * Deallocates per-run allocatable resources. Currently this consists only of + * the best and current inlier masks, which are equal in size to p->arg.N + * bytes. + * + * Reads: arg.bestInl, curr.inl, best.inl + * Writes: curr.inl, best.inl + */ + +static inline void sacFiniRun(RHO_HEST_REFC* p){ + /** + * If no output inlier mask was required, free both (internal) masks. + * Else if an (external) mask was provided as argument, find the other + * (the internal one) and free it. + */ + + if(p->arg.inl){ + if(p->arg.inl == p->best.inl){ + alfree(p->curr.inl); + }else{ + alfree(p->best.inl); + } + }else{ + alfree(p->best.inl); + alfree(p->curr.inl); + } + + p->best.inl = NULL; + p->curr.inl = NULL; +} + +/** + * Hypothesize a model. + * + * Selects randomly a sample (within the rules of PROSAC) and generates a + * new current model, and applies degeneracy tests to it. + * + * @returns 0 if hypothesized model could be rejected early as degenerate, and + * non-zero otherwise. + */ + +static inline int sacHypothesize(RHO_HEST_REFC* p){ + if(sacPROSACPhaseEndReached(p)){ + sacPROSACGoToNextPhase(p); + } + + sacGetPROSACSample(p); + if(sacIsSampleDegenerate(p)){ + return 0; + } + + sacGenerateModel(p); + if(sacIsModelDegenerate(p)){ + return 0; + } + + return 1; +} + +/** + * Verify the hypothesized model. + * + * Given the current model, evaluate its quality. If it is better than + * everything before, save as new best model (and possibly refine it). + * + * Returns 1. + */ + +static inline int sacVerify(RHO_HEST_REFC* p){ + sacEvaluateModelSPRT(p); + sacUpdateSPRT(p); + + if(sacIsBestModel(p)){ + sacSaveBestModel(p); + + if(sacIsRefineEnabled(p) && sacCanRefine(p)){ + sacRefine(p); + } + + sacUpdateBounds(p); + + if(sacIsNREnabled(p)){ + sacNStarOptimize(p); + } + } + + return 1; +} + +/** + * Check whether extrinsic guess was provided or not. + * + * @return Zero if no extrinsic guess was provided; non-zero otherwiseEE. + */ + +static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p){ + return !!p->arg.guessH; +} + +/** + * Check whether non-randomness criterion is enabled. + * + * @return Zero if non-randomness criterion disabled; non-zero if not. + */ + +static inline int sacIsNREnabled(RHO_HEST_REFC* p){ + return p->arg.flags & RHO_FLAG_ENABLE_NR; +} + +/** + * Check whether best-model-so-far refinement is enabled. + * + * @return Zero if best-model-so-far refinement disabled; non-zero if not. + */ + +static inline int sacIsRefineEnabled(RHO_HEST_REFC* p){ + return p->arg.flags & RHO_FLAG_ENABLE_REFINEMENT; +} + +/** + * Check whether final-model refinement is enabled. + * + * @return Zero if final-model refinement disabled; non-zero if not. + */ + +static inline int sacIsFinalRefineEnabled(RHO_HEST_REFC* p){ + return p->arg.flags & RHO_FLAG_ENABLE_FINAL_REFINEMENT; +} + +/** + * Computes whether the end of the current PROSAC phase has been reached. At + * PROSAC phase phNum, only matches [0, phNum) are sampled from. + * + * Accesses: + * Read: i, phEndI, phNum, phMax. + */ + +static inline int sacPROSACPhaseEndReached(RHO_HEST_REFC* p){ + return p->ctrl.i >= p->ctrl.phEndI && p->ctrl.phNum < p->ctrl.phMax; +} + +/** + * Updates unconditionally the necessary fields to move to the next PROSAC + * stage. + * + * Not idempotent. + * + * Accesses: + * Read: phNum, phEndFpI, phEndI + * Write: phNum, phEndFpI, phEndI + */ + +static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p){ + double next; + + p->ctrl.phNum++; + next = (p->ctrl.phEndFpI * p->ctrl.phNum)/(p->ctrl.phNum - m); + p->ctrl.phEndI += ceil(next - p->ctrl.phEndFpI); + p->ctrl.phEndFpI = next; +} + +/** + * Get a sample according to PROSAC rules. Namely: + * - If we're past the phase end interation, select randomly 4 out of the first + * phNum matches. + * - Otherwise, select match phNum-1 and select randomly the 3 others out of + * the first phNum-1 matches. + */ + +static inline void sacGetPROSACSample(RHO_HEST_REFC* p){ + if(p->ctrl.i > p->ctrl.phEndI){ + sacRndSmpl(4, p->ctrl.smpl, p->ctrl.phNum);/* Used to be phMax */ + }else{ + sacRndSmpl(3, p->ctrl.smpl, p->ctrl.phNum-1); + p->ctrl.smpl[3] = p->ctrl.phNum-1; + } +} + +/** + * Checks whether the *sample* is degenerate prior to model generation. + * - First, the extremely cheap numerical degeneracy test is run, which weeds + * out bad samples to the optimized GE implementation. + * - Second, the geometrical degeneracy test is run, which weeds out most other + * bad samples. + */ + +static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p){ + unsigned i0 = p->ctrl.smpl[0], i1 = p->ctrl.smpl[1], i2 = p->ctrl.smpl[2], i3 = p->ctrl.smpl[3]; + typedef struct{float x,y;} MyPt2f; + MyPt2f* pkdPts = (MyPt2f*)p->curr.pkdPts, *src = (MyPt2f*)p->arg.src, *dst = (MyPt2f*)p->arg.dst; + + /** + * Pack the matches selected by the SAC algorithm. + * Must be packed points[0:7] = {srcx0, srcy0, srcx1, srcy1, srcx2, srcy2, srcx3, srcy3} + * points[8:15] = {dstx0, dsty0, dstx1, dsty1, dstx2, dsty2, dstx3, dsty3} + * Gather 4 points into the vector + */ + + pkdPts[0] = src[i0]; + pkdPts[1] = src[i1]; + pkdPts[2] = src[i2]; + pkdPts[3] = src[i3]; + pkdPts[4] = dst[i0]; + pkdPts[5] = dst[i1]; + pkdPts[6] = dst[i2]; + pkdPts[7] = dst[i3]; + + /** + * If the matches' source points have common x and y coordinates, abort. + */ + + if(pkdPts[0].x == pkdPts[1].x || pkdPts[1].x == pkdPts[2].x || + pkdPts[2].x == pkdPts[3].x || pkdPts[0].x == pkdPts[2].x || + pkdPts[1].x == pkdPts[3].x || pkdPts[0].x == pkdPts[3].x || + pkdPts[0].y == pkdPts[1].y || pkdPts[1].y == pkdPts[2].y || + pkdPts[2].y == pkdPts[3].y || pkdPts[0].y == pkdPts[2].y || + pkdPts[1].y == pkdPts[3].y || pkdPts[0].y == pkdPts[3].y){ + return 1; + } + + /* If the matches do not satisfy the strong geometric constraint, abort. */ + /* (0 x 1) * 2 */ + float cross0s0 = pkdPts[0].y-pkdPts[1].y; + float cross0s1 = pkdPts[1].x-pkdPts[0].x; + float cross0s2 = pkdPts[0].x*pkdPts[1].y-pkdPts[0].y*pkdPts[1].x; + float dots0 = cross0s0*pkdPts[2].x + cross0s1*pkdPts[2].y + cross0s2; + float cross0d0 = pkdPts[4].y-pkdPts[5].y; + float cross0d1 = pkdPts[5].x-pkdPts[4].x; + float cross0d2 = pkdPts[4].x*pkdPts[5].y-pkdPts[4].y*pkdPts[5].x; + float dotd0 = cross0d0*pkdPts[6].x + cross0d1*pkdPts[6].y + cross0d2; + if(((int)dots0^(int)dotd0) < 0){ + return 1; + } + /* (0 x 1) * 3 */ + float cross1s0 = cross0s0; + float cross1s1 = cross0s1; + float cross1s2 = cross0s2; + float dots1 = cross1s0*pkdPts[3].x + cross1s1*pkdPts[3].y + cross1s2; + float cross1d0 = cross0d0; + float cross1d1 = cross0d1; + float cross1d2 = cross0d2; + float dotd1 = cross1d0*pkdPts[7].x + cross1d1*pkdPts[7].y + cross1d2; + if(((int)dots1^(int)dotd1) < 0){ + return 1; + } + /* (2 x 3) * 0 */ + float cross2s0 = pkdPts[2].y-pkdPts[3].y; + float cross2s1 = pkdPts[3].x-pkdPts[2].x; + float cross2s2 = pkdPts[2].x*pkdPts[3].y-pkdPts[2].y*pkdPts[3].x; + float dots2 = cross2s0*pkdPts[0].x + cross2s1*pkdPts[0].y + cross2s2; + float cross2d0 = pkdPts[6].y-pkdPts[7].y; + float cross2d1 = pkdPts[7].x-pkdPts[6].x; + float cross2d2 = pkdPts[6].x*pkdPts[7].y-pkdPts[6].y*pkdPts[7].x; + float dotd2 = cross2d0*pkdPts[4].x + cross2d1*pkdPts[4].y + cross2d2; + if(((int)dots2^(int)dotd2) < 0){ + return 1; + } + /* (2 x 3) * 1 */ + float cross3s0 = cross2s0; + float cross3s1 = cross2s1; + float cross3s2 = cross2s2; + float dots3 = cross3s0*pkdPts[1].x + cross3s1*pkdPts[1].y + cross3s2; + float cross3d0 = cross2d0; + float cross3d1 = cross2d1; + float cross3d2 = cross2d2; + float dotd3 = cross3d0*pkdPts[5].x + cross3d1*pkdPts[5].y + cross3d2; + if(((int)dots3^(int)dotd3) < 0){ + return 1; + } + + /* Otherwise, accept */ + return 0; +} + +/** + * Compute homography of matches in gathered, packed sample and output the + * current homography. + */ + +static inline void sacGenerateModel(RHO_HEST_REFC* p){ + hFuncRefC(p->curr.pkdPts, p->curr.H); +} + +/** + * Checks whether the model is itself degenerate. + * - One test: All elements of the homography are added, and if the result is + * NaN the homography is rejected. + */ + +static inline int sacIsModelDegenerate(RHO_HEST_REFC* p){ + int degenerate; + float* H = p->curr.H; + float f=H[0]+H[1]+H[2]+H[3]+H[4]+H[5]+H[6]+H[7]; + + /* degenerate = isnan(f); */ + degenerate = f!=f;/* Only NaN is not equal to itself. */ + /* degenerate = 0; */ + + + return degenerate; +} + +/** + * Evaluates the current model using SPRT for early exiting. + * + * Reads: arg.maxD, arg.src, arg.dst, curr.H, eval.* + * Writes: eval.*, curr.inl, curr.numInl + */ + +static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ + unsigned i; + unsigned isInlier; + double lambda = 1.0; + float distSq = p->arg.maxD*p->arg.maxD; + const float* src = p->arg.src; + const float* dst = p->arg.dst; + char* inl = p->curr.inl; + const float* H = p->curr.H; + + + p->ctrl.numModels++; + + p->curr.numInl = 0; + p->eval.Ntested = 0; + p->eval.good = 1; + + + /* SCALAR */ + for(i=0;iarg.N && p->eval.good;i++){ + /* Backproject */ + float x=src[i*2],y=src[i*2+1]; + float X=dst[i*2],Y=dst[i*2+1]; + + float reprojX=H[0]*x+H[1]*y+H[2]; /* ( X_1 ) ( H_11 H_12 H_13 ) (x_1) */ + float reprojY=H[3]*x+H[4]*y+H[5]; /* ( X_2 ) = ( H_21 H_22 H_23 ) (x_2) */ + float reprojZ=H[6]*x+H[7]*y+1.0; /* ( X_3 ) ( H_31 H_32 H_33=1.0 ) (x_3 = 1.0) */ + + /* reproj is in homogeneous coordinates. To bring back to "regular" coordinates, divide by Z. */ + reprojX/=reprojZ; + reprojY/=reprojZ; + + /* Compute distance */ + reprojX-=X; + reprojY-=Y; + reprojX*=reprojX; + reprojY*=reprojY; + float reprojDist = reprojX+reprojY; + + /* ... */ + isInlier = reprojDist <= distSq; + p->curr.numInl += isInlier; + *inl++ = isInlier; + + + /* SPRT */ + lambda *= isInlier ? p->eval.lambdaAccept : p->eval.lambdaReject; + p->eval.good = lambda <= p->eval.A; + /* If !p->good, the threshold A was exceeded, so we're rejecting */ + } + + + p->eval.Ntested = i; + p->eval.Ntestedtotal += i; +} + +/** + * Update either the delta or epsilon SPRT parameters, depending on the events + * that transpired in the previous evaluation. + * + * If a "good" model that is also the best was encountered, update epsilon, + * since + */ + +static inline void sacUpdateSPRT(RHO_HEST_REFC* p){ + if(p->eval.good){ + if(sacIsBestModel(p)){ + p->eval.epsilon = (double)p->curr.numInl/p->arg.N; + sacDesignSPRTTest(p); + } + }else{ + double newDelta = (double)p->curr.numInl/p->eval.Ntested; + + if(newDelta > 0 && CHNG_SIGNIFICANT(p->eval.delta, newDelta)){ + p->eval.delta = newDelta; + sacDesignSPRTTest(p); + } + } +} + +/** + * Numerically compute threshold A from the estimated delta, epsilon, t_M and + * m_S values. + * + * Epsilon: Denotes the probability that a randomly chosen data point is + * consistent with a good model. + * Delta: Denotes the probability that a randomly chosen data point is + * consistent with a bad model. + * t_M: Time needed to instantiate a model hypotheses given a sample. + * (Computing model parameters from a sample takes the same time + * as verification of t_M data points) + * m_S: The number of models that are verified per sample. + */ + +static inline double designSPRTTest(double delta, double epsilon, double t_M, double m_S){ + double An, C, K, prevAn; + unsigned i; + + /** + * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005 + * Eq (2) + */ + + C = (1-delta) * log((1-delta)/(1-epsilon)) + + delta * log( delta / epsilon ); + + /** + * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005 + * Eq (6) + * K = K_1/K_2 + 1 = (t_M*C)/m_S + 1 + */ + + K = t_M*C/m_S + 1; + + /** + * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005 + * Paragraph below Eq (6) + * + * A* = lim_{n -> infty} A_n, where + * A_0 = K1/K2 + 1 and + * A_{n+1} = K1/K2 + 1 + log(A_n) + * The series converges fast, typically within four iterations. + */ + + An = K; + i = 0; + + do{ + prevAn = An; + An = K + log(An); + }while((An-prevAn > 1.5e-8) && (++i < 10)); + + /** + * Return A = An_stopping, with n_stopping < 10 + */ + + return An; +} + +/** + * Design the SPRT test. Shorthand for + * A = sprt(delta, epsilon, t_M, m_S); + * + * Idempotent, reentrant, thread-safe. + * + * Reads: eval.delta, eval.epsilon, eval.t_M, eval.m_S + * Writes: eval.A, eval.lambdaAccept, eval.lambdaReject + */ + +static inline void sacDesignSPRTTest(RHO_HEST_REFC* p){ + p->eval.A = designSPRTTest(p->eval.delta, p->eval.epsilon, p->eval.t_M, p->eval.m_S); + p->eval.lambdaReject = ((1.0 - p->eval.delta) / (1.0 - p->eval.epsilon)); + p->eval.lambdaAccept = (( p->eval.delta ) / ( p->eval.epsilon )); +} + +/** + * Return whether the current model is the best model so far. + */ + +static inline int sacIsBestModel(RHO_HEST_REFC* p){ + return p->curr.numInl > p->best.numInl; +} + +/** + * Returns whether the current-best model is good enough to be an + * acceptable best model, by checking whether it meets the minimum + * number of inliers. + */ + +static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p){ + return p->best.numInl >= p->arg.minInl; +} + +/** + * Make current model new best model by swapping the homography, inlier mask + * and count of inliers between the current and best models. + */ + +static inline void sacSaveBestModel(RHO_HEST_REFC* p){ + float* H = p->curr.H; + char* inl = p->curr.inl; + unsigned numInl = p->curr.numInl; + p->curr.H = p->best.H; + p->curr.inl = p->best.inl; + p->curr.numInl = p->best.numInl; + p->best.H = H; + p->best.inl = inl; + p->best.numInl = numInl; +} + +/** + * Compute NR table entries [start, N) for given beta. + */ + +static inline void sacInitNonRand(double beta, + unsigned start, + unsigned N, + unsigned* nonRandMinInl){ + unsigned n = m+1 > start ? m+1 : start; + double beta_beta1_sq_chi = sqrt(beta*(1.0-beta)) * CHI_SQ; + + for(; n < N; n++){ + double mu = n * beta; + double sigma = sqrt(n)* beta_beta1_sq_chi; + unsigned i_min = ceil(m + mu + sigma); + + nonRandMinInl[n] = i_min; + } +} + +/** + * Optimize the stopping criterion to account for the non-randomness criterion + * of PROSAC. + */ + +static inline void sacNStarOptimize(RHO_HEST_REFC* p){ + unsigned min_sample_length = 10*2; /*(p->N * INLIERS_RATIO) */ + unsigned best_n = p->arg.N; + unsigned test_n = best_n; + unsigned bestNumInl = p->best.numInl; + unsigned testNumInl = bestNumInl; + + for(;test_n > min_sample_length && testNumInl;test_n--){ + if(testNumInl*best_n > bestNumInl*test_n){ + if(testNumInl < p->nr.tbl[test_n]){ + break; + } + best_n = test_n; + bestNumInl = testNumInl; + } + testNumInl -= !!p->arg.inl[test_n-1]; + } + + if(bestNumInl*p->ctrl.phMax > p->ctrl.phNumInl*best_n){ + p->ctrl.phMax = best_n; + p->ctrl.phNumInl = bestNumInl; + p->arg.maxI = sacCalcIterBound(p->arg.cfd, + (double)p->ctrl.phNumInl/p->ctrl.phMax, + m, + p->arg.maxI); + } +} + +/** + * Classic RANSAC iteration bound based on largest # of inliers. + */ + +static inline void sacUpdateBounds(RHO_HEST_REFC* p){ + p->arg.maxI = sacCalcIterBound(p->arg.cfd, + (double)p->best.numInl/p->arg.N, + m, + p->arg.maxI); +} + +/** + * Ouput the best model so far to the output argument. + */ + +static inline void sacOutputModel(RHO_HEST_REFC* p){ + if(sacIsBestModelGoodEnough(p)){ + memcpy(p->arg.finalH, p->best.H, HSIZE); + }else{ + sacOutputZeroH(p); + } +} + +/** + * Ouput a zeroed H to the output argument. + */ + +static inline void sacOutputZeroH(RHO_HEST_REFC* p){ + memset(p->arg.finalH, 0, HSIZE); +} + +/** + * Compute the real-valued number of samples per phase, given the RANSAC convergence speed, + * data set size and sample size. + */ + +static inline double sacInitPEndFpI(const unsigned ransacConvg, + const unsigned n, + const unsigned s){ + double numer=1, denom=1; + + unsigned i; + for(i=0;idataSetSize){ + /** + * Selection Sampling: + * + * Algorithm S (Selection sampling technique). To select n records at random from a set of N, where 0 < n ≤ N. + * S1. [Initialize.] Set t ← 0, m ← 0. (During this algorithm, m represents the number of records selected so far, + * and t is the total number of input records that we have dealt with.) + * S2. [Generate U.] Generate a random number U, uniformly distributed between zero and one. + * S3. [Test.] If (N – t)U ≥ n – m, go to step S5. + * S4. [Select.] Select the next record for the sample, and increase m and t by 1. If m < n, go to step S2; + * otherwise the sample is complete and the algorithm terminates. + * S5. [Skip.] Skip the next record (do not include it in the sample), increase t by 1, and go back to step S2. + * + * Replaced m with i and t with j in the below code. + */ + + unsigned i=0,j=0; + + for(i=0;i=1.){ + /** + * A certainty of picking at least one outlier means that we will need + * an infinite amount of iterations in order to find a correct solution. + */ + + retVal = maxIterBound; + }else if(atLeastOneOutlierProbability<=0.){ + /** + * The certainty of NOT picking an outlier means that only 1 iteration + * is needed to find a solution. + */ + + retVal = 1; + }else{ + /** + * Since 1-confidence (the probability of the model being based on at + * least one outlier in the data) is equal to + * (1-inlierRate**sampleSize)**numIterations (the probability of picking + * at least one outlier in numIterations samples), we can isolate + * numIterations (the return value) into + */ + + retVal = ceil(log(1.-confidence)/log(atLeastOneOutlierProbability)); + } + + /** + * Clamp to maxIterationBound. + */ + + return retVal <= maxIterBound ? retVal : maxIterBound; +} + + +/** + * Given 4 matches, computes the homography that relates them using Gaussian + * Elimination. The row operations are as given in the paper. + * + * TODO: Clean this up. The code is hideous, and might even conceal sign bugs + * (specifically relating to whether the last column should be negated, + * or not). + */ + +static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates) points followed by + destination (four x,y float coordinates) points, aligned by 32 bytes */ + float* H){ /* Homography (three 16-byte aligned rows of 3 floats) */ + float x0=*packedPoints++; + float y0=*packedPoints++; + float x1=*packedPoints++; + float y1=*packedPoints++; + float x2=*packedPoints++; + float y2=*packedPoints++; + float x3=*packedPoints++; + float y3=*packedPoints++; + float X0=*packedPoints++; + float Y0=*packedPoints++; + float X1=*packedPoints++; + float Y1=*packedPoints++; + float X2=*packedPoints++; + float Y2=*packedPoints++; + float X3=*packedPoints++; + float Y3=*packedPoints++; + + float x0X0=x0*X0, x1X1=x1*X1, x2X2=x2*X2, x3X3=x3*X3; + float x0Y0=x0*Y0, x1Y1=x1*Y1, x2Y2=x2*Y2, x3Y3=x3*Y3; + float y0X0=y0*X0, y1X1=y1*X1, y2X2=y2*X2, y3X3=y3*X3; + float y0Y0=y0*Y0, y1Y1=y1*Y1, y2Y2=y2*Y2, y3Y3=y3*Y3; + + + /** + * [0] [1] Hidden Prec + * x0 y0 1 x1 + * x1 y1 1 x1 + * x2 y2 1 x1 + * x3 y3 1 x1 + * + * Eliminate ones in column 2 and 5. + * R(0)-=R(2) + * R(1)-=R(2) + * R(3)-=R(2) + * + * [0] [1] Hidden Prec + * x0-x2 y0-y2 0 x1+1 + * x1-x2 y1-y2 0 x1+1 + * x2 y2 1 x1 + * x3-x2 y3-y2 0 x1+1 + * + * Eliminate column 0 of rows 1 and 3 + * R(1)=(x0-x2)*R(1)-(x1-x2)*R(0), y1'=(y1-y2)(x0-x2)-(x1-x2)(y0-y2) + * R(3)=(x0-x2)*R(3)-(x3-x2)*R(0), y3'=(y3-y2)(x0-x2)-(x3-x2)(y0-y2) + * + * [0] [1] Hidden Prec + * x0-x2 y0-y2 0 x1+1 + * 0 y1' 0 x2+3 + * x2 y2 1 x1 + * 0 y3' 0 x2+3 + * + * Eliminate column 1 of rows 0 and 3 + * R(3)=y1'*R(3)-y3'*R(1) + * R(0)=y1'*R(0)-(y0-y2)*R(1) + * + * [0] [1] Hidden Prec + * x0' 0 0 x3+5 + * 0 y1' 0 x2+3 + * x2 y2 1 x1 + * 0 0 0 x4+7 + * + * Eliminate columns 0 and 1 of row 2 + * R(0)/=x0' + * R(1)/=y1' + * R(2)-= (x2*R(0) + y2*R(1)) + * + * [0] [1] Hidden Prec + * 1 0 0 x6+10 + * 0 1 0 x4+6 + * 0 0 1 x4+7 + * 0 0 0 x4+7 + */ + + /** + * Eliminate ones in column 2 and 5. + * R(0)-=R(2) + * R(1)-=R(2) + * R(3)-=R(2) + */ + + /*float minor[4][2] = {{x0-x2,y0-y2}, + {x1-x2,y1-y2}, + {x2 ,y2 }, + {x3-x2,y3-y2}};*/ + /*float major[8][3] = {{x2X2-x0X0,y2X2-y0X0,(X0-X2)}, + {x2X2-x1X1,y2X2-y1X1,(X1-X2)}, + {-x2X2 ,-y2X2 ,(X2 )}, + {x2X2-x3X3,y2X2-y3X3,(X3-X2)}, + {x2Y2-x0Y0,y2Y2-y0Y0,(Y0-Y2)}, + {x2Y2-x1Y1,y2Y2-y1Y1,(Y1-Y2)}, + {-x2Y2 ,-y2Y2 ,(Y2 )}, + {x2Y2-x3Y3,y2Y2-y3Y3,(Y3-Y2)}};*/ + float minor[2][4] = {{x0-x2,x1-x2,x2 ,x3-x2}, + {y0-y2,y1-y2,y2 ,y3-y2}}; + float major[3][8] = {{x2X2-x0X0,x2X2-x1X1,-x2X2 ,x2X2-x3X3,x2Y2-x0Y0,x2Y2-x1Y1,-x2Y2 ,x2Y2-x3Y3}, + {y2X2-y0X0,y2X2-y1X1,-y2X2 ,y2X2-y3X3,y2Y2-y0Y0,y2Y2-y1Y1,-y2Y2 ,y2Y2-y3Y3}, + { (X0-X2) , (X1-X2) , (X2 ) , (X3-X2) , (Y0-Y2) , (Y1-Y2) , (Y2 ) , (Y3-Y2) }}; + + /** + * int i; + * for(i=0;i<8;i++) major[2][i]=-major[2][i]; + * Eliminate column 0 of rows 1 and 3 + * R(1)=(x0-x2)*R(1)-(x1-x2)*R(0), y1'=(y1-y2)(x0-x2)-(x1-x2)(y0-y2) + * R(3)=(x0-x2)*R(3)-(x3-x2)*R(0), y3'=(y3-y2)(x0-x2)-(x3-x2)(y0-y2) + */ + + float scalar1=minor[0][0], scalar2=minor[0][1]; + minor[1][1]=minor[1][1]*scalar1-minor[1][0]*scalar2; + + major[0][1]=major[0][1]*scalar1-major[0][0]*scalar2; + major[1][1]=major[1][1]*scalar1-major[1][0]*scalar2; + major[2][1]=major[2][1]*scalar1-major[2][0]*scalar2; + + major[0][5]=major[0][5]*scalar1-major[0][4]*scalar2; + major[1][5]=major[1][5]*scalar1-major[1][4]*scalar2; + major[2][5]=major[2][5]*scalar1-major[2][4]*scalar2; + + scalar2=minor[0][3]; + minor[1][3]=minor[1][3]*scalar1-minor[1][0]*scalar2; + + major[0][3]=major[0][3]*scalar1-major[0][0]*scalar2; + major[1][3]=major[1][3]*scalar1-major[1][0]*scalar2; + major[2][3]=major[2][3]*scalar1-major[2][0]*scalar2; + + major[0][7]=major[0][7]*scalar1-major[0][4]*scalar2; + major[1][7]=major[1][7]*scalar1-major[1][4]*scalar2; + major[2][7]=major[2][7]*scalar1-major[2][4]*scalar2; + + /** + * Eliminate column 1 of rows 0 and 3 + * R(3)=y1'*R(3)-y3'*R(1) + * R(0)=y1'*R(0)-(y0-y2)*R(1) + */ + + scalar1=minor[1][1];scalar2=minor[1][3]; + major[0][3]=major[0][3]*scalar1-major[0][1]*scalar2; + major[1][3]=major[1][3]*scalar1-major[1][1]*scalar2; + major[2][3]=major[2][3]*scalar1-major[2][1]*scalar2; + + major[0][7]=major[0][7]*scalar1-major[0][5]*scalar2; + major[1][7]=major[1][7]*scalar1-major[1][5]*scalar2; + major[2][7]=major[2][7]*scalar1-major[2][5]*scalar2; + + scalar2=minor[1][0]; + minor[0][0]=minor[0][0]*scalar1-minor[0][1]*scalar2; + + major[0][0]=major[0][0]*scalar1-major[0][1]*scalar2; + major[1][0]=major[1][0]*scalar1-major[1][1]*scalar2; + major[2][0]=major[2][0]*scalar1-major[2][1]*scalar2; + + major[0][4]=major[0][4]*scalar1-major[0][5]*scalar2; + major[1][4]=major[1][4]*scalar1-major[1][5]*scalar2; + major[2][4]=major[2][4]*scalar1-major[2][5]*scalar2; + + /** + * Eliminate columns 0 and 1 of row 2 + * R(0)/=x0' + * R(1)/=y1' + * R(2)-= (x2*R(0) + y2*R(1)) + */ + + scalar1=minor[0][0]; + major[0][0]/=scalar1; + major[1][0]/=scalar1; + major[2][0]/=scalar1; + major[0][4]/=scalar1; + major[1][4]/=scalar1; + major[2][4]/=scalar1; + + scalar1=minor[1][1]; + major[0][1]/=scalar1; + major[1][1]/=scalar1; + major[2][1]/=scalar1; + major[0][5]/=scalar1; + major[1][5]/=scalar1; + major[2][5]/=scalar1; + + + scalar1=minor[0][2];scalar2=minor[1][2]; + major[0][2]-=major[0][0]*scalar1+major[0][1]*scalar2; + major[1][2]-=major[1][0]*scalar1+major[1][1]*scalar2; + major[2][2]-=major[2][0]*scalar1+major[2][1]*scalar2; + + major[0][6]-=major[0][4]*scalar1+major[0][5]*scalar2; + major[1][6]-=major[1][4]*scalar1+major[1][5]*scalar2; + major[2][6]-=major[2][4]*scalar1+major[2][5]*scalar2; + + /* Only major matters now. R(3) and R(7) correspond to the hollowed-out rows. */ + scalar1=major[0][7]; + major[1][7]/=scalar1; + major[2][7]/=scalar1; + + scalar1=major[0][0];major[1][0]-=scalar1*major[1][7];major[2][0]-=scalar1*major[2][7]; + scalar1=major[0][1];major[1][1]-=scalar1*major[1][7];major[2][1]-=scalar1*major[2][7]; + scalar1=major[0][2];major[1][2]-=scalar1*major[1][7];major[2][2]-=scalar1*major[2][7]; + scalar1=major[0][3];major[1][3]-=scalar1*major[1][7];major[2][3]-=scalar1*major[2][7]; + scalar1=major[0][4];major[1][4]-=scalar1*major[1][7];major[2][4]-=scalar1*major[2][7]; + scalar1=major[0][5];major[1][5]-=scalar1*major[1][7];major[2][5]-=scalar1*major[2][7]; + scalar1=major[0][6];major[1][6]-=scalar1*major[1][7];major[2][6]-=scalar1*major[2][7]; + + + /* One column left (Two in fact, but the last one is the homography) */ + scalar1=major[1][3]; + + major[2][3]/=scalar1; + scalar1=major[1][0];major[2][0]-=scalar1*major[2][3]; + scalar1=major[1][1];major[2][1]-=scalar1*major[2][3]; + scalar1=major[1][2];major[2][2]-=scalar1*major[2][3]; + scalar1=major[1][4];major[2][4]-=scalar1*major[2][3]; + scalar1=major[1][5];major[2][5]-=scalar1*major[2][3]; + scalar1=major[1][6];major[2][6]-=scalar1*major[2][3]; + scalar1=major[1][7];major[2][7]-=scalar1*major[2][3]; + + + /* Homography is done. */ + H[0]=major[2][0]; + H[1]=major[2][1]; + H[2]=major[2][2]; + + H[3]=major[2][4]; + H[4]=major[2][5]; + H[5]=major[2][6]; + + H[6]=major[2][7]; + H[7]=major[2][3]; + H[8]=1.0; +} + +/** + * Returns whether refinement is possible. + * + * NB This is separate from whether it is *enabled*. + */ + +static inline int sacCanRefine(RHO_HEST_REFC* p){ + /** + * If we only have 4 matches, GE's result is already optimal and cannot + * be refined any further. + */ + + return p->best.numInl > m; +} + +/** + * Refines the best-so-far homography. + * + * BUG: Totally broken for now. DO NOT ENABLE. + */ + +static inline void sacRefine(RHO_HEST_REFC* p){ + int i, j; + float S; /* Sum of squared errors */ + float L = 1;/* Lambda of LevMarq */ + + for(i=0;ilm.JtJ, p->lm.Jte, &S, p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N); + sacScaleDiag8x8(p->lm.JtJ, L, p->lm.JtJ); + sacChol8x8(p->lm.JtJ, p->lm.JtJ); + sacTRInv8x8(p->lm.JtJ, p->lm.JtJ); + sacTRISolve8x8(p->lm.JtJ, p->lm.Jte, dH); + sacSub8x1(p->best.H, p->best.H, dH); + } +} + +/** + * Compute directly the JtJ, Jte and sum-of-squared-error for a given + * homography and set of inliers. + * + * This is possible because the product of J and its transpose as well as with + * the error and the sum-of-squared-error can all be computed additively + * (match-by-match), as one would intuitively expect; All matches make + * contributions to the error independently of each other. + * + * What this allows is a constant-space implementation of Lev-Marq that can + * nevertheless be vectorized if need be. + * + * @param JtJ + * @param Jte + * @param Sp + * @param H + * @param src + * @param dst + * @param inl + * @param N + */ + +static inline void sacCalcJtMats(float (* restrict JtJ)[8], + float* restrict Jte, + float* restrict Sp, + const float* restrict H, + const float* restrict src, + const float* restrict dst, + const char* restrict inl, + unsigned N){ + unsigned i; + float S; + + /* Zero out JtJ, Jte and S */ + memset(JtJ, 0, 8*8*sizeof(*JtJ)); + memset(Jte, 0, 8*1*sizeof(*Jte)); + S = 0.0f; + + /* Additively compute JtJ and Jte */ + for(i=0;i + * 0 U22 0 U22^-1 + * + * Becomes + * + * L11 0 L11^-1 0 + * -> + * L21 L22 -L22^-1*L21*L11^-1 L22^-1 + * + * Since + * + * ( -L11^T^-1*L21^T*L22^T^-1 )^T = -L22^T^-1^T*L21^T^T*L11^T^-1^T + * = -L22^T^T^-1*L21^T^T*L11^T^T^-1 + * = -L22^-1*L21*L11^-1 + */ + +static inline void sacTRInv8x8(const float (*L)[8], + float (*M)[8]){ + float s[2][2], t[2][2]; + float u[4][4], v[4][4]; + + /* + L00 0 0 0 0 0 0 0 + L10 L11 0 0 0 0 0 0 + L20 L21 L22 0 0 0 0 0 + L30 L31 L32 L33 0 0 0 0 + L40 L41 L42 L43 L44 0 0 0 + L50 L51 L52 L53 L54 L55 0 0 + L60 L61 L62 L63 L64 L65 L66 0 + L70 L71 L72 L73 L74 L75 L76 L77 + */ + + /* Invert 4*2 1x1 matrices; Starts recursion. */ + M[0][0] = 1.0f/L[0][0]; + M[1][1] = 1.0f/L[1][1]; + M[2][2] = 1.0f/L[2][2]; + M[3][3] = 1.0f/L[3][3]; + M[4][4] = 1.0f/L[4][4]; + M[5][5] = 1.0f/L[5][5]; + M[6][6] = 1.0f/L[6][6]; + M[7][7] = 1.0f/L[7][7]; + + /* + M00 0 0 0 0 0 0 0 + L10 M11 0 0 0 0 0 0 + L20 L21 M22 0 0 0 0 0 + L30 L31 L32 M33 0 0 0 0 + L40 L41 L42 L43 M44 0 0 0 + L50 L51 L52 L53 L54 M55 0 0 + L60 L61 L62 L63 L64 L65 M66 0 + L70 L71 L72 L73 L74 L75 L76 M77 + */ + + /* 4*2 Matrix products of 1x1 matrices */ + M[1][0] = -M[1][1]*L[1][0]*M[0][0]; + M[3][2] = -M[3][3]*L[3][2]*M[2][2]; + M[5][4] = -M[5][5]*L[5][4]*M[4][4]; + M[7][6] = -M[7][7]*L[7][6]*M[6][6]; + + /* + M00 0 0 0 0 0 0 0 + M10 M11 0 0 0 0 0 0 + L20 L21 M22 0 0 0 0 0 + L30 L31 M32 M33 0 0 0 0 + L40 L41 L42 L43 M44 0 0 0 + L50 L51 L52 L53 M54 M55 0 0 + L60 L61 L62 L63 L64 L65 M66 0 + L70 L71 L72 L73 L74 L75 M76 M77 + */ + + /* 2*2 Matrix products of 2x2 matrices */ + + /* + (M22 0 ) (L20 L21) (M00 0 ) + - (M32 M33) x (L30 L31) x (M10 M11) + */ + + s[0][0] = M[2][2]*L[2][0]; + s[0][1] = M[2][2]*L[2][1]; + s[1][0] = M[3][2]*L[2][0]+M[3][3]*L[3][0]; + s[1][1] = M[3][2]*L[2][1]+M[3][3]*L[3][1]; + + t[0][0] = s[0][0]*M[0][0]+s[0][1]*M[1][0]; + t[0][1] = s[0][1]*M[1][1]; + t[1][0] = s[1][0]*M[0][0]+s[1][1]*M[1][0]; + t[1][1] = s[1][1]*M[1][1]; + + M[2][0] = -t[0][0]; + M[2][1] = -t[0][1]; + M[3][0] = -t[1][0]; + M[3][1] = -t[1][1]; + + /* + (M66 0 ) (L64 L65) (M44 0 ) + - (L76 M77) x (L74 L75) x (M54 M55) + */ + + s[0][0] = M[6][6]*L[6][4]; + s[0][1] = M[6][6]*L[6][5]; + s[1][0] = M[7][6]*L[6][4]+M[7][7]*L[7][4]; + s[1][1] = M[7][6]*L[6][5]+M[7][7]*L[7][5]; + + t[0][0] = s[0][0]*M[4][4]+s[0][1]*M[5][4]; + t[0][1] = s[0][1]*M[5][5]; + t[1][0] = s[1][0]*M[4][4]+s[1][1]*M[5][4]; + t[1][1] = s[1][1]*M[5][5]; + + M[6][4] = -t[0][0]; + M[6][5] = -t[0][1]; + M[7][4] = -t[1][0]; + M[7][5] = -t[1][1]; + + /* + M00 0 0 0 0 0 0 0 + M10 M11 0 0 0 0 0 0 + M20 M21 M22 0 0 0 0 0 + M30 M31 M32 M33 0 0 0 0 + L40 L41 L42 L43 M44 0 0 0 + L50 L51 L52 L53 M54 M55 0 0 + L60 L61 L62 L63 M64 M65 M66 0 + L70 L71 L72 L73 M74 M75 M76 M77 + */ + + /* 1*2 Matrix products of 4x4 matrices */ + + /* + (M44 0 0 0 ) (L40 L41 L42 L43) (M00 0 0 0 ) + (M54 M55 0 0 ) (L50 L51 L52 L53) (M10 M11 0 0 ) + (M64 M65 M66 0 ) (L60 L61 L62 L63) (M20 M21 M22 0 ) + - (M74 M75 M76 M77) x (L70 L71 L72 L73) x (M30 M31 M32 M33) + */ + + u[0][0] = M[4][4]*L[4][0]; + u[0][1] = M[4][4]*L[4][1]; + u[0][2] = M[4][4]*L[4][2]; + u[0][3] = M[4][4]*L[4][3]; + u[1][0] = M[5][4]*L[4][0]+M[5][5]*L[5][0]; + u[1][1] = M[5][4]*L[4][1]+M[5][5]*L[5][1]; + u[1][2] = M[5][4]*L[4][2]+M[5][5]*L[5][2]; + u[1][3] = M[5][4]*L[4][3]+M[5][5]*L[5][3]; + u[2][0] = M[6][4]*L[4][0]+M[6][5]*L[5][0]+M[6][6]*L[6][0]; + u[2][1] = M[6][4]*L[4][1]+M[6][5]*L[5][1]+M[6][6]*L[6][1]; + u[2][2] = M[6][4]*L[4][2]+M[6][5]*L[5][2]+M[6][6]*L[6][2]; + u[2][3] = M[6][4]*L[4][3]+M[6][5]*L[5][3]+M[6][6]*L[6][3]; + 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]; + 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]; + 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]; + 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]; + + 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]; + v[0][1] = u[0][1]*M[1][1]+u[0][2]*M[2][1]+u[0][3]*M[3][1]; + v[0][2] = u[0][2]*M[2][2]+u[0][3]*M[3][2]; + v[0][3] = u[0][3]*M[3][3]; + 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]; + v[1][1] = u[1][1]*M[1][1]+u[1][2]*M[2][1]+u[1][3]*M[3][1]; + v[1][2] = u[1][2]*M[2][2]+u[1][3]*M[3][2]; + v[1][3] = u[1][3]*M[3][3]; + 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]; + v[2][1] = u[2][1]*M[1][1]+u[2][2]*M[2][1]+u[2][3]*M[3][1]; + v[2][2] = u[2][2]*M[2][2]+u[2][3]*M[3][2]; + v[2][3] = u[2][3]*M[3][3]; + 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]; + v[3][1] = u[3][1]*M[1][1]+u[3][2]*M[2][1]+u[3][3]*M[3][1]; + v[3][2] = u[3][2]*M[2][2]+u[3][3]*M[3][2]; + v[3][3] = u[3][3]*M[3][3]; + + M[4][0] = -v[0][0]; + M[4][1] = -v[0][1]; + M[4][2] = -v[0][2]; + M[4][3] = -v[0][3]; + M[5][0] = -v[1][0]; + M[5][1] = -v[1][1]; + M[5][2] = -v[1][2]; + M[5][3] = -v[1][3]; + M[6][0] = -v[2][0]; + M[6][1] = -v[2][1]; + M[6][2] = -v[2][2]; + M[6][3] = -v[2][3]; + M[7][0] = -v[3][0]; + M[7][1] = -v[3][1]; + M[7][2] = -v[3][2]; + M[7][3] = -v[3][3]; + + /* + M00 0 0 0 0 0 0 0 + M10 M11 0 0 0 0 0 0 + M20 M21 M22 0 0 0 0 0 + M30 M31 M32 M33 0 0 0 0 + M40 M41 M42 M43 M44 0 0 0 + M50 M51 M52 M53 M54 M55 0 0 + M60 M61 M62 M63 M64 M65 M66 0 + M70 M71 M72 M73 M74 M75 M76 M77 + */ +} + +/** + * Solves dH = inv(JtJ) Jte. The argument lower-triangular matrix is the + * inverse of L as produced by the Cholesky decomposition LL^T of the matrix + * JtJ; Thus the operation performed here is a left-multiplication of a vector + * by two triangular matrices. The math is below: + * + * JtJ = LL^T + * Linv = L^-1 + * (JtJ)^-1 = (LL^T)^-1 + * = (L^T^-1)(Linv) + * = (Linv^T)(Linv) + * dH = ((JtJ)^-1) (Jte) + * = (Linv^T)(Linv) (Jte) + * + * where J is nx8, Jt is 8xn, JtJ is 8x8 PD, e is nx1, Jte is 8x1, L is lower + * triangular 8x8 and dH is 8x1. + */ + +static inline void sacTRISolve8x8(const float (*L)[8], + const float* Jte, + float* dH){ + float t[8]; + + t[0] = L[0][0]*Jte[0]; + t[1] = L[1][0]*Jte[0]+L[1][1]*Jte[1]; + t[2] = L[2][0]*Jte[0]+L[2][1]*Jte[1]+L[2][2]*Jte[2]; + t[3] = L[3][0]*Jte[0]+L[3][1]*Jte[1]+L[3][2]*Jte[2]+L[3][3]*Jte[3]; + 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]; + 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]; + 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]; + 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]; + + + 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]; + 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]; + 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]; + 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]; + dH[4] = L[4][4]*t[4]+L[5][4]*t[5]+L[6][4]*t[6]+L[7][4]*t[7]; + dH[5] = L[5][5]*t[5]+L[6][5]*t[6]+L[7][5]*t[7]; + dH[6] = L[6][6]*t[6]+L[7][6]*t[7]; + dH[7] = L[7][7]*t[7]; +} + +/** + * Multiply the diagonal elements of the 8x8 matrix A by 1+lambda and store to + * B. + * + * A and B may overlap exactly or not at all; Partial overlap is forbidden. + */ + +static inline void sacScaleDiag8x8(const float (*A)[8], + float lambda, + float (*B)[8]){ + float lambdap1 = lambda + 1.0f; + int i; + + if(A != B){ + memcpy((void*)B, (void*)A, 8*8*sizeof(float)); + } + + for(i=0;i<8;i++){ + B[i][i] *= lambdap1; + } +} + +/** + * Subtract dH from H. + */ + +static inline void sacSub8x1(float* Hout, const float* H, const float* dH){ + Hout[0] = H[0] - dH[0]; + Hout[1] = H[1] - dH[1]; + Hout[2] = H[2] - dH[2]; + Hout[3] = H[3] - dH[3]; + Hout[4] = H[4] - dH[4]; + Hout[5] = H[5] - dH[5]; + Hout[6] = H[6] - dH[6]; + Hout[7] = H[7] - dH[7]; +} + + + +#ifdef __cplusplus +} +#endif diff --git a/modules/calib3d/src/rhorefc.h b/modules/calib3d/src/rhorefc.h new file mode 100644 index 0000000000000000000000000000000000000000..3c0014188f7118d28e2e3ac6d81b333fd773b8ad --- /dev/null +++ b/modules/calib3d/src/rhorefc.h @@ -0,0 +1,368 @@ +/* + IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + + By downloading, copying, installing or using the software you agree to this license. + If you do not agree to this license, do not download, install, + copy or use the software. + + + BSD 3-Clause License + + Copyright (C) 2014, Olexa Bilaniuk, Hamid Bazargani & Robert Laganiere, all rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistribution's of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistribution's in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * The name of the copyright holders may not be used to endorse or promote products + derived from this software without specific prior written permission. + + This software is provided by the copyright holders and contributors "as is" and + any express or implied warranties, including, but not limited to, the implied + warranties of merchantability and fitness for a particular purpose are disclaimed. + In no event shall the Intel Corporation or contributors be liable for any direct, + indirect, incidental, special, exemplary, or consequential damages + (including, but not limited to, procurement of substitute goods or services; + loss of use, data, or profits; or business interruption) however caused + and on any theory of liability, whether in contract, strict liability, + or tort (including negligence or otherwise) arising in any way out of + the use of this software, even if advised of the possibility of such damage. +*/ + +/** + * Bilaniuk, Olexa, Hamid Bazargani, and Robert Laganiere. "Fast Target + * Recognition on Mobile Devices: Revisiting Gaussian Elimination for the + * Estimation of Planar Homographies." In Computer Vision and Pattern + * Recognition Workshops (CVPRW), 2014 IEEE Conference on, pp. 119-125. + * IEEE, 2014. + */ + +/* Include Guards */ +#ifndef __RHOREFC_H__ +#define __RHOREFC_H__ + + + +/* Includes */ + + + + + +/* Defines */ +#ifdef __cplusplus + +/* C++ does not have the restrict keyword. */ +#ifdef restrict +#undef restrict +#endif +#define restrict + +#else + +/* C99 and over has the restrict keyword. */ +#if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L +#define restrict +#endif + +#endif + + +/* Flags */ +#ifndef RHO_FLAG_NONE +#define RHO_FLAG_NONE (0U<<0) +#endif +#ifndef RHO_FLAG_ENABLE_NR +#define RHO_FLAG_ENABLE_NR (1U<<0) +#endif +#ifndef RHO_FLAG_ENABLE_REFINEMENT +#define RHO_FLAG_ENABLE_REFINEMENT (1U<<1) +#endif +#ifndef RHO_FLAG_ENABLE_FINAL_REFINEMENT +#define RHO_FLAG_ENABLE_FINAL_REFINEMENT (1U<<2) +#endif + + + +/* Data structures */ + +/** + * Homography Estimation context. + */ + +typedef struct{ + /** + * Virtual Arguments. + * + * Exactly the same as at function call, except: + * - minInl is enforced to be >= 4. + */ + + struct{ + const float* restrict src; + const float* restrict dst; + char* restrict inl; + unsigned N; + float maxD; + unsigned maxI; + unsigned rConvg; + double cfd; + unsigned minInl; + double beta; + unsigned flags; + const float* guessH; + float* finalH; + } arg; + + /* PROSAC Control */ + struct{ + unsigned i; /* Iteration Number */ + unsigned phNum; /* Phase Number */ + unsigned phEndI; /* Phase End Iteration */ + double phEndFpI; /* Phase floating-point End Iteration */ + unsigned phMax; /* Termination phase number */ + unsigned phNumInl; /* Number of inliers for termination phase */ + unsigned numModels; /* Number of models tested */ + unsigned* restrict smpl; /* Sample of match indexes */ + } ctrl; + + /* Current model being tested */ + struct{ + float* restrict pkdPts; /* Packed points */ + float* restrict H; /* Homography */ + char* restrict inl; /* Mask of inliers */ + unsigned numInl; /* Number of inliers */ + } curr; + + /* Best model (so far) */ + struct{ + float* restrict H; /* Homography */ + char* restrict inl; /* Mask of inliers */ + unsigned numInl; /* Number of inliers */ + } best; + + /* Non-randomness criterion */ + struct{ + unsigned* restrict tbl; /* Non-Randomness: Table */ + unsigned size; /* Non-Randomness: Size */ + double beta; /* Non-Randomness: Beta */ + } nr; + + /* SPRT Evaluator */ + struct{ + double t_M; /* t_M */ + double m_S; /* m_S */ + double epsilon; /* Epsilon */ + double delta; /* delta */ + double A; /* SPRT Threshold */ + unsigned Ntested; /* Number of points tested */ + unsigned Ntestedtotal; /* Number of points tested in total */ + int good; /* Good/bad flag */ + double lambdaAccept; /* Accept multiplier */ + double lambdaReject; /* Reject multiplier */ + } eval; + + /* Levenberg-Marquardt Refinement */ + struct{ + float* ws; /* Levenberg-Marqhard Workspace */ + float (* restrict JtJ)[8]; /* JtJ matrix */ + float (* restrict tmp1)[8]; /* Temporary 1 */ + float (* restrict tmp2)[8]; /* Temporary 2 */ + float* restrict Jte; /* Jte vector */ + } lm; +} RHO_HEST_REFC; + + + +/* Extern C */ +#ifdef __cplusplus +namespace cv{ +/* extern "C" { */ +#endif + + + +/* Functions */ + +/** + * Initialize the estimator context, by allocating the aligned buffers + * internally needed. + * + * @param [in/out] p The uninitialized estimator context to initialize. + * @return 0 if successful; non-zero if an error occured. + */ + +int rhoRefCInit(RHO_HEST_REFC* p); + + +/** + * Ensure that the estimator context's internal table for non-randomness + * criterion is at least of the given size, and uses the given beta. The table + * should be larger than the maximum number of matches fed into the estimator. + * + * A value of N of 0 requests deallocation of the table. + * + * @param [in] p The initialized estimator context + * @param [in] N If 0, deallocate internal table. If > 0, ensure that the + * internal table is of at least this size, reallocating if + * necessary. + * @param [in] beta The beta-factor to use within the table. + * @return 0 if successful; non-zero if an error occured. + */ + +int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta); + + + + +/** + * Finalize the estimator context, by freeing the aligned buffers used + * internally. + * + * @param [in] p The initialized estimator context to finalize. + */ + +void rhoRefCFini(RHO_HEST_REFC* p); + + +/** + * Estimates the homography using the given context, matches and parameters to + * PROSAC. + * + * The given context must have been initialized. + * + * The matches are provided as two arrays of N single-precision, floating-point + * (x,y) points. Points with corresponding offsets in the two arrays constitute + * a match. The homography estimation attempts to find the 3x3 matrix H which + * best maps the homogeneous-coordinate points in the source array to their + * corresponding homogeneous-coordinate points in the destination array. + * + * Note: At least 4 matches must be provided (N >= 4). + * Note: A point in either array takes up 2 floats. The first of two stores + * the x-coordinate and the second of the two stores the y-coordinate. + * Thus, the arrays resemble this in memory: + * + * src = [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...] + * Matches: | | | | | + * dst = [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...] + * Note: The matches are expected to be provided sorted by quality, or at + * least not be worse-than-random in ordering. + * + * A pointer to the base of an array of N bytes can be provided. It serves as + * an output mask to indicate whether the corresponding match is an inlier to + * the returned homography, if any. A zero indicates an outlier; A non-zero + * value indicates an inlier. + * + * The PROSAC estimator requires a few parameters of its own. These are: + * + * - The maximum distance that a source point projected onto the destination + * plane can be from its putative match and still be considered an + * inlier. Must be non-negative. + * A sane default is 3.0. + * - The maximum number of PROSAC iterations. This corresponds to the + * largest number of samples that will be drawn and tested. + * A sane default is 2000. + * - The RANSAC convergence parameter. This corresponds to the number of + * iterations after which PROSAC will start sampling like RANSAC. + * A sane default is 2000. + * - The confidence threshold. This corresponds to the probability of + * finding a correct solution. Must be bounded by [0, 1]. + * A sane default is 0.995. + * - The minimum number of inliers acceptable. Only a solution with at + * least this many inliers will be returned. The minimum is 4. + * A sane default is 10% of N. + * - The beta-parameter for the non-randomness termination criterion. + * Ignored if non-randomness criterion disabled, otherwise must be + * bounded by (0, 1). + * A sane default is 0.35. + * - Optional flags to control the estimation. Available flags are: + * HEST_FLAG_NONE: + * No special processing. + * HEST_FLAG_ENABLE_NR: + * Enable non-randomness criterion. If set, the beta parameter + * must also be set. + * HEST_FLAG_ENABLE_REFINEMENT: + * Enable refinement of each new best model, as they are found. + * HEST_FLAG_ENABLE_FINAL_REFINEMENT: + * Enable one final refinement of the best model found before + * returning it. + * + * The PROSAC estimator optionally accepts an extrinsic initial guess of H. + * + * The PROSAC estimator outputs a final estimate of H provided it was able to + * find one with a minimum of supporting inliers. If it was not, it outputs + * the all-zero matrix. + * + * The extrinsic guess at and final estimate of H are both in the same form: + * A 3x3 single-precision floating-point matrix with step 3. Thus, it is a + * 9-element array of floats, with the elements as follows: + * + * [ H00, H01, H02, + * H10, H11, H12, + * H20, H21, 1.0 ] + * + * Notice that the homography is normalized to H22 = 1.0. + * + * The function returns the number of inliers if it was able to find a + * homography with at least the minimum required support, and 0 if it was not. + * + * + * @param [in/out] p The context to use for homography estimation. Must + * be already initialized. Cannot be NULL. + * @param [in] src The pointer to the source points of the matches. + * Must be aligned to 4 bytes. Cannot be NULL. + * @param [in] dst The pointer to the destination points of the matches. + * Must be aligned to 4 bytes. Cannot be NULL. + * @param [out] inl The pointer to the output mask of inlier matches. + * Must be aligned to 4 bytes. May be NULL. + * @param [in] N The number of matches. Minimum 4. + * @param [in] maxD The maximum distance. Minimum 0. + * @param [in] maxI The maximum number of PROSAC iterations. + * @param [in] rConvg The RANSAC convergence parameter. + * @param [in] cfd The required confidence in the solution. + * @param [in] minInl The minimum required number of inliers. Minimum 4. + * @param [in] beta The beta-parameter for the non-randomness criterion. + * @param [in] flags A union of flags to fine-tune the estimation. + * @param [in] guessH An extrinsic guess at the solution H, or NULL if + * none provided. + * @param [out] finalH The final estimation of H, or the zero matrix if + * the minimum number of inliers was not met. + * Cannot be NULL. + * @return The number of inliers if the minimum number of + * inliers for acceptance was reached; 0 otherwise. + */ + +unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation context. */ + const float* restrict src, /* Source points */ + const float* restrict dst, /* Destination points */ + char* restrict inl, /* Inlier mask */ + unsigned N, /* = src.length = dst.length = inl.length */ + float maxD, /* 3.0 */ + unsigned maxI, /* 2000 */ + unsigned rConvg, /* 2000 */ + double cfd, /* 0.995 */ + unsigned minInl, /* 4 */ + double beta, /* 0.35 */ + unsigned flags, /* 0 */ + const float* guessH, /* Extrinsic guess, NULL if none provided */ + float* finalH); /* Final result. */ + + + + +/* Extern C */ +#ifdef __cplusplus +/* } *//* End extern "C" */ +} +#endif + + + + +#endif diff --git a/modules/calib3d/src/rhosse2.cpp b/modules/calib3d/src/rhosse2.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5fc5bc187029c5eb9d5c2802ff909e01bdfe34a4 --- /dev/null +++ b/modules/calib3d/src/rhosse2.cpp @@ -0,0 +1,1497 @@ +/* + IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + + By downloading, copying, installing or using the software you agree to this license. + If you do not agree to this license, do not download, install, + copy or use the software. + + + BSD 3-Clause License + + Copyright (C) 2014, Olexa Bilaniuk, Hamid Bazargani & Robert Laganiere, all rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistribution's of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistribution's in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * The name of the copyright holders may not be used to endorse or promote products + derived from this software without specific prior written permission. + + This software is provided by the copyright holders and contributors "as is" and + any express or implied warranties, including, but not limited to, the implied + warranties of merchantability and fitness for a particular purpose are disclaimed. + In no event shall the Intel Corporation or contributors be liable for any direct, + indirect, incidental, special, exemplary, or consequential damages + (including, but not limited to, procurement of substitute goods or services; + loss of use, data, or profits; or business interruption) however caused + and on any theory of liability, whether in contract, strict liability, + or tort (including negligence or otherwise) arising in any way out of + the use of this software, even if advised of the possibility of such damage. +*/ + +/** + * Bilaniuk, Olexa, Hamid Bazargani, and Robert Laganiere. "Fast Target + * Recognition on Mobile Devices: Revisiting Gaussian Elimination for the + * Estimation of Planar Homographies." In Computer Vision and Pattern + * Recognition Workshops (CVPRW), 2014 IEEE Conference on, pp. 119-125. + * IEEE, 2014. + */ + +/* Includes */ +#include +#include +#include +#include +#include +#include +#include +#include +#include "rhosse2.h" + + + +/* Defines */ +#define MEM_ALIGN 32 +#define HSIZE (3*4*sizeof(float)) +#define MIN_DELTA_CHNG 0.1 +#define REL_CHNG(a, b) (fabs((a) - (b))/(a)) +#define CHNG_SIGNIFICANT(a, b) (REL_CHNG(a, b) > MIN_DELTA_CHNG) +#define CHI_STAT 2.706 +#define CHI_SQ 1.645 + + + +namespace cv{ + +/* Data Structures */ + + + +/* Prototypes */ +static inline void* almalloc(size_t nBytes); +static inline void alfree(void* ptr); + +static inline int sacInitRun(RHO_HEST_SSE2* restrict p, + const float* restrict src, + const float* restrict dst, + char* restrict inl, + unsigned N, + float maxD, + unsigned maxI, + unsigned rConvg, + double cfd, + unsigned minInl, + double beta, + unsigned flags, + const float* guessH, + float* finalH); +static inline void sacFiniRun(RHO_HEST_SSE2* p); +static inline int sacIsNREnabled(RHO_HEST_SSE2* p); +static inline int sacIsRefineEnabled(RHO_HEST_SSE2* p); +static inline int sacIsFinalRefineEnabled(RHO_HEST_SSE2* p); +static inline int sacPhaseEndReached(RHO_HEST_SSE2* p); +static inline void sacGoToNextPhase(RHO_HEST_SSE2* p); +static inline void sacGetPROSACSample(RHO_HEST_SSE2* p); +static inline int sacIsSampleDegenerate(RHO_HEST_SSE2* p); +static inline void sacGenerateModel(RHO_HEST_SSE2* p); +static inline int sacIsModelDegenerate(RHO_HEST_SSE2* p); +static inline void sacEvaluateModelSPRT(RHO_HEST_SSE2* p); +static inline void sacUpdateSPRT(RHO_HEST_SSE2* p); +static inline void sacDesignSPRTTest(RHO_HEST_SSE2* p); +static inline int sacIsBestModel(RHO_HEST_SSE2* p); +static inline int sacIsBestModelGoodEnough(RHO_HEST_SSE2* p); +static inline void sacSaveBestModel(RHO_HEST_SSE2* p); +static inline void sacInitNonRand(double beta, + unsigned start, + unsigned N, + unsigned* nonRandMinInl); +static inline void sacNStarOptimize(RHO_HEST_SSE2* p); +static inline void sacUpdateBounds(RHO_HEST_SSE2* p); +static inline void sacOutputModel(RHO_HEST_SSE2* p); + +static inline double sacInitPEndFpI(const unsigned ransacConvg, + const unsigned n, + const unsigned m); +static inline void sacRndSmpl(unsigned sampleSize, + unsigned* currentSample, + unsigned dataSetSize); +static inline double sacRandom(void); +static inline unsigned sacCalcIterBound(double confidence, + double inlierRate, + unsigned sampleSize, + unsigned maxIterBound); +static inline void hFuncRefC(float* packedPoints, float* H); + + + +/* Functions */ + +/** + * Initialize the estimator context, by allocating the aligned buffers + * internally needed. + * + * @param [in/out] p The uninitialized estimator context to initialize. + * @return 0 if successful; non-zero if an error occured. + */ + +int rhoSSE2Init(RHO_HEST_SSE2* p){ + p->smpl = (unsigned*)almalloc(4*sizeof(*p->smpl)); + p->H = (float*) almalloc(HSIZE); + p->bestH = (float*) almalloc(HSIZE); + p->pkdPts = (float*) almalloc(4*2*2*sizeof(*p->pkdPts)); + p->nrTBL = NULL; + p->nrSize = 0; + p->nrBeta = 0.0; + + int ret = p->smpl && + p->H && + p->bestH && + p->pkdPts; + + if(!ret){ + rhoSSE2Fini(p); + } + + return ret; +} + + +/** + * Ensure that the estimator context's internal table for non-randomness + * criterion is at least of the given size, and uses the given beta. The table + * should be larger than the maximum number of matches fed into the estimator. + * + * A value of N of 0 requests deallocation of the table. + * + * @param [in] p The initialized estimator context + * @param [in] N If 0, deallocate internal table. If > 0, ensure that the + * internal table is of at least this size, reallocating if + * necessary. + * @param [in] beta The beta-factor to use within the table. + * @return 1 if successful; 0 if an error occured. + */ + +int rhoSSE2EnsureCapacity(RHO_HEST_SSE2* p, unsigned N, double beta){ + unsigned* tmp; + + + if(N == 0){ + /* Deallocate table */ + alfree(p->nrTBL); + p->nrTBL = NULL; + p->nrSize = 0; + }else{ + /* Ensure table at least as big as N and made for correct beta. */ + if(p->nrTBL && p->nrBeta == beta && p->nrSize >= N){ + /* Table already correctly set up */ + }else{ + if(p->nrSize < N){ + /* Reallocate table because it is too small. */ + tmp = (unsigned*)almalloc(N*sizeof(unsigned)); + if(!tmp){ + return 0; + } + + /* Must recalculate in whole or part. */ + if(p->nrBeta != beta){ + /* Beta changed; recalculate in whole. */ + sacInitNonRand(beta, 0, N, tmp); + alfree(p->nrTBL); + }else{ + /* Beta did not change; Copy over any work already done. */ + memcpy(tmp, p->nrTBL, p->nrSize*sizeof(unsigned)); + sacInitNonRand(beta, p->nrSize, N, tmp); + alfree(p->nrTBL); + } + + p->nrTBL = tmp; + p->nrSize = N; + p->nrBeta = beta; + }else{ + /* Might recalculate in whole, or not at all. */ + if(p->nrBeta != beta){ + /* Beta changed; recalculate in whole. */ + sacInitNonRand(beta, 0, p->nrSize, p->nrTBL); + p->nrBeta = beta; + }else{ + /* Beta did not change; Table was already big enough. Do nothing. */ + /* Besides, this is unreachable. */ + } + } + } + } + + return 1; +} + + +/** + * Finalize the estimator context, by freeing the aligned buffers used + * internally. + * + * @param [in] p The initialized estimator context to finalize. + */ + +void rhoSSE2Fini(RHO_HEST_SSE2* p){ + alfree(p->smpl); + alfree(p->H); + alfree(p->bestH); + alfree(p->pkdPts); + alfree(p->nrTBL); +} + + +/** + * Estimates the homography using the given context, matches and parameters to + * PROSAC. + * + * @param [in/out] p The context to use for homography estimation. Must + * be already initialized. Cannot be NULL. + * @param [in] src The pointer to the source points of the matches. + * Must be aligned to 16 bytes. Cannot be NULL. + * @param [in] dst The pointer to the destination points of the matches. + * Must be aligned to 16 bytes. Cannot be NULL. + * @param [out] bestInl The pointer to the output mask of inlier matches. + * Must be aligned to 16 bytes. May be NULL. + * @param [in] N The number of matches. + * @param [in] maxD The maximum distance. + * @param [in] maxI The maximum number of PROSAC iterations. + * @param [in] rConvg The RANSAC convergence parameter. + * @param [in] cfd The required confidence in the solution. + * @param [in] minInl The minimum required number of inliers. + * @param [in] beta The beta-parameter for the non-randomness criterion. + * @param [in] flags A union of flags to control the estimation. + * @param [in] guessH An extrinsic guess at the solution H, or NULL if + * none provided. + * @param [out] finalH The final estimation of H, or the zero matrix if + * the minimum number of inliers was not met. + * Cannot be NULL. + * @return The number of inliers if the minimum number of + * inliers for acceptance was reached; 0 otherwise. + */ + +unsigned rhoSSE2(RHO_HEST_SSE2* restrict p, /* Homography estimation context. */ + const float* restrict src, /* Source points */ + const float* restrict dst, /* Destination points */ + char* restrict bestInl, /* Inlier mask */ + unsigned N, /* = src.length = dst.length = inl.length */ + float maxD, /* 3.0 */ + unsigned maxI, /* 2000 */ + unsigned rConvg, /* 2000 */ + double cfd, /* 0.995 */ + unsigned minInl, /* 4 */ + double beta, /* 0.35 */ + unsigned flags, /* 0 */ + const float* guessH, /* Extrinsic guess, NULL if none provided */ + float* finalH){ /* Final result. */ + + /** + * Setup + */ + + if(!sacInitRun(p, src, dst, bestInl, N, maxD, maxI, rConvg, cfd, minInl, beta, flags, guessH, finalH)){ + sacFiniRun(p); + return 0; + } + + + /** + * PROSAC Loop + */ + + for(p->i=0; p->i < p->maxI; p->i++){ + if(sacPhaseEndReached(p)){ + sacGoToNextPhase(p); + } + + sacGetPROSACSample(p); + if(sacIsSampleDegenerate(p)){ + continue; + } + + sacGenerateModel(p); + if(sacIsModelDegenerate(p)){ + continue; + } + + sacEvaluateModelSPRT(p); + sacUpdateSPRT(p); + if(sacIsBestModel(p)){ + if(sacIsRefineEnabled(p)){ + /* sacRefine(p) */ + } + + sacSaveBestModel(p); + sacUpdateBounds(p); + + if(sacIsNREnabled(p)){ + sacNStarOptimize(p); + } + } + } + + + /** + * Teardown + */ + + if(sacIsFinalRefineEnabled(p)){ + /* sacRefineFinal(p) */ + } + + sacOutputModel(p); + sacFiniRun(p); + return sacIsBestModelGoodEnough(p) ? p->bestNumInl : 0; +} + + +/** + * Allocate memory aligned to a boundary of MEMALIGN. + */ + +static inline void* almalloc(size_t nBytes){ + if(nBytes){ + unsigned char* ptr = (unsigned char*)malloc(MEM_ALIGN + nBytes); + if(ptr){ + unsigned char* adj = (unsigned char*)(((intptr_t)(ptr+MEM_ALIGN))&((intptr_t)(-MEM_ALIGN))); + ptrdiff_t diff = adj - ptr; + adj[-1] = diff - 1; + return adj; + } + } + + return NULL; +} + +/** + * Free aligned memory + */ + +static inline void alfree(void* ptr){ + if(ptr){ + unsigned char* cptr = (unsigned char*)ptr; + free(cptr - (ptrdiff_t)cptr[-1] - 1); + } +} + + +/** + * Initialize SAC for a run. + * + * Passed all the arguments of hest. + */ + +static inline int sacInitRun(RHO_HEST_SSE2* restrict p, + const float* restrict src, + const float* restrict dst, + char* restrict bestInl, + unsigned N, + float maxD, + unsigned maxI, + unsigned rConvg, + double cfd, + unsigned minInl, + double beta, + unsigned flags, + const float* guessH, + float* finalH){ + p->src = src; + p->dst = dst; + p->allocBestInl = !bestInl; + p->bestInl = bestInl ? bestInl : (char*)almalloc(N); + p->inl = (char*)almalloc(N); + p->N = N; + p->maxD = maxD; + p->maxI = maxI; + p->rConvg = rConvg; + p->cfd = cfd; + p->minInl = minInl < 4 ? 4 : minInl; + p->beta = beta; + p->flags = flags; + p->guessH = guessH; + p->finalH = finalH; + + if(p->guessH){ + memcpy(p->H, p->guessH, HSIZE); + } + memset(p->bestH, 0, HSIZE); + memset(p->finalH, 0, HSIZE); + + if(!p->inl || !p->bestInl){/* Malloc failure */ + return 0; + } + if(sacIsNREnabled(p) && !rhoSSE2EnsureCapacity(p, N, beta)){ + return 0; + } + + p->phNum = 4; + p->phEndI = 1; + p->phEndFpI = sacInitPEndFpI(p->rConvg, p->N, 4); + p->phMax = p->N; + p->phNumInl = 0; + p->bestNumInl = 0; + p->numInl = 0; + p->numModels = 0; + p->Ntested = 0; + p->Ntestedtotal = 0; + p->good = 1; + p->t_M = 25; + p->m_S = 1; + p->epsilon = 0.1; + p->delta = 0.01; + sacDesignSPRTTest(p); + + return 1; +} + +/** + * Finalize SAC run. + * + * @param p + */ + +static inline void sacFiniRun(RHO_HEST_SSE2* p){ + if(p->allocBestInl){ + alfree(p->bestInl); + } + alfree(p->inl); +} + +/** + * Check whether non-randomness criterion is enabled. + * + * @param p + * @return Zero if disabled; non-zero if not. + */ + +static inline int sacIsNREnabled(RHO_HEST_SSE2* p){ + return p->flags & RHO_FLAG_ENABLE_NR; +} + +/** + * Check whether best-model-so-far refinement is enabled. + * + * @param p + * @return Zero if disabled; non-zero if not. + */ + +static inline int sacIsRefineEnabled(RHO_HEST_SSE2* p){ + return p->flags & RHO_FLAG_ENABLE_REFINEMENT; +} + +/** + * Check whether final-model refinement is enabled. + * + * @param p + * @return Zero if disabled; non-zero if not. + */ + +static inline int sacIsFinalRefineEnabled(RHO_HEST_SSE2* p){ + return p->flags & RHO_FLAG_ENABLE_FINAL_REFINEMENT; +} + +/** + * @brief sacPhaseEndReached + * @param p + * @return + */ + +static inline int sacPhaseEndReached(RHO_HEST_SSE2* p){ + return p->i >= p->phEndI && p->phNum < p->phMax; +} + +/** + * @brief sacGoToNextPhase + * @param p + */ + +static inline void sacGoToNextPhase(RHO_HEST_SSE2* p){ + double next; + unsigned m = 4; + + p->phNum++; + next = (p->phEndFpI * p->phNum)/(p->phNum - m); + p->phEndI += ceil(next - p->phEndFpI); + p->phEndFpI = next; +} + +/** + * @brief sacGetPROSACSample + * @param p + */ + +static inline void sacGetPROSACSample(RHO_HEST_SSE2* p){ + if(p->i > p->phEndI){ + sacRndSmpl(4, p->smpl, p->phNum);/* Used to be phMax */ + }else{ + sacRndSmpl(3, p->smpl, p->phNum-1); + p->smpl[3] = p->phNum-1; + } +} + +/** + * @brief sacIsSampleDegenerate + * @param p + * @return + */ + +static inline int sacIsSampleDegenerate(RHO_HEST_SSE2* p){ + unsigned i0 = p->smpl[0], i1 = p->smpl[1], i2 = p->smpl[2], i3 = p->smpl[3]; + + /** + * Pack the matches selected by the SAC algorithm. + * Must be packed points[0:7] = {srcx0, srcy0, srcx1, srcy1, srcx2, srcy2, srcx3, srcy3} + * points[8:15] = {dstx0, dsty0, dstx1, dsty1, dstx2, dsty2, dstx3, dsty3} + * Gather 4 points into the vector + */ + + __m128 src10 = _mm_castpd_ps(_mm_load_sd((const double*)&p->src[2*i0])); + src10 = _mm_loadh_pi(src10, (__m64*)&p->src[2*i1]); + __m128 src32 = _mm_castpd_ps(_mm_load_sd((const double*)&p->src[2*i2])); + src32 = _mm_loadh_pi(src32, (__m64*)&p->src[2*i3]); + __m128 dst10 = _mm_castpd_ps(_mm_load_sd((const double*)&p->dst[2*i0])); + dst10 = _mm_loadh_pi(dst10, (__m64*)&p->dst[2*i1]); + __m128 dst32 = _mm_castpd_ps(_mm_load_sd((const double*)&p->dst[2*i2])); + dst32 = _mm_loadh_pi(dst32, (__m64*)&p->dst[2*i3]); + + + /** + * If the matches' source points have common x and y coordinates, abort. + */ + + /** + * Check: + * packedPoints[0].x == packedPoints[2].x + * packedPoints[0].y == packedPoints[2].y + * packedPoints[1].x == packedPoints[3].x + * packedPoints[1].y == packedPoints[3].y + */ + + __m128 chkEq0 = _mm_cmpeq_ps(src10, src32); + + /** + * Check: + * packedPoints[1].x == packedPoints[2].x + * packedPoints[1].y == packedPoints[2].y + * packedPoints[0].x == packedPoints[3].x + * packedPoints[0].y == packedPoints[3].y + */ + + __m128 chkEq1 = _mm_cmpeq_ps(_mm_shuffle_ps(src10, src10, _MM_SHUFFLE(1, 0, 3, 2)), src32); + + /** + * Check: + * packedPoints[0].x == packedPoints[1].x + * packedPoints[0].y == packedPoints[1].y + * packedPoints[2].x == packedPoints[3].x + * packedPoints[2].y == packedPoints[3].y + */ + + __m128 chkEq2 = _mm_cmpeq_ps(_mm_shuffle_ps(src10, src32, _MM_SHUFFLE(1, 0, 1, 0)), + _mm_shuffle_ps(src10, src32, _MM_SHUFFLE(3, 2, 3, 2))); + + /* Verify */ + if(_mm_movemask_ps(_mm_or_ps(chkEq0, _mm_or_ps(chkEq1, chkEq2)))){ + return 1; + } + + /* If the matches do not satisfy the strong geometric constraint, abort. */ + + /** + * p6420x = (p6.x, p4.x, p2.x, p0.x) + * p6420y = (p6.y, p4.y, p2.y, p0.y) + * p7531x = (p7.x, p5.x, p3.x, p1.x) + * p7531y = (p7.y, p5.y, p3.y, p1.y) + * crosssd0 = p6420y - p7531y = (cross2d0, cross0d0, cross2s0, cross0s0) + * crosssd1 = p7531x - p6420x = (cross2d1, cross0d1, cross2s1, cross0s1) + * crosssd2 = p6420x * p7531y - p6420y * p7531x = (cross2d2, cross0d2, cross2s2, cross0s2) + * + * shufcrosssd0 = (cross0d0, cross2d0, cross0s0, cross2s0) + * shufcrosssd1 = (cross0d1, cross2d1, cross0s1, cross2s1) + * shufcrosssd2 = (cross0d2, cross2d2, cross0s2, cross2s2) + * + * dotsd0 = shufcrosssd0 * p6420x + + * shufcrosssd1 * p6420y + + * shufcrosssd2 + * = (dotd0, dotd2, dots0, dots2) + * dotsd1 = shufcrosssd0 * p7531x + + * shufcrosssd1 * p7531y + + * shufcrosssd2 + * = (dotd1, dotd3, dots1, dots3) + * + * dots = shufps(dotsd0, dotsd1, _MM_SHUFFLE(1, 0, 1, 0)) + * dotd = shufps(dotsd0, dotsd1, _MM_SHUFFLE(3, 2, 3, 2)) + * movmaskps(dots ^ dotd) + */ + + __m128 p3210x = _mm_shuffle_ps(src10, src32, _MM_SHUFFLE(2, 0, 2, 0)); + __m128 p3210y = _mm_shuffle_ps(src10, src32, _MM_SHUFFLE(3, 1, 3, 1)); + __m128 p7654x = _mm_shuffle_ps(dst10, dst32, _MM_SHUFFLE(2, 0, 2, 0)); + __m128 p7654y = _mm_shuffle_ps(dst10, dst32, _MM_SHUFFLE(3, 1, 3, 1)); + __m128 p6420x = _mm_shuffle_ps(p3210x, p7654x, _MM_SHUFFLE(2, 0, 2, 0)); + __m128 p6420y = _mm_shuffle_ps(p3210y, p7654y, _MM_SHUFFLE(2, 0, 2, 0)); + __m128 p7531x = _mm_shuffle_ps(p3210x, p7654x, _MM_SHUFFLE(3, 1, 3, 1)); + __m128 p7531y = _mm_shuffle_ps(p3210y, p7654y, _MM_SHUFFLE(3, 1, 3, 1)); + + __m128 crosssd0 = _mm_sub_ps(p6420y, p7531y); + __m128 crosssd1 = _mm_sub_ps(p7531x, p6420x); + __m128 crosssd2 = _mm_sub_ps(_mm_mul_ps(p6420x, p7531y), _mm_mul_ps(p6420y, p7531x)); + + __m128 shufcrosssd0 = _mm_shuffle_ps(crosssd0, crosssd0, _MM_SHUFFLE(2, 3, 0, 1)); + __m128 shufcrosssd1 = _mm_shuffle_ps(crosssd1, crosssd1, _MM_SHUFFLE(2, 3, 0, 1)); + __m128 shufcrosssd2 = _mm_shuffle_ps(crosssd2, crosssd2, _MM_SHUFFLE(2, 3, 0, 1)); + + __m128 dotsd0 = _mm_add_ps(_mm_add_ps(_mm_mul_ps(shufcrosssd0, p6420x), + _mm_mul_ps(shufcrosssd1, p6420y)), + shufcrosssd2); + __m128 dotsd1 = _mm_add_ps(_mm_add_ps(_mm_mul_ps(shufcrosssd0, p7531x), + _mm_mul_ps(shufcrosssd1, p7531y)), + shufcrosssd2); + + __m128 dots = _mm_shuffle_ps(dotsd0, dotsd1, _MM_SHUFFLE(0, 1, 0, 1)); + __m128 dotd = _mm_shuffle_ps(dotsd0, dotsd1, _MM_SHUFFLE(2, 3, 2, 3)); + + /* if(_mm_movemask_ps(_mm_cmpge_ps(_mm_setzero_ps(), _mm_mul_ps(dots, dotd)))){ */ + if(_mm_movemask_epi8(_mm_cmplt_epi32(_mm_xor_si128(_mm_cvtps_epi32(dots), _mm_cvtps_epi32(dotd)), _mm_setzero_si128()))){ + return 1; + } + + + /* Otherwise, proceed with evaluation */ + _mm_store_ps(&p->pkdPts[0], src10); + _mm_store_ps(&p->pkdPts[4], src32); + _mm_store_ps(&p->pkdPts[8], dst10); + _mm_store_ps(&p->pkdPts[12], dst32); + + return 0; +} + +/** + * Compute homography of matches in p->pkdPts with hFuncRefC and store in p->H. + * + * @param p + */ + +static inline void sacGenerateModel(RHO_HEST_SSE2* p){ + hFuncRefC(p->pkdPts, p->H); +} + +/** + * @brief sacIsModelDegenerate + * @param p + * @return + */ + +static inline int sacIsModelDegenerate(RHO_HEST_SSE2* p){ + int degenerate; + float* H = p->H; + float f=H[0]+H[1]+H[2]+H[4]+H[5]+H[6]+H[8]+H[9]; + + /* degenerate = isnan(f); */ + degenerate = f!=f;/* Only NaN is not equal to itself. */ + /* degenerate = 0; */ + + if(degenerate){return degenerate;} + +#if 0 + + /** + * Convexity test + * + * x' = Hx for i=1..4 must be convex. + * + * [ x' ] [ H00 H01 H02 ] [ x ] + * [ y' ] = [ H10 H11 H12 ] [ y ], where: + * [ z' ] [ H20 H21 H22 ] [ 1 ] + * + * p0 = (0, 0) + * p1 = (0, 1) + * p2 = (1, 1) + * p3 = (1, 0) + */ + + float pt[4][2]; + float pz[4][1]; + + pt[0][0] = H[2]; + pt[0][1] = H[6]; + pz[0][0] = H[10]; + pt[1][0] = H[1]+H[2]; + pt[1][1] = H[5]+H[6]; + pz[1][0] = H[9]+H[10]; + pt[2][0] = H[0]+H[1]+H[2]; + pt[2][1] = H[4]+H[5]+H[6]; + pz[2][0] = H[8]+H[9]+H[10]; + pt[3][0] = H[0]+H[2]; + pt[3][1] = H[4]+H[6]; + pz[3][0] = H[8]+H[10]; + + pt[0][0] /= pz[0][0]; + pt[0][1] /= pz[0][0]; + pt[1][0] /= pz[1][0]; + pt[1][1] /= pz[1][0]; + pt[2][0] /= pz[2][0]; + pt[2][1] /= pz[2][0]; + pt[3][0] /= pz[3][0]; + pt[3][1] /= pz[3][0]; + + /** + * Crossproduct: + * + * (x, y, z) = (ay bz - az by, + * az bx - ax bz, + * ax by - ay bx) + */ + + __m128 src10 = _mm_load_ps(&pt[0][0]); + __m128 src32 = _mm_load_ps(&pt[2][0]); + + __m128 p3210x = _mm_shuffle_ps(src10, src32, _MM_SHUFFLE(2, 0, 2, 0)); + __m128 p3210y = _mm_shuffle_ps(src10, src32, _MM_SHUFFLE(3, 1, 3, 1)); + __m128 p2103x = _mm_shuffle_ps(p3210x, p3210x, _MM_SHUFFLE(2, 1, 0, 3)); + __m128 p2103y = _mm_shuffle_ps(p3210y, p3210y, _MM_SHUFFLE(2, 1, 0, 3)); + __m128 vax = _mm_sub_ps(p3210x, p2103x); + __m128 vay = _mm_sub_ps(p3210y, p2103y); + __m128 vbx = _mm_shuffle_ps(vax, vax, _MM_SHUFFLE(2, 1, 0, 3)); + __m128 vby = _mm_shuffle_ps(vay, vay, _MM_SHUFFLE(2, 1, 0, 3)); + + __m128 cross = _mm_sub_ps(_mm_mul_ps(vax, vby), _mm_mul_ps(vay, vbx)); + + degenerate = _mm_movemask_ps(cross); + degenerate = degenerate != 0x0; +#endif + return degenerate; +} + +/** + * @brief sacEvaluateModelSPRT + * @param p + */ + +static inline void sacEvaluateModelSPRT(RHO_HEST_SSE2* p){ + unsigned i = 0; + unsigned isInlier; + double lambda = 1.0; + float distSq = p->maxD*p->maxD; + const float* src = p->src; + const float* dst = p->dst; + char* inl = p->inl; + float* H = p->H; + + + p->numModels++; + + p->numInl = 0; + p->Ntested = 0; + p->good = 1; + + + /* VECTOR */ + const __m128 distSqV=_mm_set1_ps(distSq); + + const __m128 H00=_mm_set1_ps(H[0]); + const __m128 H01=_mm_set1_ps(H[1]); + const __m128 H02=_mm_set1_ps(H[2]); + const __m128 H10=_mm_set1_ps(H[4]); + const __m128 H11=_mm_set1_ps(H[5]); + const __m128 H12=_mm_set1_ps(H[6]); + const __m128 H20=_mm_set1_ps(H[8]); + const __m128 H21=_mm_set1_ps(H[9]); + const __m128 H22=_mm_set1_ps(H[10]); + + for(;i<(p->N-3) && p->good;i+=4){ + /* Backproject */ + __m128 x, y, X, Y, inter0, inter1, inter2, inter3; + inter0 = _mm_loadu_ps(src+2*i); + inter1 = _mm_loadu_ps(src+2*i+4); + inter2 = _mm_loadu_ps(dst+2*i); + inter3 = _mm_loadu_ps(dst+2*i+4); + + x = _mm_shuffle_ps(inter0, inter1, _MM_SHUFFLE(2, 0, 2, 0)); + y = _mm_shuffle_ps(inter0, inter1, _MM_SHUFFLE(3, 1, 3, 1)); + X = _mm_shuffle_ps(inter2, inter3, _MM_SHUFFLE(2, 0, 2, 0)); + Y = _mm_shuffle_ps(inter2, inter3, _MM_SHUFFLE(3, 1, 3, 1)); + + __m128 reprojX = _mm_add_ps(_mm_add_ps(_mm_mul_ps(H00, x), _mm_mul_ps(H01, y)), H02); + __m128 reprojY = _mm_add_ps(_mm_add_ps(_mm_mul_ps(H10, x), _mm_mul_ps(H11, y)), H12); + __m128 reprojZ = _mm_add_ps(_mm_add_ps(_mm_mul_ps(H20, x), _mm_mul_ps(H21, y)), H22); + + __m128 recipZ = _mm_rcp_ps(reprojZ); + reprojX = _mm_mul_ps(reprojX, recipZ); + reprojY = _mm_mul_ps(reprojY, recipZ); + /* reprojX = _mm_div_ps(reprojX, reprojZ); */ + /* reprojY = _mm_div_ps(reprojY, reprojZ); */ + + reprojX = _mm_sub_ps(reprojX, X); + reprojY = _mm_sub_ps(reprojY, Y); + + reprojX = _mm_mul_ps(reprojX, reprojX); + reprojY = _mm_mul_ps(reprojY, reprojY); + + __m128 reprojDistV = _mm_add_ps(reprojX, reprojY); + + __m128 cmp = _mm_cmple_ps(reprojDistV, distSqV); + int msk = _mm_movemask_ps(cmp); + + /* ... */ + /* 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15*/ + static const unsigned bitCnt[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4}; + p->numInl += bitCnt[msk]; + + static const char byteMsk[16][4] = {{0,0,0,0},{1,0,0,0},{0,1,0,0},{1,1,0,0}, + {0,0,1,0},{1,0,1,0},{0,1,1,0},{1,1,1,0}, + {0,0,0,1},{1,0,0,1},{0,1,0,1},{1,1,0,1}, + {0,0,1,1},{1,0,1,1},{0,1,1,1},{1,1,1,1}}; + memcpy(inl, byteMsk[msk], 4); + inl+=4; + + + /* SPRT */ + lambda *= p->lambdaTBL[msk]; + p->good = lambda <= p->A; + /* If !p->good, the threshold A was exceeded, so we're rejecting */ + } + + /* SCALAR */ + for(;iN && p->good;i++){ + /* Backproject */ + float x=src[i*2],y=src[i*2+1]; + float X=dst[i*2],Y=dst[i*2+1]; + + float reprojX=H[0]*x+H[1]*y+H[2]; /* ( X_1 ) ( H_11 H_12 H_13 ) (x_1) */ + float reprojY=H[4]*x+H[5]*y+H[6]; /* ( X_2 ) = ( H_21 H_22 H_23 ) (x_2) */ + float reprojZ=H[8]*x+H[9]*y+H[10];/* ( X_3 ) ( H_31 H_32 H_33=1.0 ) (x_3 = 1.0) */ + + /* reproj is in homogeneous coordinates. To bring back to "regular" coordinates, divide by Z. */ + reprojX/=reprojZ; + reprojY/=reprojZ; + + /* Compute distance */ + reprojX-=X; + reprojY-=Y; + reprojX*=reprojX; + reprojY*=reprojY; + float reprojDist = reprojX+reprojY; + + /* ... */ + isInlier = reprojDist <= distSq; + p->numInl += isInlier; + *inl++ = isInlier; + + + /* SPRT */ + lambda *= isInlier ? p->lambdaAccept : p->lambdaReject; + p->good = lambda <= p->A; + /* If !p->good, the threshold A was exceeded, so we're rejecting */ + } + + + p->Ntested = i; + p->Ntestedtotal += i; +} + +/** + * Update either the delta or epsilon SPRT parameters, depending on the events + * that transpired in the previous evaluation. + * + * If a "good" model that is also the best was encountered, update epsilon, + * since + */ + +static inline void sacUpdateSPRT(RHO_HEST_SSE2* p){ + if(p->good){ + if(sacIsBestModel(p)){ + p->epsilon = (double)p->numInl/p->N; + sacDesignSPRTTest(p); + } + }else{ + double newDelta = (double)p->numInl/p->Ntested; + + if(newDelta > 0 && CHNG_SIGNIFICANT(p->delta, newDelta)){ + p->delta = newDelta; + sacDesignSPRTTest(p); + } + } +} + +/** + * Numerically compute threshold A from the estimated delta, epsilon, t_M and + * m_S values. + * + * Epsilon: Denotes the probability that a randomly chosen data point is + * consistent with a good model. + * Delta: Denotes the probability that a randomly chosen data point is + * consistent with a bad model. + * t_M: Time needed to instantiate a model hypotheses given a sample. + * (Computing model parameters from a sample takes the same time + * as verification of t_M data points) + * m_S: The number of models that are verified per sample. + */ + +static inline double designSPRTTest(double delta, double epsilon, double t_M, double m_S){ + double An, C, K, prevAn; + unsigned i; + + /** + * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005 + * Eq (2) + */ + + C = (1-delta) * log((1-delta)/(1-epsilon)) + + delta * log( delta / epsilon ); + + /** + * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005 + * Eq (6) + * K = K_1/K_2 + 1 = (t_M*C)/m_S + 1 + */ + + K = t_M*C/m_S + 1; + + /** + * Randomized RANSAC with Sequential Probability Ratio Test, ICCV 2005 + * Paragraph below Eq (6) + * + * A* = lim_{n -> infty} A_n, where + * A_0 = K1/K2 + 1 and + * A_{n+1} = K1/K2 + 1 + log(A_n) + * The series converges fast, typically within four iterations. + */ + + An = K; + i = 0; + + do{ + prevAn = An; + An = K + log(An); + }while((An-prevAn > 1.5e-8) && (++i < 10)); + + /** + * Return A = An_stopping, with n_stopping < 10 + */ + + return An; +} + +/** + * Design the SPRT test. Shorthand for + * A = sprt(delta, epsilon, t_M, m_S); + * + * Sets p->A, p->lambdaAccept, p->lambdaReject and p->lambdaLUT + */ + +static inline void sacDesignSPRTTest(RHO_HEST_SSE2* p){ + p->A = designSPRTTest(p->delta, p->epsilon, p->t_M, p->m_S); + p->lambdaReject = ((1.0 - p->delta) / (1.0 - p->epsilon)); + p->lambdaAccept = (( p->delta ) / ( p->epsilon )); + + double a0r4 = p->lambdaReject*p->lambdaReject*p->lambdaReject*p->lambdaReject; + double a1r3 = p->lambdaAccept*p->lambdaReject*p->lambdaReject*p->lambdaReject; + double a2r2 = p->lambdaAccept*p->lambdaAccept*p->lambdaReject*p->lambdaReject; + double a3r1 = p->lambdaAccept*p->lambdaAccept*p->lambdaAccept*p->lambdaReject; + double a4r0 = p->lambdaAccept*p->lambdaAccept*p->lambdaAccept*p->lambdaAccept; + + p->lambdaTBL[ 0] = a0r4; + p->lambdaTBL[ 1] = p->lambdaTBL[ 2] = p->lambdaTBL[ 4] = p->lambdaTBL[ 8] = a1r3; + p->lambdaTBL[ 3] = p->lambdaTBL[ 5] = p->lambdaTBL[ 6] = p->lambdaTBL[ 9] = p->lambdaTBL[10] = p->lambdaTBL[12] = a2r2; + p->lambdaTBL[ 7] = p->lambdaTBL[11] = p->lambdaTBL[13] = p->lambdaTBL[14] = a3r1; + p->lambdaTBL[15] = a4r0; +} + +/** + * Return whether the current model is the best model so far. + */ + +static inline int sacIsBestModel(RHO_HEST_SSE2* p){ + return p->numInl > p->bestNumInl; +} + +/** + * Returns whether the current-best model is good enough to be an + * acceptable best model, by checking whether it meets the minimum + * number of inliers. + */ + +static inline int sacIsBestModelGoodEnough(RHO_HEST_SSE2* p){ + return p->bestNumInl >= p->minInl; +} + +/** + * + */ + +static inline void sacSaveBestModel(RHO_HEST_SSE2* p){ + p->bestNumInl = p->numInl; + memcpy(p->bestH, p->H, HSIZE); + memcpy(p->bestInl, p->inl, p->N); +} + +/** + * + */ + +static inline void sacInitNonRand(double beta, + unsigned start, + unsigned N, + unsigned* nonRandMinInl){ + unsigned m = 4; + unsigned n = m+1 > start ? m+1 : start; + double beta_beta1_sq_chi = sqrt(beta*(1.0-beta)) * CHI_SQ; + + for(; n < N; n++){ + double mu = n * beta; + double sigma = sqrt(n)* beta_beta1_sq_chi; + unsigned i_min = ceil(m + mu + sigma); + + nonRandMinInl[n] = i_min; + } +} + +/** + * + */ + +static inline void sacNStarOptimize(RHO_HEST_SSE2* p){ + unsigned min_sample_length = 10*2; /*(p->N * INLIERS_RATIO) */ + unsigned best_n = p->N; + unsigned test_n = best_n; + unsigned bestNumInl = p->bestNumInl; + unsigned testNumInl = bestNumInl; + + for(;test_n > min_sample_length && testNumInl;test_n--){ + if(testNumInl*best_n > bestNumInl*test_n){ + if(testNumInl < p->nrTBL[test_n]){ + break; + } + best_n = test_n; + bestNumInl = testNumInl; + } + testNumInl -= !!p->bestInl[test_n-1]; + } + + if(bestNumInl*p->phMax > p->phNumInl*best_n){ + p->phMax = best_n; + p->phNumInl = bestNumInl; + p->maxI = sacCalcIterBound(p->cfd, + (double)p->phNumInl/p->phMax, + 4, + p->maxI); + } +} + +/** + * + */ + +static inline void sacUpdateBounds(RHO_HEST_SSE2* p){ + p->maxI = sacCalcIterBound(p->cfd, + (double)p->bestNumInl/p->N, + 4, + p->maxI); +} + +/** + * @brief sacOutputModel + * @param p + */ + +static inline void sacOutputModel(RHO_HEST_SSE2* p){ + if(!sacIsBestModelGoodEnough(p)){ + memset(p->bestH, 0, HSIZE); + } + + if(p->finalH){ + memcpy(p->finalH, p->bestH, HSIZE); + } +} + +/** + * Compute the real-valued number of samples per phase, given the RANSAC convergence speed, + * data set size and sample size. + */ + +static inline double sacInitPEndFpI(const unsigned ransacConvg, + const unsigned n, + const unsigned m){ + double numer=1, denom=1; + + unsigned i; + for(i=0;idataSetSize){ + /** + * Selection Sampling: + * + * Algorithm S (Selection sampling technique). To select n records at random from a set of N, where 0 < n ≤ N. + * S1. [Initialize.] Set t ← 0, m ← 0. (During this algorithm, m represents the number of records selected so far, + * and t is the total number of input records that we have dealt with.) + * S2. [Generate U.] Generate a random number U, uniformly distributed between zero and one. + * S3. [Test.] If (N – t)U ≥ n – m, go to step S5. + * S4. [Select.] Select the next record for the sample, and increase m and t by 1. If m < n, go to step S2; + * otherwise the sample is complete and the algorithm terminates. + * S5. [Skip.] Skip the next record (do not include it in the sample), increase t by 1, and go back to step S2. + */ + + unsigned m=0,t=0; + + for(m=0;m=1.){ + /** + * A certainty of picking at least one outlier means that we will need + * an infinite amount of iterations in order to find a correct solution. + */ + + retVal = maxIterBound; + }else if(atLeastOneOutlierProbability<=0.){ + /** + * The certainty of NOT picking an outlier means that only 1 iteration + * is needed to find a solution. + */ + + retVal = 1; + }else{ + /** + * Since 1-confidence (the probability of the model being based on at + * least one outlier in the data) is equal to + * (1-inlierRate**sampleSize)**numIterations (the probability of picking + * at least one outlier in numIterations samples), we can isolate + * numIterations (the return value) into + */ + + retVal = ceil(log(1.-confidence)/log(atLeastOneOutlierProbability)); + } + + /** + * Clamp to maxIterationBound. + */ + + return retVal <= maxIterBound ? retVal : maxIterBound; +} + + +/* Transposed, C */ +static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates) points followed by + destination (four x,y float coordinates) points, aligned by 32 bytes */ + float* H){ /* Homography (three 16-byte aligned rows of 3 floats) */ + float x0=*packedPoints++; + float y0=*packedPoints++; + float x1=*packedPoints++; + float y1=*packedPoints++; + float x2=*packedPoints++; + float y2=*packedPoints++; + float x3=*packedPoints++; + float y3=*packedPoints++; + float X0=*packedPoints++; + float Y0=*packedPoints++; + float X1=*packedPoints++; + float Y1=*packedPoints++; + float X2=*packedPoints++; + float Y2=*packedPoints++; + float X3=*packedPoints++; + float Y3=*packedPoints++; + + float x0X0=x0*X0, x1X1=x1*X1, x2X2=x2*X2, x3X3=x3*X3; + float x0Y0=x0*Y0, x1Y1=x1*Y1, x2Y2=x2*Y2, x3Y3=x3*Y3; + float y0X0=y0*X0, y1X1=y1*X1, y2X2=y2*X2, y3X3=y3*X3; + float y0Y0=y0*Y0, y1Y1=y1*Y1, y2Y2=y2*Y2, y3Y3=y3*Y3; + + + /** + * [0] [1] Hidden Prec + * x0 y0 1 x1 + * x1 y1 1 x1 + * x2 y2 1 x1 + * x3 y3 1 x1 + * + * Eliminate ones in column 2 and 5. + * R(0)-=R(2) + * R(1)-=R(2) + * R(3)-=R(2) + * + * [0] [1] Hidden Prec + * x0-x2 y0-y2 0 x1+1 + * x1-x2 y1-y2 0 x1+1 + * x2 y2 1 x1 + * x3-x2 y3-y2 0 x1+1 + * + * Eliminate column 0 of rows 1 and 3 + * R(1)=(x0-x2)*R(1)-(x1-x2)*R(0), y1'=(y1-y2)(x0-x2)-(x1-x2)(y0-y2) + * R(3)=(x0-x2)*R(3)-(x3-x2)*R(0), y3'=(y3-y2)(x0-x2)-(x3-x2)(y0-y2) + * + * [0] [1] Hidden Prec + * x0-x2 y0-y2 0 x1+1 + * 0 y1' 0 x2+3 + * x2 y2 1 x1 + * 0 y3' 0 x2+3 + * + * Eliminate column 1 of rows 0 and 3 + * R(3)=y1'*R(3)-y3'*R(1) + * R(0)=y1'*R(0)-(y0-y2)*R(1) + * + * [0] [1] Hidden Prec + * x0' 0 0 x3+5 + * 0 y1' 0 x2+3 + * x2 y2 1 x1 + * 0 0 0 x4+7 + * + * Eliminate columns 0 and 1 of row 2 + * R(0)/=x0' + * R(1)/=y1' + * R(2)-= (x2*R(0) + y2*R(1)) + * + * [0] [1] Hidden Prec + * 1 0 0 x6+10 + * 0 1 0 x4+6 + * 0 0 1 x4+7 + * 0 0 0 x4+7 + */ + + /** + * Eliminate ones in column 2 and 5. + * R(0)-=R(2) + * R(1)-=R(2) + * R(3)-=R(2) + */ + + /*float minor[4][2] = {{x0-x2,y0-y2}, + {x1-x2,y1-y2}, + {x2 ,y2 }, + {x3-x2,y3-y2}};*/ + /*float major[8][3] = {{x2X2-x0X0,y2X2-y0X0,(X0-X2)}, + {x2X2-x1X1,y2X2-y1X1,(X1-X2)}, + {-x2X2 ,-y2X2 ,(X2 )}, + {x2X2-x3X3,y2X2-y3X3,(X3-X2)}, + {x2Y2-x0Y0,y2Y2-y0Y0,(Y0-Y2)}, + {x2Y2-x1Y1,y2Y2-y1Y1,(Y1-Y2)}, + {-x2Y2 ,-y2Y2 ,(Y2 )}, + {x2Y2-x3Y3,y2Y2-y3Y3,(Y3-Y2)}};*/ + float minor[2][4] = {{x0-x2,x1-x2,x2 ,x3-x2}, + {y0-y2,y1-y2,y2 ,y3-y2}}; + float major[3][8] = {{x2X2-x0X0,x2X2-x1X1,-x2X2 ,x2X2-x3X3,x2Y2-x0Y0,x2Y2-x1Y1,-x2Y2 ,x2Y2-x3Y3}, + {y2X2-y0X0,y2X2-y1X1,-y2X2 ,y2X2-y3X3,y2Y2-y0Y0,y2Y2-y1Y1,-y2Y2 ,y2Y2-y3Y3}, + { (X0-X2) , (X1-X2) , (X2 ) , (X3-X2) , (Y0-Y2) , (Y1-Y2) , (Y2 ) , (Y3-Y2) }}; + + /** + * int i; + * for(i=0;i<8;i++) major[2][i]=-major[2][i]; + * Eliminate column 0 of rows 1 and 3 + * R(1)=(x0-x2)*R(1)-(x1-x2)*R(0), y1'=(y1-y2)(x0-x2)-(x1-x2)(y0-y2) + * R(3)=(x0-x2)*R(3)-(x3-x2)*R(0), y3'=(y3-y2)(x0-x2)-(x3-x2)(y0-y2) + */ + + float scalar1=minor[0][0], scalar2=minor[0][1]; + minor[1][1]=minor[1][1]*scalar1-minor[1][0]*scalar2; + + major[0][1]=major[0][1]*scalar1-major[0][0]*scalar2; + major[1][1]=major[1][1]*scalar1-major[1][0]*scalar2; + major[2][1]=major[2][1]*scalar1-major[2][0]*scalar2; + + major[0][5]=major[0][5]*scalar1-major[0][4]*scalar2; + major[1][5]=major[1][5]*scalar1-major[1][4]*scalar2; + major[2][5]=major[2][5]*scalar1-major[2][4]*scalar2; + + scalar2=minor[0][3]; + minor[1][3]=minor[1][3]*scalar1-minor[1][0]*scalar2; + + major[0][3]=major[0][3]*scalar1-major[0][0]*scalar2; + major[1][3]=major[1][3]*scalar1-major[1][0]*scalar2; + major[2][3]=major[2][3]*scalar1-major[2][0]*scalar2; + + major[0][7]=major[0][7]*scalar1-major[0][4]*scalar2; + major[1][7]=major[1][7]*scalar1-major[1][4]*scalar2; + major[2][7]=major[2][7]*scalar1-major[2][4]*scalar2; + + /** + * Eliminate column 1 of rows 0 and 3 + * R(3)=y1'*R(3)-y3'*R(1) + * R(0)=y1'*R(0)-(y0-y2)*R(1) + */ + + scalar1=minor[1][1];scalar2=minor[1][3]; + major[0][3]=major[0][3]*scalar1-major[0][1]*scalar2; + major[1][3]=major[1][3]*scalar1-major[1][1]*scalar2; + major[2][3]=major[2][3]*scalar1-major[2][1]*scalar2; + + major[0][7]=major[0][7]*scalar1-major[0][5]*scalar2; + major[1][7]=major[1][7]*scalar1-major[1][5]*scalar2; + major[2][7]=major[2][7]*scalar1-major[2][5]*scalar2; + + scalar2=minor[1][0]; + minor[0][0]=minor[0][0]*scalar1-minor[0][1]*scalar2; + + major[0][0]=major[0][0]*scalar1-major[0][1]*scalar2; + major[1][0]=major[1][0]*scalar1-major[1][1]*scalar2; + major[2][0]=major[2][0]*scalar1-major[2][1]*scalar2; + + major[0][4]=major[0][4]*scalar1-major[0][5]*scalar2; + major[1][4]=major[1][4]*scalar1-major[1][5]*scalar2; + major[2][4]=major[2][4]*scalar1-major[2][5]*scalar2; + + /** + * Eliminate columns 0 and 1 of row 2 + * R(0)/=x0' + * R(1)/=y1' + * R(2)-= (x2*R(0) + y2*R(1)) + */ + + scalar1=minor[0][0]; + major[0][0]/=scalar1; + major[1][0]/=scalar1; + major[2][0]/=scalar1; + major[0][4]/=scalar1; + major[1][4]/=scalar1; + major[2][4]/=scalar1; + + scalar1=minor[1][1]; + major[0][1]/=scalar1; + major[1][1]/=scalar1; + major[2][1]/=scalar1; + major[0][5]/=scalar1; + major[1][5]/=scalar1; + major[2][5]/=scalar1; + + + scalar1=minor[0][2];scalar2=minor[1][2]; + major[0][2]-=major[0][0]*scalar1+major[0][1]*scalar2; + major[1][2]-=major[1][0]*scalar1+major[1][1]*scalar2; + major[2][2]-=major[2][0]*scalar1+major[2][1]*scalar2; + + major[0][6]-=major[0][4]*scalar1+major[0][5]*scalar2; + major[1][6]-=major[1][4]*scalar1+major[1][5]*scalar2; + major[2][6]-=major[2][4]*scalar1+major[2][5]*scalar2; + + /* Only major matters now. R(3) and R(7) correspond to the hollowed-out rows. */ + scalar1=major[0][7]; + major[1][7]/=scalar1; + major[2][7]/=scalar1; + + scalar1=major[0][0];major[1][0]-=scalar1*major[1][7];major[2][0]-=scalar1*major[2][7]; + scalar1=major[0][1];major[1][1]-=scalar1*major[1][7];major[2][1]-=scalar1*major[2][7]; + scalar1=major[0][2];major[1][2]-=scalar1*major[1][7];major[2][2]-=scalar1*major[2][7]; + scalar1=major[0][3];major[1][3]-=scalar1*major[1][7];major[2][3]-=scalar1*major[2][7]; + scalar1=major[0][4];major[1][4]-=scalar1*major[1][7];major[2][4]-=scalar1*major[2][7]; + scalar1=major[0][5];major[1][5]-=scalar1*major[1][7];major[2][5]-=scalar1*major[2][7]; + scalar1=major[0][6];major[1][6]-=scalar1*major[1][7];major[2][6]-=scalar1*major[2][7]; + + + /* One column left (Two in fact, but the last one is the homography) */ + scalar1=major[1][3]; + + major[2][3]/=scalar1; + scalar1=major[1][0];major[2][0]-=scalar1*major[2][3]; + scalar1=major[1][1];major[2][1]-=scalar1*major[2][3]; + scalar1=major[1][2];major[2][2]-=scalar1*major[2][3]; + scalar1=major[1][4];major[2][4]-=scalar1*major[2][3]; + scalar1=major[1][5];major[2][5]-=scalar1*major[2][3]; + scalar1=major[1][6];major[2][6]-=scalar1*major[2][3]; + scalar1=major[1][7];major[2][7]-=scalar1*major[2][3]; + + + /* Homography is done. */ + H[0]=major[2][0]; + H[1]=major[2][1]; + H[2]=major[2][2]; + + H[4]=major[2][4]; + H[5]=major[2][5]; + H[6]=major[2][6]; + + H[8]=major[2][7]; + H[9]=major[2][3]; + H[10]=1.0; +} + + +} /* End namespace cv */ diff --git a/modules/calib3d/src/rhosse2.h b/modules/calib3d/src/rhosse2.h new file mode 100644 index 0000000000000000000000000000000000000000..ad3d2b18be194522eb1aaf2ce903d76da0775450 --- /dev/null +++ b/modules/calib3d/src/rhosse2.h @@ -0,0 +1,331 @@ +/* + IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + + By downloading, copying, installing or using the software you agree to this license. + If you do not agree to this license, do not download, install, + copy or use the software. + + + BSD 3-Clause License + + Copyright (C) 2014, Olexa Bilaniuk, Hamid Bazargani & Robert Laganiere, all rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + * Redistribution's of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + * Redistribution's in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * The name of the copyright holders may not be used to endorse or promote products + derived from this software without specific prior written permission. + + This software is provided by the copyright holders and contributors "as is" and + any express or implied warranties, including, but not limited to, the implied + warranties of merchantability and fitness for a particular purpose are disclaimed. + In no event shall the Intel Corporation or contributors be liable for any direct, + indirect, incidental, special, exemplary, or consequential damages + (including, but not limited to, procurement of substitute goods or services; + loss of use, data, or profits; or business interruption) however caused + and on any theory of liability, whether in contract, strict liability, + or tort (including negligence or otherwise) arising in any way out of + the use of this software, even if advised of the possibility of such damage. +*/ + +/** + * Bilaniuk, Olexa, Hamid Bazargani, and Robert Laganiere. "Fast Target + * Recognition on Mobile Devices: Revisiting Gaussian Elimination for the + * Estimation of Planar Homographies." In Computer Vision and Pattern + * Recognition Workshops (CVPRW), 2014 IEEE Conference on, pp. 119-125. + * IEEE, 2014. + */ + +/* Include Guards */ +#ifndef __RHOSSE2_H__ +#define __RHOSSE2_H__ + + + +/* Includes */ + + + + + +/* Defines */ +#ifdef __cplusplus + +/* C++ does not have the restrict keyword. */ +#ifdef restrict +#undef restrict +#endif +#define restrict + +#else + +/* C99 and over has the restrict keyword. */ +#if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L +#define restrict +#endif + +#endif + + +/* Flags */ +#ifndef RHO_FLAG_NONE +#define RHO_FLAG_NONE (0U<<0) +#endif +#ifndef RHO_FLAG_ENABLE_NR +#define RHO_FLAG_ENABLE_NR (1U<<0) +#endif +#ifndef RHO_FLAG_ENABLE_REFINEMENT +#define RHO_FLAG_ENABLE_REFINEMENT (1U<<1) +#endif +#ifndef RHO_FLAG_ENABLE_FINAL_REFINEMENT +#define RHO_FLAG_ENABLE_FINAL_REFINEMENT (1U<<2) +#endif + + + +/* Data structures */ + +/** + * Homography Estimation context. + */ + +typedef struct{ + /* Virtual Arguments */ + const float* restrict src; + const float* restrict dst; + int allocBestInl; + char* restrict bestInl; + unsigned N; + float maxD; + unsigned maxI; + unsigned rConvg; + double cfd; + unsigned minInl; + double beta; + unsigned flags; + const float* guessH; + float* finalH; + + /* PROSAC */ + unsigned i; /* Iteration Number */ + unsigned phNum; /* Phase Number */ + unsigned phEndI; /* Phase End Iteration */ + double phEndFpI; /* Phase floating-point End Iteration */ + unsigned phMax; /* Termination phase number */ + unsigned phNumInl; /* Number of inliers for termination phase */ + unsigned bestNumInl; /* Best number of inliers */ + unsigned numInl; /* Current number of inliers */ + unsigned numModels; /* Number of models tested */ + unsigned* restrict smpl; /* Sample */ + float* restrict H; /* Current homography */ + float* restrict bestH; /* Best homography */ + float* restrict pkdPts; /* Packed points */ + char* restrict inl; /* Inliers to current model */ + unsigned* restrict nrTBL; /* Non-Randomness: Table */ + unsigned nrSize; /* Non-Randomness: Size */ + double nrBeta; /* Non-Randomness: Beta */ + + /* SPRT */ + double t_M; /* t_M */ + double m_S; /* m_S */ + double epsilon; /* Epsilon */ + double delta; /* delta */ + double A; /* SPRT Threshold */ + unsigned Ntested; /* Number of points tested */ + unsigned Ntestedtotal; /* Number of points tested in total */ + int good; /* Good/bad flag */ + double lambdaAccept; /* Accept multiplier */ + double lambdaReject; /* Reject multiplier */ + double lambdaTBL[16]; /* Multiplier LUT */ +} RHO_HEST_SSE2; + + + +/* Extern C */ +#ifdef __cplusplus +namespace cv{ +//extern "C" { +#endif + + + +/* Functions */ + +/** + * Initialize the estimator context, by allocating the aligned buffers + * internally needed. + * + * @param [in/out] p The uninitialized estimator context to initialize. + * @return 0 if successful; non-zero if an error occured. + */ + +int rhoSSE2Init(RHO_HEST_SSE2* p); + + +/** + * Ensure that the estimator context's internal table for non-randomness + * criterion is at least of the given size, and uses the given beta. The table + * should be larger than the maximum number of matches fed into the estimator. + * + * A value of N of 0 requests deallocation of the table. + * + * @param [in] p The initialized estimator context + * @param [in] N If 0, deallocate internal table. If > 0, ensure that the + * internal table is of at least this size, reallocating if + * necessary. + * @param [in] beta The beta-factor to use within the table. + * @return 0 if successful; non-zero if an error occured. + */ + +int rhoSSE2EnsureCapacity(RHO_HEST_SSE2* p, unsigned N, double beta); + + +/** + * Finalize the estimator context, by freeing the aligned buffers used + * internally. + * + * @param [in] p The initialized estimator context to finalize. + */ + +void rhoSSE2Fini(RHO_HEST_SSE2* p); + + +/** + * Estimates the homography using the given context, matches and parameters to + * PROSAC. + * + * The given context must have been initialized. + * + * The matches are provided as two arrays of N single-precision, floating-point + * (x,y) points. Points with corresponding offsets in the two arrays constitute + * a match. The homography estimation attempts to find the 3x3 matrix H which + * best maps the homogeneous-coordinate points in the source array to their + * corresponding homogeneous-coordinate points in the destination array. + * + * Note: At least 4 matches must be provided (N >= 4). + * Note: A point in either array takes up 2 floats. The first of two stores + * the x-coordinate and the second of the two stores the y-coordinate. + * Thus, the arrays resemble this in memory: + * + * src = [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...] + * Matches: | | | | | + * dst = [x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, ...] + * Note: The matches are expected to be provided sorted by quality, or at + * least not be worse-than-random in ordering. + * + * A pointer to the base of an array of N bytes can be provided. It serves as + * an output mask to indicate whether the corresponding match is an inlier to + * the returned homography, if any. A zero indicates an outlier; A non-zero + * value indicates an inlier. + * + * The PROSAC estimator requires a few parameters of its own. These are: + * + * - The maximum distance that a source point projected onto the destination + * plane can be from its putative match and still be considered an + * inlier. Must be non-negative. + * A sane default is 3.0. + * - The maximum number of PROSAC iterations. This corresponds to the + * largest number of samples that will be drawn and tested. + * A sane default is 2000. + * - The RANSAC convergence parameter. This corresponds to the number of + * iterations after which PROSAC will start sampling like RANSAC. + * A sane default is 2000. + * - The confidence threshold. This corresponds to the probability of + * finding a correct solution. Must be bounded by [0, 1]. + * A sane default is 0.995. + * - The minimum number of inliers acceptable. Only a solution with at + * least this many inliers will be returned. The minimum is 4. + * A sane default is 10% of N. + * - The beta-parameter for the non-randomness termination criterion. + * Ignored if non-randomness criterion disabled, otherwise must be + * bounded by (0, 1). + * A sane default is 0.35. + * - Optional flags to control the estimation. Available flags are: + * HEST_FLAG_NONE: + * No special processing. + * HEST_FLAG_ENABLE_NR: + * Enable non-randomness criterion. If set, the beta parameter + * must also be set. + * HEST_FLAG_ENABLE_REFINEMENT: + * Enable refinement of each new best model, as they are found. + * HEST_FLAG_ENABLE_FINAL_REFINEMENT: + * Enable one final refinement of the best model found before + * returning it. + * + * The PROSAC estimator additionally accepts an extrinsic initial guess of H, + * and outputs a final estimate at H provided it was able to find one with a + * minimum of supporting inliers. If it was not, it outputs the all-zero + * matrix. + * + * The extrinsic guess at and final estimate of H are both in the same form: + * A 3x3 single-precision floating-point matrix with step 4. Thus, it is a + * 12-element array of floats, with the elements as follows: + * + * [ H00, H01, H02, , + * H10, H11, H12, , + * H20, H21, H22, ] + * + * The function returns the number of inliers if it was able to find a + * homography with at least the minimum required support, and 0 if it was not. + * + * + * @param [in/out] p The context to use for homography estimation. Must + * be already initialized. Cannot be NULL. + * @param [in] src The pointer to the source points of the matches. + * Must be aligned to 16 bytes. Cannot be NULL. + * @param [in] dst The pointer to the destination points of the matches. + * Must be aligned to 16 bytes. Cannot be NULL. + * @param [out] inl The pointer to the output mask of inlier matches. + * Must be aligned to 16 bytes. May be NULL. + * @param [in] N The number of matches. + * @param [in] maxD The maximum distance. + * @param [in] maxI The maximum number of PROSAC iterations. + * @param [in] rConvg The RANSAC convergence parameter. + * @param [in] cfd The required confidence in the solution. + * @param [in] minInl The minimum required number of inliers. + * @param [in] beta The beta-parameter for the non-randomness criterion. + * @param [in] flags A union of flags to control the estimation. + * @param [in] guessH An extrinsic guess at the solution H, or NULL if + * none provided. + * @param [out] finalH The final estimation of H, or the zero matrix if + * the minimum number of inliers was not met. + * Cannot be NULL. + * @return The number of inliers if the minimum number of + * inliers for acceptance was reached; 0 otherwise. + */ + +unsigned rhoSSE2(RHO_HEST_SSE2* restrict p, /* Homography estimation context. */ + const float* restrict src, /* Source points */ + const float* restrict dst, /* Destination points */ + char* restrict bestInl, /* Inlier mask */ + unsigned N, /* = src.length = dst.length = inl.length */ + float maxD, /* 3.0 */ + unsigned maxI, /* 2000 */ + unsigned rConvg, /* 2000 */ + double cfd, /* 0.995 */ + unsigned minInl, /* 4 */ + double beta, /* 0.35 */ + unsigned flags, /* 0 */ + const float* guessH, /* Extrinsic guess, NULL if none provided */ + float* finalH); /* Final result. */ + + + + +/* Extern C */ +#ifdef __cplusplus +//} +} +#endif + + + + +#endif diff --git a/modules/calib3d/test/test_homography.cpp b/modules/calib3d/test/test_homography.cpp index 59d92905a4c570ef3b532994a10058511e7d61c6..1199cd6cd066f29439725bf43688f87c45d0c818 100644 --- a/modules/calib3d/test/test_homography.cpp +++ b/modules/calib3d/test/test_homography.cpp @@ -62,10 +62,10 @@ #define MAX_COUNT_OF_POINTS 303 #define COUNT_NORM_TYPES 3 -#define METHODS_COUNT 3 +#define METHODS_COUNT 4 int NORM_TYPE[COUNT_NORM_TYPES] = {cv::NORM_L1, cv::NORM_L2, cv::NORM_INF}; -int METHOD[METHODS_COUNT] = {0, cv::RANSAC, cv::LMEDS}; +int METHOD[METHODS_COUNT] = {0, cv::RANSAC, cv::LMEDS, cv::RHO}; using namespace cv; using namespace std; @@ -144,7 +144,7 @@ void CV_HomographyTest::print_information_1(int j, int N, int _method, const Mat cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else cout << "vector "; cout << " Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector "; cout << endl; cout << "Count of points: " << N << endl; cout << endl; - cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl; + cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else if (_method == cv::RHO) cout << "RHO"; else cout << "LMEDS"; cout << endl; cout << "Homography matrix:" << endl; cout << endl; cout << H << endl; cout << endl; cout << "Number of rows: " << H.rows << " Number of cols: " << H.cols << endl; cout << endl; @@ -156,7 +156,7 @@ void CV_HomographyTest::print_information_2(int j, int N, int _method, const Mat cout << "Type of srcPoints: "; if ((j>-1) && (j<2)) cout << "Mat of CV_32FC2"; else cout << "vector "; cout << " Type of dstPoints: "; if (j % 2 == 0) cout << "Mat of CV_32FC2"; else cout << "vector "; cout << endl; cout << "Count of points: " << N << endl; cout << endl; - cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else cout << "LMEDS"; cout << endl; + cout << "Method: "; if (_method == 0) cout << 0; else if (_method == 8) cout << "RANSAC"; else if (_method == cv::RHO) cout << "RHO"; else cout << "LMEDS"; cout << endl; cout << "Original matrix:" << endl; cout << endl; cout << H << endl; cout << endl; cout << "Found matrix:" << endl; cout << endl; @@ -339,14 +339,15 @@ void CV_HomographyTest::run(int) continue; } + case cv::RHO: case RANSAC: { cv::Mat mask [4]; double diff; - Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, RANSAC, reproj_threshold, mask[0]), - cv::findHomography(src_mat_2f, dst_vec, RANSAC, reproj_threshold, mask[1]), - cv::findHomography(src_vec, dst_mat_2f, RANSAC, reproj_threshold, mask[2]), - cv::findHomography(src_vec, dst_vec, RANSAC, reproj_threshold, mask[3]) }; + Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, method, reproj_threshold, mask[0]), + cv::findHomography(src_mat_2f, dst_vec, method, reproj_threshold, mask[1]), + cv::findHomography(src_vec, dst_mat_2f, method, reproj_threshold, mask[2]), + cv::findHomography(src_vec, dst_vec, method, reproj_threshold, mask[3]) }; for (int j = 0; j < 4; ++j) { @@ -466,14 +467,15 @@ void CV_HomographyTest::run(int) continue; } + case cv::RHO: case RANSAC: { cv::Mat mask_res [4]; - Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, RANSAC, reproj_threshold, mask_res[0]), - cv::findHomography(src_mat_2f, dst_vec, RANSAC, reproj_threshold, mask_res[1]), - cv::findHomography(src_vec, dst_mat_2f, RANSAC, reproj_threshold, mask_res[2]), - cv::findHomography(src_vec, dst_vec, RANSAC, reproj_threshold, mask_res[3]) }; + Mat H_res_64 [4] = { cv::findHomography(src_mat_2f, dst_mat_2f, method, reproj_threshold, mask_res[0]), + cv::findHomography(src_mat_2f, dst_vec, method, reproj_threshold, mask_res[1]), + cv::findHomography(src_vec, dst_mat_2f, method, reproj_threshold, mask_res[2]), + cv::findHomography(src_vec, dst_vec, method, reproj_threshold, mask_res[3]) }; for (int j = 0; j < 4; ++j) {