From 68e59d6154c6f60cbcb41358b29e42c03d184961 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Mon, 17 Nov 2014 15:02:18 -0500 Subject: [PATCH 001/171] [RHO] Initial commit of RHO algorithm for rapid homography estimation. Implements the RHO algorithm as presented in Paper: 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. - Complete, heavily documented reference C implementation, as well as temporarily disabled dirty SSE2 port. - Enabled tests for RHO in test_homography; Currently these fail presumably due to too-stringent accuracy requirements. - Refinement and final refinement are not yet functional; Do not pass their corresponding flags to RHO. --- modules/calib3d/include/opencv2/calib3d.hpp | 3 +- modules/calib3d/src/fundam.cpp | 77 +- modules/calib3d/src/rhorefc.cpp | 2042 +++++++++++++++++++ modules/calib3d/src/rhorefc.h | 368 ++++ modules/calib3d/src/rhosse2.cpp | 1497 ++++++++++++++ modules/calib3d/src/rhosse2.h | 331 +++ modules/calib3d/test/test_homography.cpp | 26 +- 7 files changed, 4329 insertions(+), 15 deletions(-) create mode 100644 modules/calib3d/src/rhorefc.cpp create mode 100644 modules/calib3d/src/rhorefc.h create mode 100644 modules/calib3d/src/rhosse2.cpp create mode 100644 modules/calib3d/src/rhosse2.h diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 4f405afc6..397b87a0e 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 c700ece70..07cf68a0f 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 000000000..cda70f416 --- /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 000000000..3c0014188 --- /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 000000000..5fc5bc187 --- /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 000000000..ad3d2b18b --- /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 59d92905a..1199cd6cd 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) { From 33a3fba2d1d448166e03a4f83864adc33a657af9 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Mon, 12 Jan 2015 04:58:07 -0500 Subject: [PATCH 002/171] Work on LevMarq code. Refactoring of Cholesky decomposition. Fix for memory corruption bug. LevMarq as a whole still non-functional. --- modules/calib3d/src/rhorefc.cpp | 349 ++++++++++++++++++++------------ modules/calib3d/src/rhorefc.h | 1 - 2 files changed, 218 insertions(+), 132 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index cda70f416..1da916d9b 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -132,24 +132,25 @@ static inline unsigned sacCalcIterBound(double confidence, 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 sacCalcJacobianErrors(const float* restrict H, + const float* restrict src, + const float* restrict dst, + const char* restrict inl, + unsigned N, + float (* restrict JtJ)[8], + float* restrict Jte, + float* restrict Sp); +static inline float sacDs(const float (*JtJ)[8], + const float* dH, + const float* Jte); +static inline int sacChol8x8Damped (const float (*A)[8], + float lambda, + 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); @@ -189,6 +190,11 @@ int rhoRefCInit(RHO_HEST_REFC* p){ p->nr.size = 0; p->nr.beta = 0.0; + p->lm.ws = NULL; + p->lm.JtJ = NULL; + p->lm.tmp1 = NULL; + p->lm.Jte = NULL; + int areAllAllocsSuccessful = p->ctrl.smpl && p->curr.H && @@ -502,10 +508,30 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ return 0; } + /** + * LevMarq workspace alloc. + * + * Runs third, consists only in a few conditional mallocs. If malloc fails + * we wish to quit before doing any serious work. + */ + + if(sacIsRefineEnabled(p) || sacIsFinalRefineEnabled(p)){ + p->lm.ws = (float*)almalloc(2*8*8*sizeof(float) + 1*8*sizeof(float)); + if(!p->lm.ws){ + return 0; + } + + p->lm.JtJ = (float(*)[8])(p->lm.ws + 0*8*8); + p->lm.tmp1 = (float(*)[8])(p->lm.ws + 1*8*8); + p->lm.Jte = (float*) (p->lm.ws + 2*8*8); + }else{ + p->lm.ws = NULL; + } + /** * Reset scalar per-run state. * - * Runs third because there's no point in resetting/calculating a large + * Runs fourth because there's no point in resetting/calculating a large * number of fields if something in the above junk failed. */ @@ -570,6 +596,13 @@ static inline void sacFiniRun(RHO_HEST_REFC* p){ p->best.inl = NULL; p->curr.inl = NULL; + + /** + * ₣ree the Levenberg-Marquardt workspace. + */ + + alfree(p->lm.ws); + p->lm.ws = NULL; } /** @@ -912,6 +945,11 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ * * If a "good" model that is also the best was encountered, update epsilon, * since + * + * Reads: eval.good, eval.delta, eval.epsilon, eval.t_M, eval.m_S, + * curr.numInl, best.numInl + * Writes: eval.epsilon, eval.delta, eval.A, eval.lambdaAccept, + * eval.lambdaReject */ static inline void sacUpdateSPRT(RHO_HEST_REFC* p){ @@ -1032,9 +1070,11 @@ 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; @@ -1538,18 +1578,47 @@ static inline int sacCanRefine(RHO_HEST_REFC* p){ */ static inline void sacRefine(RHO_HEST_REFC* p){ - int i, j; - float S; /* Sum of squared errors */ - float L = 1;/* Lambda of LevMarq */ + int i = 0; + float S = 0, newS = 0, dS = 0;/* Sum of squared errors */ + float R = 0; /* R - Parameter */ + float L = 1.0f; /* Lambda of LevMarq */ + float dH[8], newH[8]; + /** + * Iteratively refine the homography. + */ + + /* Find initial conditions */ + sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, + p->lm.JtJ, p->lm.Jte, &S); + + /* Levenberg-Marquardt Loop */ 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); + /* The code below becomes an infinite loop when L reeaches infinity. + while(sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1)){ + L *= 2.0f; + }*/ + sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1); + sacTRInv8x8 (p->lm.tmp1, p->lm.tmp1); + sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH); + sacSub8x1 (newH, p->best.H, dH); + sacCalcJacobianErrors(newH, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, + NULL, NULL, &newS); + dS = sacDs (p->lm.JtJ, dH, p->lm.Jte); + R = (S - newS)/(fabs(dS) > DBL_EPSILON ? dS : 1);/* Don't understand */ + + if(R > 0.75f){ + L *= 0.5f; + }else if(R < 0.25f){ + L *= 2.0f; + } + + if(newS < S){ + S = newS; + memcpy(p->best.H, newH, sizeof(newH)); + sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, + p->lm.JtJ, p->lm.Jte, &S); + } } } @@ -1564,31 +1633,22 @@ static inline void sacRefine(RHO_HEST_REFC* p){ * * 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){ +static inline void sacCalcJacobianErrors(const float* restrict H, + const float* restrict src, + const float* restrict dst, + const char* restrict inl, + unsigned N, + float (* restrict JtJ)[8], + float* restrict Jte, + float* restrict Sp){ unsigned i; float S; /* Zero out JtJ, Jte and S */ - memset(JtJ, 0, 8*8*sizeof(*JtJ)); - memset(Jte, 0, 8*1*sizeof(*Jte)); + if(JtJ){memset(JtJ, 0, 8*8*sizeof(float));} + if(Jte){memset(Jte, 0, 8*1*sizeof(float));} S = 0.0f; /* Additively compute JtJ and Jte */ @@ -1633,81 +1693,119 @@ static inline void sacCalcJtMats(float (* restrict JtJ)[8], S += e; /* Compute Jacobian */ - float dxh11 = x * iW; - float dxh12 = y * iW; - float dxh13 = iW; - float dxh21 = 0.0f; - float dxh22 = 0.0f; - float dxh23 = 0.0f; - float dxh31 = -reprojX*x * iW; - float dxh32 = -reprojX*y * iW; + if(JtJ || Jte){ + float dxh11 = x * iW; + float dxh12 = y * iW; + float dxh13 = iW; + float dxh21 = 0.0f; + float dxh22 = 0.0f; + float dxh23 = 0.0f; + float dxh31 = -reprojX*x * iW; + float dxh32 = -reprojX*y * iW; - float dyh11 = 0.0f; - float dyh12 = 0.0f; - float dyh13 = 0.0f; - float dyh21 = x * iW; - float dyh22 = y * iW; - float dyh23 = iW; - float dyh31 = -reprojY*x * iW; - float dyh32 = -reprojY*y * iW; + float dyh11 = 0.0f; + float dyh12 = 0.0f; + float dyh13 = 0.0f; + float dyh21 = x * iW; + float dyh22 = y * iW; + float dyh23 = iW; + float dyh31 = -reprojY*x * iW; + float dyh32 = -reprojY*y * iW; - /* Update Jte: X Y */ - Jte[0] += eX *dxh11 + eY *dyh11; - Jte[1] += eX *dxh12 + eY *dyh12; - Jte[2] += eX *dxh13 + eY *dyh13; - Jte[3] += eX *dxh21 + eY *dyh21; - Jte[4] += eX *dxh22 + eY *dyh22; - Jte[5] += eX *dxh23 + eY *dyh23; - Jte[6] += eX *dxh31 + eY *dyh31; - Jte[7] += eX *dxh32 + eY *dyh32; + /* Update Jte: X Y */ + if(Jte){ + Jte[0] += eX *dxh11 + eY *dyh11; + Jte[1] += eX *dxh12 + eY *dyh12; + Jte[2] += eX *dxh13 + eY *dyh13; + Jte[3] += eX *dxh21 + eY *dyh21; + Jte[4] += eX *dxh22 + eY *dyh22; + Jte[5] += eX *dxh23 + eY *dyh23; + Jte[6] += eX *dxh31 + eY *dyh31; + Jte[7] += eX *dxh32 + eY *dyh32; + } - /* Update JtJ: X Y */ - JtJ[0][0] += dxh11*dxh11 + dyh11*dyh11; + /* Update JtJ: X Y */ + if(JtJ){ + JtJ[0][0] += dxh11*dxh11 + dyh11*dyh11; - JtJ[1][0] += dxh11*dxh12 + dyh11*dyh12; - JtJ[1][1] += dxh12*dxh12 + dyh12*dyh12; + JtJ[1][0] += dxh11*dxh12 + dyh11*dyh12; + JtJ[1][1] += dxh12*dxh12 + dyh12*dyh12; - JtJ[2][0] += dxh11*dxh13 + dyh11*dyh13; - JtJ[2][1] += dxh12*dxh13 + dyh12*dyh13; - JtJ[2][2] += dxh13*dxh13 + dyh13*dyh13; + JtJ[2][0] += dxh11*dxh13 + dyh11*dyh13; + JtJ[2][1] += dxh12*dxh13 + dyh12*dyh13; + JtJ[2][2] += dxh13*dxh13 + dyh13*dyh13; - JtJ[3][0] += dxh11*dxh21 + dyh11*dyh21; - JtJ[3][1] += dxh12*dxh21 + dyh12*dyh21; - JtJ[3][2] += dxh13*dxh21 + dyh13*dyh21; - JtJ[3][3] += dxh21*dxh21 + dyh21*dyh21; + JtJ[3][0] += dxh11*dxh21 + dyh11*dyh21; + JtJ[3][1] += dxh12*dxh21 + dyh12*dyh21; + JtJ[3][2] += dxh13*dxh21 + dyh13*dyh21; + JtJ[3][3] += dxh21*dxh21 + dyh21*dyh21; - JtJ[4][0] += dxh11*dxh22 + dyh11*dyh22; - JtJ[4][1] += dxh12*dxh22 + dyh12*dyh22; - JtJ[4][2] += dxh13*dxh22 + dyh13*dyh22; - JtJ[4][3] += dxh21*dxh22 + dyh21*dyh22; - JtJ[4][4] += dxh22*dxh22 + dyh22*dyh22; + JtJ[4][0] += dxh11*dxh22 + dyh11*dyh22; + JtJ[4][1] += dxh12*dxh22 + dyh12*dyh22; + JtJ[4][2] += dxh13*dxh22 + dyh13*dyh22; + JtJ[4][3] += dxh21*dxh22 + dyh21*dyh22; + JtJ[4][4] += dxh22*dxh22 + dyh22*dyh22; - JtJ[5][0] += dxh11*dxh23 + dyh11*dyh23; - JtJ[5][1] += dxh12*dxh23 + dyh12*dyh23; - JtJ[5][2] += dxh13*dxh23 + dyh13*dyh23; - JtJ[5][3] += dxh21*dxh23 + dyh21*dyh23; - JtJ[5][4] += dxh22*dxh23 + dyh22*dyh23; - JtJ[5][5] += dxh23*dxh23 + dyh23*dyh23; + JtJ[5][0] += dxh11*dxh23 + dyh11*dyh23; + JtJ[5][1] += dxh12*dxh23 + dyh12*dyh23; + JtJ[5][2] += dxh13*dxh23 + dyh13*dyh23; + JtJ[5][3] += dxh21*dxh23 + dyh21*dyh23; + JtJ[5][4] += dxh22*dxh23 + dyh22*dyh23; + JtJ[5][5] += dxh23*dxh23 + dyh23*dyh23; - JtJ[6][0] += dxh11*dxh31 + dyh11*dyh31; - JtJ[6][1] += dxh12*dxh31 + dyh12*dyh31; - JtJ[6][2] += dxh13*dxh31 + dyh13*dyh31; - JtJ[6][3] += dxh21*dxh31 + dyh21*dyh31; - JtJ[6][4] += dxh22*dxh31 + dyh22*dyh31; - JtJ[6][5] += dxh23*dxh31 + dyh23*dyh31; - JtJ[6][6] += dxh31*dxh31 + dyh31*dyh31; + JtJ[6][0] += dxh11*dxh31 + dyh11*dyh31; + JtJ[6][1] += dxh12*dxh31 + dyh12*dyh31; + JtJ[6][2] += dxh13*dxh31 + dyh13*dyh31; + JtJ[6][3] += dxh21*dxh31 + dyh21*dyh31; + JtJ[6][4] += dxh22*dxh31 + dyh22*dyh31; + JtJ[6][5] += dxh23*dxh31 + dyh23*dyh31; + JtJ[6][6] += dxh31*dxh31 + dyh31*dyh31; - JtJ[7][0] += dxh11*dxh32 + dyh11*dyh32; - JtJ[7][1] += dxh12*dxh32 + dyh12*dyh32; - JtJ[7][2] += dxh13*dxh32 + dyh13*dyh32; - JtJ[7][3] += dxh21*dxh32 + dyh21*dyh32; - JtJ[7][4] += dxh22*dxh32 + dyh22*dyh32; - JtJ[7][5] += dxh23*dxh32 + dyh23*dyh32; - JtJ[7][6] += dxh31*dxh32 + dyh31*dyh32; - JtJ[7][7] += dxh32*dxh32 + dyh32*dyh32; + JtJ[7][0] += dxh11*dxh32 + dyh11*dyh32; + JtJ[7][1] += dxh12*dxh32 + dyh12*dyh32; + JtJ[7][2] += dxh13*dxh32 + dyh13*dyh32; + JtJ[7][3] += dxh21*dxh32 + dyh21*dyh32; + JtJ[7][4] += dxh22*dxh32 + dyh22*dyh32; + JtJ[7][5] += dxh23*dxh32 + dyh23*dyh32; + JtJ[7][6] += dxh31*dxh32 + dyh31*dyh32; + JtJ[7][7] += dxh32*dxh32 + dyh32*dyh32; + } + } } - *Sp = S; + if(Sp){*Sp = S;} +} + +/** + * Compute the derivative of the rate of change of the SSE. + * + * Inspired entirely by OpenCV's levmarq.cpp. To be reviewed. + */ + +static inline float sacDs(const float (*JtJ)[8], + const float* dH, + const float* Jte){ + float tdH[8]; + int i, j; + float dS = 0; + + /* Perform tdH = -JtJ*dH + 2*Jte. */ + for(i=0;i<8;i++){ + tdH[i] = 0; + + for(j=0;j<8;j++){ + tdH[i] -= JtJ[i][j] * dH[j]; + } + + tdH[i] += 2*Jte[i]; + } + + /* Perform dS = dH.dot(tdH). */ + for(i=0;i<8;i++){ + dS += dH[i]*tdH[i]; + } + + return dS; } /** @@ -1718,14 +1816,20 @@ static inline void sacCalcJtMats(float (* restrict JtJ)[8], * A and L can overlap fully (in-place) or not at all, but may not partially * overlap. * + * For damping, the diagonal elements are scaled by 1.0 + lambda. + * + * Returns 0 if decomposition successful, and non-zero otherwise. + * * Source: http://en.wikipedia.org/wiki/Cholesky_decomposition# * The_Cholesky.E2.80.93Banachiewicz_and_Cholesky.E2.80.93Crout_algorithms */ -static inline void sacChol8x8(const float (*A)[8], - float (*L)[8]){ +static inline int sacChol8x8Damped(const float (*A)[8], + float lambda, + float (*L)[8]){ const register int N = 8; int i, j, k; + float lambdap1 = lambda + 1.0f; double x; for(i=0;i Date: Mon, 12 Jan 2015 05:37:40 -0500 Subject: [PATCH 003/171] More LevMarq bugfixes. LevMarq now doesn't outright fail, but doesn't seem to improve things much if at all. --- modules/calib3d/src/fundam.cpp | 3 ++- modules/calib3d/src/rhorefc.cpp | 18 +++++++++++++----- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 07cf68a0f..79beb3e5c 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -327,7 +327,8 @@ static bool createAndRunRHORegistrator(double confidence, int maxIters, double r confidence, 4, beta, - RHO_FLAG_ENABLE_NR, + /*RHO_FLAG_ENABLE_NR,*/ + RHO_FLAG_ENABLE_NR | RHO_FLAG_ENABLE_FINAL_REFINEMENT, NULL, (float*)tmpH.data); rhoRefCFini(&p); diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 1da916d9b..b4b34eb7c 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -66,7 +66,7 @@ #define CHI_SQ 1.645 #define RLO 0.25 #define RHI 0.75 -#define MAXLEVMARQITERS 10 +#define MAXLEVMARQITERS 50 #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 */ @@ -1592,13 +1592,21 @@ static inline void sacRefine(RHO_HEST_REFC* p){ sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, p->lm.JtJ, p->lm.Jte, &S); + /*{ + for(int j=0;j<8;j++){ + for(int k=0;k<8;k++){ + printf("%12.6g%s", p->lm.JtJ[j][k], k==7 ? "\n" : ", "); + } + } + }*/ + /* Levenberg-Marquardt Loop */ for(i=0;ilm.JtJ, L, p->lm.tmp1)){ L *= 2.0f; - }*/ - sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1); + } + //sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1); sacTRInv8x8 (p->lm.tmp1, p->lm.tmp1); sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH); sacSub8x1 (newH, p->best.H, dH); @@ -1682,7 +1690,7 @@ static inline void sacCalcJacobianErrors(const float* restrict H, float X = dst[2*i+0]; float Y = dst[2*i+1]; float W = (H[6]*x + H[7]*y + 1.0f); - float iW = W FLT_EPSILON ? 1.0f/W : 0; float reprojX = (H[0]*x + H[1]*y + H[2]) * iW; float reprojY = (H[3]*x + H[4]*y + H[5]) * iW; From 4401095b38d02c5d0b99683bfb852d819941578c Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Mon, 12 Jan 2015 11:33:05 -0500 Subject: [PATCH 004/171] Optimizations to Jacobian and error calculations. Deleted multiplications by zero and consequent additions of zero terms. --- modules/calib3d/src/rhorefc.cpp | 88 ++++++++++++++++----------------- 1 file changed, 44 insertions(+), 44 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index b4b34eb7c..5777275ff 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -1722,61 +1722,61 @@ static inline void sacCalcJacobianErrors(const float* restrict H, /* Update Jte: X Y */ if(Jte){ - Jte[0] += eX *dxh11 + eY *dyh11; - Jte[1] += eX *dxh12 + eY *dyh12; - Jte[2] += eX *dxh13 + eY *dyh13; - Jte[3] += eX *dxh21 + eY *dyh21; - Jte[4] += eX *dxh22 + eY *dyh22; - Jte[5] += eX *dxh23 + eY *dyh23; - Jte[6] += eX *dxh31 + eY *dyh31; - Jte[7] += eX *dxh32 + eY *dyh32; + Jte[0] += eX *dxh11 ;/* +0 */ + Jte[1] += eX *dxh12 ;/* +0 */ + Jte[2] += eX *dxh13 ;/* +0 */ + Jte[3] += eY *dyh21;/* 0+ */ + Jte[4] += eY *dyh22;/* 0+ */ + Jte[5] += eY *dyh23;/* 0+ */ + Jte[6] += eX *dxh31 + eY *dyh31;/* + */ + Jte[7] += eX *dxh32 + eY *dyh32;/* + */ } /* Update JtJ: X Y */ if(JtJ){ - JtJ[0][0] += dxh11*dxh11 + dyh11*dyh11; + JtJ[0][0] += dxh11*dxh11 ;/* +0 */ - JtJ[1][0] += dxh11*dxh12 + dyh11*dyh12; - JtJ[1][1] += dxh12*dxh12 + dyh12*dyh12; + JtJ[1][0] += dxh11*dxh12 ;/* +0 */ + JtJ[1][1] += dxh12*dxh12 ;/* +0 */ - JtJ[2][0] += dxh11*dxh13 + dyh11*dyh13; - JtJ[2][1] += dxh12*dxh13 + dyh12*dyh13; - JtJ[2][2] += dxh13*dxh13 + dyh13*dyh13; + JtJ[2][0] += dxh11*dxh13 ;/* +0 */ + JtJ[2][1] += dxh12*dxh13 ;/* +0 */ + JtJ[2][2] += dxh13*dxh13 ;/* +0 */ - JtJ[3][0] += dxh11*dxh21 + dyh11*dyh21; - JtJ[3][1] += dxh12*dxh21 + dyh12*dyh21; - JtJ[3][2] += dxh13*dxh21 + dyh13*dyh21; - JtJ[3][3] += dxh21*dxh21 + dyh21*dyh21; + /*JtJ[3][0] += ;/* 0+0 */ + /*JtJ[3][1] += ;/* 0+0 */ + /*JtJ[3][2] += ;/* 0+0 */ + JtJ[3][3] += dyh21*dyh21;/* 0+ */ - JtJ[4][0] += dxh11*dxh22 + dyh11*dyh22; - JtJ[4][1] += dxh12*dxh22 + dyh12*dyh22; - JtJ[4][2] += dxh13*dxh22 + dyh13*dyh22; - JtJ[4][3] += dxh21*dxh22 + dyh21*dyh22; - JtJ[4][4] += dxh22*dxh22 + dyh22*dyh22; + /*JtJ[4][0] += ;/* 0+0 */ + /*JtJ[4][1] += ;/* 0+0 */ + /*JtJ[4][2] += ;/* 0+0 */ + JtJ[4][3] += dyh21*dyh22;/* 0+ */ + JtJ[4][4] += dyh22*dyh22;/* 0+ */ - JtJ[5][0] += dxh11*dxh23 + dyh11*dyh23; - JtJ[5][1] += dxh12*dxh23 + dyh12*dyh23; - JtJ[5][2] += dxh13*dxh23 + dyh13*dyh23; - JtJ[5][3] += dxh21*dxh23 + dyh21*dyh23; - JtJ[5][4] += dxh22*dxh23 + dyh22*dyh23; - JtJ[5][5] += dxh23*dxh23 + dyh23*dyh23; + /*JtJ[5][0] += ;/* 0+0 */ + /*JtJ[5][1] += ;/* 0+0 */ + /*JtJ[5][2] += ;/* 0+0 */ + JtJ[5][3] += dyh21*dyh23;/* 0+ */ + JtJ[5][4] += dyh22*dyh23;/* 0+ */ + JtJ[5][5] += dyh23*dyh23;/* 0+ */ - JtJ[6][0] += dxh11*dxh31 + dyh11*dyh31; - JtJ[6][1] += dxh12*dxh31 + dyh12*dyh31; - JtJ[6][2] += dxh13*dxh31 + dyh13*dyh31; - JtJ[6][3] += dxh21*dxh31 + dyh21*dyh31; - JtJ[6][4] += dxh22*dxh31 + dyh22*dyh31; - JtJ[6][5] += dxh23*dxh31 + dyh23*dyh31; - JtJ[6][6] += dxh31*dxh31 + dyh31*dyh31; + JtJ[6][0] += dxh11*dxh31 ;/* +0 */ + JtJ[6][1] += dxh12*dxh31 ;/* +0 */ + JtJ[6][2] += dxh13*dxh31 ;/* +0 */ + JtJ[6][3] += dyh21*dyh31;/* 0+ */ + JtJ[6][4] += dyh22*dyh31;/* 0+ */ + JtJ[6][5] += dyh23*dyh31;/* 0+ */ + JtJ[6][6] += dxh31*dxh31 + dyh31*dyh31;/* + */ - JtJ[7][0] += dxh11*dxh32 + dyh11*dyh32; - JtJ[7][1] += dxh12*dxh32 + dyh12*dyh32; - JtJ[7][2] += dxh13*dxh32 + dyh13*dyh32; - JtJ[7][3] += dxh21*dxh32 + dyh21*dyh32; - JtJ[7][4] += dxh22*dxh32 + dyh22*dyh32; - JtJ[7][5] += dxh23*dxh32 + dyh23*dyh32; - JtJ[7][6] += dxh31*dxh32 + dyh31*dyh32; - JtJ[7][7] += dxh32*dxh32 + dyh32*dyh32; + JtJ[7][0] += dxh11*dxh32 ;/* +0 */ + JtJ[7][1] += dxh12*dxh32 ;/* +0 */ + JtJ[7][2] += dxh13*dxh32 ;/* +0 */ + JtJ[7][3] += dyh21*dyh32;/* 0+ */ + JtJ[7][4] += dyh22*dyh32;/* 0+ */ + JtJ[7][5] += dyh23*dyh32;/* 0+ */ + JtJ[7][6] += dxh31*dxh32 + dyh31*dyh32;/* + */ + JtJ[7][7] += dxh32*dxh32 + dyh32*dyh32;/* + */ } } } From ff91af825da1ad4e08de2e224ce5260b1ab8a6e3 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Wed, 14 Jan 2015 04:22:48 -0500 Subject: [PATCH 005/171] LevMarq made semi-functional. Replaced the complex rules OpenCV uses to select lambda with a naive but fast heuristic. It's imperfect but produces good results. It is still subject to the same problem as OpenCV - namely, on occasion LevMarq will make a poor result even worse. --- modules/calib3d/src/rhorefc.cpp | 115 ++++++++++++++++++++------------ 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 5777275ff..4e96eaf67 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -66,7 +66,7 @@ #define CHI_SQ 1.645 #define RLO 0.25 #define RHI 0.75 -#define MAXLEVMARQITERS 50 +#define MAXLEVMARQITERS 100 #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 */ @@ -1571,10 +1571,20 @@ static inline int sacCanRefine(RHO_HEST_REFC* p){ return p->best.numInl > m; } + + +static inline void dumpMat8x8(float (*M)[8]){ + for(int i=0;i<8;i++){ + printf("\t"); + for(int j=0;j<=i;j++){ + printf("%14.6e%s", M[i][j], j==i?"\n":", "); + } + } + printf("\n"); +} + /** - * Refines the best-so-far homography. - * - * BUG: Totally broken for now. DO NOT ENABLE. + * Refines the best-so-far homography (p->best.H). */ static inline void sacRefine(RHO_HEST_REFC* p){ @@ -1591,41 +1601,62 @@ static inline void sacRefine(RHO_HEST_REFC* p){ /* Find initial conditions */ sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, p->lm.JtJ, p->lm.Jte, &S); + /*printf("Initial Error: %12.6f\n", S);*/ - /*{ - for(int j=0;j<8;j++){ - for(int k=0;k<8;k++){ - printf("%12.6g%s", p->lm.JtJ[j][k], k==7 ? "\n" : ", "); - } - } - }*/ - - /* Levenberg-Marquardt Loop */ + /*Levenberg-Marquardt Loop.*/ for(i=0;ilm.JtJ, L, p->lm.tmp1)){ - L *= 2.0f; + /** + * Attempt a step given current state + * - Jacobian-x-Jacobian (JtJ) + * - Jacobian-x-error (Jte) + * - Sum of squared errors (S) + * and current parameter + * - Lambda (L) + * . + * + * This is done by solving the system of equations + * Ax = b + * where A (JtJ) and b (Jte) are sourced from our current state, and + * the solution x becomes a step (dH) that is applied to best.H in + * order to compute a candidate homography (newH). + * + * The system above is solved by Cholesky decomposition of a + * sufficently-damped JtJ into a lower-triangular matrix (and its + * transpose), whose inverse is then computed. This inverse (and its + * transpose) then multiply JtJ in order to find dH. + */ + + /*printf("Lambda: %f\n", L);*/ + while(!sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1)){ + L *= 2.0f;/*printf("CholFail! Lambda: %f\n", L);*/ } - //sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1); sacTRInv8x8 (p->lm.tmp1, p->lm.tmp1); sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH); sacSub8x1 (newH, p->best.H, dH); sacCalcJacobianErrors(newH, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, NULL, NULL, &newS); - dS = sacDs (p->lm.JtJ, dH, p->lm.Jte); - R = (S - newS)/(fabs(dS) > DBL_EPSILON ? dS : 1);/* Don't understand */ - if(R > 0.75f){ - L *= 0.5f; - }else if(R < 0.25f){ - L *= 2.0f; - } + /** + * If the new Sum of Square Errors (newS) corresponding to newH is + * lower than the previous one, save the current state and undamp + * sligthly (decrease L). Else damp more (increase L). + */ if(newS < S){ S = newS; memcpy(p->best.H, newH, sizeof(newH)); sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, p->lm.JtJ, p->lm.Jte, &S); + /*printf("Error: %12.6f\n", S);*/ + L *= 0.5; + if(L1.0f/FLT_EPSILON){ + break; + } } } } @@ -1705,15 +1736,15 @@ static inline void sacCalcJacobianErrors(const float* restrict H, float dxh11 = x * iW; float dxh12 = y * iW; float dxh13 = iW; - float dxh21 = 0.0f; - float dxh22 = 0.0f; - float dxh23 = 0.0f; + /*float dxh21 = 0.0f;*/ + /*float dxh22 = 0.0f;*/ + /*float dxh23 = 0.0f;*/ float dxh31 = -reprojX*x * iW; float dxh32 = -reprojX*y * iW; - float dyh11 = 0.0f; - float dyh12 = 0.0f; - float dyh13 = 0.0f; + /*float dyh11 = 0.0f;*/ + /*float dyh12 = 0.0f;*/ + /*float dyh13 = 0.0f;*/ float dyh21 = x * iW; float dyh22 = y * iW; float dyh23 = iW; @@ -1743,20 +1774,20 @@ static inline void sacCalcJacobianErrors(const float* restrict H, JtJ[2][1] += dxh12*dxh13 ;/* +0 */ JtJ[2][2] += dxh13*dxh13 ;/* +0 */ - /*JtJ[3][0] += ;/* 0+0 */ - /*JtJ[3][1] += ;/* 0+0 */ - /*JtJ[3][2] += ;/* 0+0 */ + /*JtJ[3][0] += ; 0+0 */ + /*JtJ[3][1] += ; 0+0 */ + /*JtJ[3][2] += ; 0+0 */ JtJ[3][3] += dyh21*dyh21;/* 0+ */ - /*JtJ[4][0] += ;/* 0+0 */ - /*JtJ[4][1] += ;/* 0+0 */ - /*JtJ[4][2] += ;/* 0+0 */ + /*JtJ[4][0] += ; 0+0 */ + /*JtJ[4][1] += ; 0+0 */ + /*JtJ[4][2] += ; 0+0 */ JtJ[4][3] += dyh21*dyh22;/* 0+ */ JtJ[4][4] += dyh22*dyh22;/* 0+ */ - /*JtJ[5][0] += ;/* 0+0 */ - /*JtJ[5][1] += ;/* 0+0 */ - /*JtJ[5][2] += ;/* 0+0 */ + /*JtJ[5][0] += ; 0+0 */ + /*JtJ[5][1] += ; 0+0 */ + /*JtJ[5][2] += ; 0+0 */ JtJ[5][3] += dyh21*dyh23;/* 0+ */ JtJ[5][4] += dyh22*dyh23;/* 0+ */ JtJ[5][5] += dyh23*dyh23;/* 0+ */ @@ -1826,7 +1857,7 @@ static inline float sacDs(const float (*JtJ)[8], * * For damping, the diagonal elements are scaled by 1.0 + lambda. * - * Returns 0 if decomposition successful, and non-zero otherwise. + * Returns zero if decomposition unsuccessful, and non-zero otherwise. * * Source: http://en.wikipedia.org/wiki/Cholesky_decomposition# * The_Cholesky.E2.80.93Banachiewicz_and_Cholesky.E2.80.93Crout_algorithms @@ -1857,13 +1888,13 @@ static inline int sacChol8x8Damped(const float (*A)[8], x -= (double)L[j][k] * L[j][k];/* - Sum_{k=0..j-1} Ljk^2 */ } if(x<0){ - return 1; + return 0; } L[j][j] = sqrt(x); /* Ljj = sqrt( ... ) */ } } - return 0; + return 1; } /** From 02124f19e6a445b0712495f69f829afb849214fe Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Thu, 15 Jan 2015 04:21:16 -0500 Subject: [PATCH 006/171] Further LevMarq improvements. Implemented a damping-parameter choice strategy similar to that described in http://www2.imm.dtu.dk/documents/ftp/tr99/tr05_99.pdf. Removed a few debug statements. Chosen a new starting lambda value, 0.01. We now actually output the mask of inliers. --- modules/calib3d/src/fundam.cpp | 5 +- modules/calib3d/src/rhorefc.cpp | 109 ++++++++++++++++---------------- 2 files changed, 57 insertions(+), 57 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 79beb3e5c..1fafc14ec 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -287,7 +287,9 @@ static bool createAndRunRHORegistrator(double confidence, int maxIters, double r /* Run RHO. Needs cleanup or separate function to invoke. */ Mat tmpH = Mat(3, 3, CV_32FC1); - tempMask = Mat(npoints, 1, CV_8U); + if(!tempMask.data){ + tempMask = Mat(npoints, 1, CV_8U); + } double beta = 0.35;/* 0.35 is a value that often works. */ #if CV_SSE2 && 0 @@ -339,6 +341,7 @@ static bool createAndRunRHORegistrator(double confidence, int maxIters, double r for(int k=0;kbest.numInl > m; } - - -static inline void dumpMat8x8(float (*M)[8]){ - for(int i=0;i<8;i++){ - printf("\t"); - for(int j=0;j<=i;j++){ - printf("%14.6e%s", M[i][j], j==i?"\n":", "); - } - } - printf("\n"); -} - /** * Refines the best-so-far homography (p->best.H). */ static inline void sacRefine(RHO_HEST_REFC* p){ - int i = 0; - float S = 0, newS = 0, dS = 0;/* Sum of squared errors */ - float R = 0; /* R - Parameter */ - float L = 1.0f; /* Lambda of LevMarq */ + int i; + float S, newS; /* Sum of squared errors */ + float gain; /* Gain-parameter. */ + float L = 0.01f;/* Lambda of LevMarq */ float dH[8], newH[8]; /** @@ -1601,7 +1593,6 @@ static inline void sacRefine(RHO_HEST_REFC* p){ /* Find initial conditions */ sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, p->lm.JtJ, p->lm.Jte, &S); - /*printf("Initial Error: %12.6f\n", S);*/ /*Levenberg-Marquardt Loop.*/ for(i=0;ilm.JtJ, L, p->lm.tmp1)){ - L *= 2.0f;/*printf("CholFail! Lambda: %f\n", L);*/ + L *= 2.0f; } sacTRInv8x8 (p->lm.tmp1, p->lm.tmp1); sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH); sacSub8x1 (newH, p->best.H, dH); sacCalcJacobianErrors(newH, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, NULL, NULL, &newS); + gain = sacLMGain(dH, p->lm.Jte, S, newS, L); + /*printf("Lambda: %12.6f S: %12.6f newS: %12.6f Gain: %12.6f\n", + L, S, newS, gain);*/ /** - * If the new Sum of Square Errors (newS) corresponding to newH is - * lower than the previous one, save the current state and undamp - * sligthly (decrease L). Else damp more (increase L). + * If the gain is positive (i.e., the new Sum of Square Errors (newS) + * corresponding to newH is lower than the previous one (S) ), save + * the current state and accept the new step dH. + * + * If the gain is below LM_GAIN_LO, damp more (increase L), even if the + * gain was positive. If the gain is above LM_GAIN_HI, damp less + * (decrease L). Otherwise the gain is left unchanged. */ - if(newS < S){ + if(gain < LM_GAIN_LO){ + L *= 8; + if(L>1000.0f/FLT_EPSILON){ + break;/* FIXME: Most naive termination criterion imaginable. */ + } + }else if(gain > LM_GAIN_HI){ + L *= 0.5; + } + + if(gain > 0){ S = newS; memcpy(p->best.H, newH, sizeof(newH)); sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, p->lm.JtJ, p->lm.Jte, &S); - /*printf("Error: %12.6f\n", S);*/ - L *= 0.5; - if(L1.0f/FLT_EPSILON){ - break; - } } } } @@ -1816,35 +1812,36 @@ static inline void sacCalcJacobianErrors(const float* restrict H, } /** - * Compute the derivative of the rate of change of the SSE. + * Compute the Levenberg-Marquardt "gain" obtained by the given step dH. * - * Inspired entirely by OpenCV's levmarq.cpp. To be reviewed. + * Drawn from http://www2.imm.dtu.dk/documents/ftp/tr99/tr05_99.pdf. */ -static inline float sacDs(const float (*JtJ)[8], - const float* dH, - const float* Jte){ - float tdH[8]; - int i, j; - float dS = 0; +static inline float sacLMGain(const float* dH, + const float* Jte, + const float S, + const float newS, + const float lambda){ + float dS = S-newS; + float dL = 0; + int i; - /* Perform tdH = -JtJ*dH + 2*Jte. */ + /* Compute h^t h... */ for(i=0;i<8;i++){ - tdH[i] = 0; - - for(j=0;j<8;j++){ - tdH[i] -= JtJ[i][j] * dH[j]; - } - - tdH[i] += 2*Jte[i]; + dL += dH[i]*dH[i]; } - - /* Perform dS = dH.dot(tdH). */ + /* Compute mu * h^t h... */ + dL *= lambda; + /* Subtract h^t F'... */ for(i=0;i<8;i++){ - dS += dH[i]*tdH[i]; + dL += dH[i]*Jte[i];/* += as opposed to -=, since dH we compute is + opposite sign. */ } + /* Multiply by 1/2... */ + dL *= 0.5; - return dS; + /* Return gain as S-newS / L0 - LH. */ + return fabs(dL) < FLT_EPSILON ? dS : dS / dL; } /** From 69b146412a01d64f68e73686ee7bc21da55485c7 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Feb 2015 22:55:40 -0500 Subject: [PATCH 007/171] Edited Doxygen documentation in the module calib3d. Added a mention within calib3d.hpp that the flag RHO is available as an option on calls to findHomography(). --- modules/calib3d/include/opencv2/calib3d.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 3b8e3c7c1..bfc5c5f7e 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -266,8 +266,9 @@ a vector\ . - **0** - a regular method using all the points - **RANSAC** - RANSAC-based robust method - **LMEDS** - Least-Median robust method +- **RHO** - PROSAC-based robust method @param ransacReprojThreshold Maximum allowed reprojection error to treat a point pair as an inlier -(used in the RANSAC method only). That is, if +(used in the RANSAC and RHO methods only). That is, if \f[\| \texttt{dstPoints} _i - \texttt{convertPointsHomogeneous} ( \texttt{H} * \texttt{srcPoints} _i) \| > \texttt{ransacReprojThreshold}\f] then the point \f$i\f$ is considered an outlier. If srcPoints and dstPoints are measured in pixels, it usually makes sense to set this parameter somewhere in the range of 1 to 10. @@ -290,7 +291,7 @@ pairs to compute an initial homography estimate with a simple least-squares sche However, if not all of the point pairs ( \f$srcPoints_i\f$, \f$dstPoints_i\f$ ) fit the rigid perspective transformation (that is, there are some outliers), this initial estimate will be poor. In this case, -you can use one of the two robust methods. Both methods, RANSAC and LMeDS , try many different +you can use one of the three robust methods. The methods RANSAC, LMeDS and RHO try many different random subsets of the corresponding point pairs (of four pairs each), estimate the homography matrix using this subset and a simple least-square algorithm, and then compute the quality/goodness of the computed homography (which is the number of inliers for RANSAC or the median re-projection error for @@ -301,7 +302,7 @@ Regardless of the method, robust or not, the computed homography matrix is refin inliers only in case of a robust method) with the Levenberg-Marquardt method to reduce the re-projection error even more. -The method RANSAC can handle practically any ratio of outliers but it needs a threshold to +The methods RANSAC and RHO can handle practically any ratio of outliers but need a threshold to distinguish inliers from outliers. The method LMeDS does not need any threshold but it works correctly only when there are more than 50% of inliers. Finally, if there are no outliers and the noise is rather small, use the default method (method=0). From ce0570b77744676c463ea2c8617d5f6a828950cf Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Feb 2015 23:33:52 -0500 Subject: [PATCH 008/171] Splitting vectorized code into separate branch. Deleted SSE code from master branch. Slight cleanups in fundam.cpp were made as a consequence. --- modules/calib3d/src/fundam.cpp | 123 +-- modules/calib3d/src/rhosse2.cpp | 1497 ------------------------------- modules/calib3d/src/rhosse2.h | 331 ------- 3 files changed, 69 insertions(+), 1882 deletions(-) delete mode 100644 modules/calib3d/src/rhosse2.cpp delete mode 100644 modules/calib3d/src/rhosse2.h diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 1fafc14ec..16e406057 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -42,9 +42,6 @@ #include "precomp.hpp" #include "rhorefc.h" -#if CV_SSE2 -#include "rhosse2.h" -#endif #include namespace cv @@ -279,65 +276,83 @@ 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; +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; + double beta = 0.35;/* 0.35 is a value that often works. */ - /* Run RHO. Needs cleanup or separate function to invoke. */ + /* Create temporary output matrix (RHO outputs a single-precision H only). */ Mat tmpH = Mat(3, 3, CV_32FC1); + + /* Create output mask. */ if(!tempMask.data){ 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,*/ - RHO_FLAG_ENABLE_NR | RHO_FLAG_ENABLE_FINAL_REFINEMENT, - NULL, - (float*)tmpH.data); - rhoRefCFini(&p); - } + + /** + * Make use of the RHO estimator API. + * + * This is where the math happens. A homography estimation context is + * initialized, used, then finalized. + */ + + RHO_HEST_REFC p; + rhoRefCInit(&p); + + /** + * Optional. Ideally, the context would survive across calls to + * findHomography(), but no clean way appears to exit to do so. The price + * to pay is marginally more computational work than strictly needed. + */ + + rhoRefCEnsureCapacity(&p, npoints, beta); + + /** + * The critical call. All parameters are heavily documented in rhorefc.h. + * + * Currently, NR (Non-Randomness criterion) and Final Refinement (with + * internal, optimized Levenberg-Marquardt method) are enabled. However, + * while refinement seems to correctly smooth jitter most of the time, when + * refinement fails it tends to make the estimate visually very much worse. + * It may be necessary to remove the refinement flags in a future commit if + * this behaviour is too problematic. + */ + + 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 | RHO_FLAG_ENABLE_FINAL_REFINEMENT, + NULL, + (float*)tmpH.data); + + /** + * Cleanup. + */ + + rhoRefCFini(&p); + + /* Convert float homography to double precision. */ tmpH.convertTo(_H, CV_64FC1); - /* Maps non-zero maks elems to 1, for the sake of the testcase. */ + /* Maps non-zero mask elems to 1, for the sake of the testcase. */ for(int k=0;k -#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 deleted file mode 100644 index ad3d2b18b..000000000 --- a/modules/calib3d/src/rhosse2.h +++ /dev/null @@ -1,331 +0,0 @@ -/* - 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 From 2609e77af791949db374dbeb3f337be62345aa0a Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Wed, 4 Feb 2015 02:26:01 -0500 Subject: [PATCH 009/171] Silence spurious loss-of-data warnings from Windows. Added explicit casts to silence warnings in fundam.cpp and rhorefc.cpp. --- modules/calib3d/src/fundam.cpp | 10 +++++----- modules/calib3d/src/rhorefc.cpp | 18 +++++++++--------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 16e406057..327670249 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -332,12 +332,12 @@ static bool createAndRunRHORegistrator(double confidence, (const float*)src.data, (const float*)dst.data, (char*) tempMask.data, - npoints, - ransacReprojThreshold, - maxIters, - maxIters, + (unsigned) npoints, + (float) ransacReprojThreshold, + (unsigned) maxIters, + (unsigned) maxIters, confidence, - 4, + 4U, beta, RHO_FLAG_ENABLE_NR | RHO_FLAG_ENABLE_FINAL_REFINEMENT, NULL, diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 40d125a53..6fc410d98 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -412,7 +412,7 @@ static inline void* almalloc(size_t 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; + adj[-1] = (unsigned char)(diff - 1); return adj; } } @@ -735,7 +735,7 @@ static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p){ p->ctrl.phNum++; next = (p->ctrl.phEndFpI * p->ctrl.phNum)/(p->ctrl.phNum - m); - p->ctrl.phEndI += ceil(next - p->ctrl.phEndFpI); + p->ctrl.phEndI += (unsigned)ceil(next - p->ctrl.phEndFpI); p->ctrl.phEndFpI = next; } @@ -913,7 +913,7 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ 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) */ + float reprojZ=H[6]*x+H[7]*y+1.0f; /* ( 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; @@ -929,7 +929,7 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ /* ... */ isInlier = reprojDist <= distSq; p->curr.numInl += isInlier; - *inl++ = isInlier; + *inl++ = (char)isInlier; /* SPRT */ @@ -1098,7 +1098,7 @@ static inline void sacInitNonRand(double beta, for(; n < N; n++){ double mu = n * beta; double sigma = sqrt(n)* beta_beta1_sq_chi; - unsigned i_min = ceil(m + mu + sigma); + unsigned i_min = (unsigned)ceil(m + mu + sigma); nonRandMinInl[n] = i_min; } @@ -1234,7 +1234,7 @@ static inline void sacRndSmpl(unsigned sampleSize, int inList; do{ - currentSample[i]=dataSetSize*sacRandom(); + currentSample[i] = (unsigned)(dataSetSize*sacRandom()); inList=0; for(j=0;j Date: Wed, 4 Feb 2015 12:12:14 -0500 Subject: [PATCH 010/171] Corrected typo in comment. The inverted JtJ does not multiply itself, but rather Jte. Correct this in the comment. --- modules/calib3d/src/rhorefc.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 6fc410d98..6d8924480 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -1583,7 +1583,7 @@ static inline void sacRefine(RHO_HEST_REFC* p){ int i; float S, newS; /* Sum of squared errors */ float gain; /* Gain-parameter. */ - float L = 0.01f;/* Lambda of LevMarq */ + float L = 100.0f;/* Lambda of LevMarq */ float dH[8], newH[8]; /** @@ -1614,7 +1614,7 @@ static inline void sacRefine(RHO_HEST_REFC* p){ * The system above is solved by Cholesky decomposition of a * sufficently-damped JtJ into a lower-triangular matrix (and its * transpose), whose inverse is then computed. This inverse (and its - * transpose) then multiply JtJ in order to find dH. + * transpose) then multiply Jte in order to find dH. */ while(!sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1)){ From 87c2b8197ac4b4af3d049577d5c0e3443fc33c28 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Thu, 5 Feb 2015 02:18:41 -0500 Subject: [PATCH 011/171] Bug fixes in mask output. Previously, the output mask of inliers could remain completely uninitialized. This fix is the first part of a solution. --- modules/calib3d/src/fundam.cpp | 7 ++----- modules/calib3d/src/rhorefc.cpp | 6 ++++++ 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 327670249..1366e9baa 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -286,7 +286,7 @@ static bool createAndRunRHORegistrator(double confidence, OutputArray _tempMask){ Mat src = _src.getMat(); Mat dst = _dst.getMat(); - Mat tempMask = _tempMask.getMat(); + Mat tempMask; bool result; double beta = 0.35;/* 0.35 is a value that often works. */ @@ -294,10 +294,7 @@ static bool createAndRunRHORegistrator(double confidence, Mat tmpH = Mat(3, 3, CV_32FC1); /* Create output mask. */ - if(!tempMask.data){ - tempMask = Mat(npoints, 1, CV_8U); - } - + tempMask = Mat(npoints, 1, CV_8U); /** * Make use of the RHO estimator API. diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 6d8924480..3cc9a8a1c 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -512,6 +512,9 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ return 0; } + memset(p->best.inl, 0, p->arg.N); + memset(p->curr.inl, 0, p->arg.N); + /** * LevMarq workspace alloc. * @@ -1155,6 +1158,9 @@ static inline void sacUpdateBounds(RHO_HEST_REFC* p){ static inline void sacOutputModel(RHO_HEST_REFC* p){ if(sacIsBestModelGoodEnough(p)){ memcpy(p->arg.finalH, p->best.H, HSIZE); + if(p->arg.inl != p->best.inl){ + memcpy(p->arg.inl, p->best.inl, p->arg.N); + } }else{ sacOutputZeroH(p); } From adac8c04bb8dcd07f01fe865b75156dea79419e9 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Sat, 7 Feb 2015 00:56:58 -0500 Subject: [PATCH 012/171] Converted to C++ style, + bugfixes. The code has been refactored in response to feedback on Pull Request Also, outputZeroH() now also zeroes the inlier set, much like outputModel(). --- modules/calib3d/src/fundam.cpp | 9 +- modules/calib3d/src/rhorefc.cpp | 1098 ++++++++++++++++++------------- modules/calib3d/src/rhorefc.h | 114 +--- 3 files changed, 662 insertions(+), 559 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 1366e9baa..554827583 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -303,8 +303,7 @@ static bool createAndRunRHORegistrator(double confidence, * initialized, used, then finalized. */ - RHO_HEST_REFC p; - rhoRefCInit(&p); + RHO_HEST_REFC* p = rhoRefCInit(); /** * Optional. Ideally, the context would survive across calls to @@ -312,7 +311,7 @@ static bool createAndRunRHORegistrator(double confidence, * to pay is marginally more computational work than strictly needed. */ - rhoRefCEnsureCapacity(&p, npoints, beta); + rhoRefCEnsureCapacity(p, npoints, beta); /** * The critical call. All parameters are heavily documented in rhorefc.h. @@ -325,7 +324,7 @@ static bool createAndRunRHORegistrator(double confidence, * this behaviour is too problematic. */ - result = !!rhoRefC(&p, + result = !!rhoRefC(p, (const float*)src.data, (const float*)dst.data, (char*) tempMask.data, @@ -344,7 +343,7 @@ static bool createAndRunRHORegistrator(double confidence, * Cleanup. */ - rhoRefCFini(&p); + rhoRefCFini(p); /* Convert float homography to double precision. */ tmpH.convertTo(_H, CV_64FC1); diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 3cc9a8a1c..167accd97 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -44,6 +44,7 @@ */ /* Includes */ +#include #include #include #include @@ -52,88 +53,203 @@ #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 100 -#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 */ -#define LM_GAIN_LO 0.25 /* See sacLMGain(). */ -#define LM_GAIN_HI 0.75 /* See sacLMGain(). */ +const int MEM_ALIGN = 32; +const size_t HSIZE = (3*3*sizeof(float)); +const double MIN_DELTA_CHNG = 0.1; +const double CHI_STAT = 2.706; +const double CHI_SQ = 1.645; +const double RLO = 0.25; +const double RHI = 0.75; +const int MAXLEVMARQITERS = 100; +const int SMPL_SIZE = 4; /* 4 points required per model */ +const int SPRT_T_M = 25; /* Guessing 25 match evlauations / 1 model generation */ +const int SPRT_M_S = 1; /* 1 model per sample */ +const double SPRT_EPSILON = 0.1; /* No explanation */ +const double SPRT_DELTA = 0.01; /* No explanation */ +const double LM_GAIN_LO = 0.25; /* See sacLMGain(). */ +const double LM_GAIN_HI = 0.75; /* See sacLMGain(). */ + + /* For the sake of cv:: namespace ONLY: */ -#ifdef __cplusplus namespace cv{/* For C support, replace with extern "C" { */ -#endif - /* Data Structures */ +struct RHO_HEST_REFC{ + /** + * Virtual Arguments. + * + * Exactly the same as at function call, except: + * - minInl is enforced to be >= 4. + */ + + struct{ + const float* src; + const float* dst; + char* inl; + unsigned N; + float maxD; + unsigned maxI; + unsigned rConvg; + double cfd; + unsigned minInl; + double beta; + unsigned flags; + const float* guessH; + float* finalH; + } 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* smpl; /* Sample of match indexes */ + } ctrl; + + /* Current model being tested */ + struct{ + float* pkdPts; /* Packed points */ + float* H; /* Homography */ + char* inl; /* Mask of inliers */ + unsigned numInl; /* Number of inliers */ + } curr; + + /* Best model (so far) */ + struct{ + float* H; /* Homography */ + char* inl; /* Mask of inliers */ + unsigned numInl; /* Number of inliers */ + } best; + + /* Non-randomness criterion */ + struct{ + std::vector 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 (* JtJ)[8]; /* JtJ matrix */ + float (* tmp1)[8]; /* Temporary 1 */ + float* Jte; /* Jte vector */ + } lm; + + /* Initialized? */ + int init; + + + /* Empty constructors and destructors */ + public: + RHO_HEST_REFC(); + private: /* Forbid copying. */ + RHO_HEST_REFC(const RHO_HEST_REFC&); + public: + ~RHO_HEST_REFC(); + + /* Methods to implement external interface */ + inline int initialize(void); + inline int sacEnsureCapacity(unsigned N, double beta); + inline void finalize(void); + unsigned rhoRefC(const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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. */ -/* Prototypes */ + /* Methods to implement internals */ + inline int initRun(void); + inline void finiRun(void); + inline int haveExtrinsicGuess(void); + inline int hypothesize(void); + inline int verify(void); + inline int isNREnabled(void); + inline int isRefineEnabled(void); + inline int isFinalRefineEnabled(void); + inline int PROSACPhaseEndReached(void); + inline void PROSACGoToNextPhase(void); + inline void getPROSACSample(void); + inline int isSampleDegenerate(void); + inline void generateModel(void); + inline int isModelDegenerate(void); + inline void evaluateModelSPRT(void); + inline void updateSPRT(void); + inline void designSPRTTest(void); + inline int isBestModel(void); + inline int isBestModelGoodEnough(void); + inline void saveBestModel(void); + inline void nStarOptimize(void); + inline void updateBounds(void); + inline void outputModel(void); + inline void outputZeroH(void); + inline int canRefine(void); + inline void refine(void); +}; + + + + +/** + * Prototypes for purely-computational code. + */ + static inline void* almalloc(size_t nBytes); -static inline void alfree(void* ptr); +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 sacInitNonRand (double beta, + unsigned start, + unsigned N, + unsigned* nonRandMinInl); +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 void sacCalcJacobianErrors(const float* restrict H, const float* restrict src, const float* restrict dst, @@ -142,164 +258,62 @@ static inline void sacCalcJacobianErrors(const float* restrict H, float (* restrict JtJ)[8], float* restrict Jte, float* restrict Sp); -static inline float sacLMGain(const float* dH, - const float* Jte, - const float S, - const float newS, - const float lambda); -static inline int sacChol8x8Damped (const float (*A)[8], - float lambda, - 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 sacSub8x1(float* Hout, const float* H, const float* dH); +static inline float sacLMGain (const float* dH, + const float* Jte, + const float S, + const float newS, + const float lambda); +static inline int sacChol8x8Damped (const float (*A)[8], + float lambda, + 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 sacSub8x1 (float* Hout, + const float* H, + const float* dH); /* Functions */ /** - * Initialize the estimator context, by allocating the aligned buffers - * internally needed. + * External access to context constructor. * - * 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. + * @return A pointer to the context if successful; NULL if an error occured. */ -int rhoRefCInit(RHO_HEST_REFC* p){ - memset(p, 0, sizeof(*p)); +RHO_HEST_REFC* rhoRefCInit(void){ + RHO_HEST_REFC* p = new RHO_HEST_REFC; - 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; - - p->lm.ws = NULL; - p->lm.JtJ = NULL; - p->lm.tmp1 = NULL; - p->lm.Jte = NULL; - - - int areAllAllocsSuccessful = p->ctrl.smpl && - p->curr.H && - p->best.H && - p->curr.pkdPts; - - if(!areAllAllocsSuccessful){ - rhoRefCFini(p); + if(!p->initialize()){ + delete p; + p = NULL; } - return areAllAllocsSuccessful; + return 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 1 if successful; 0 if an error occured. - * - * Reads: nr.* - * Writes: nr.* + * External access to non-randomness table resize. */ 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; + return p->sacEnsureCapacity(N, beta); } /** - * Finalize the estimator context, by freeing the aligned buffers used - * internally. + * External access to context destructor. * * @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)); + delete p; } @@ -332,76 +346,26 @@ void rhoRefCFini(RHO_HEST_REFC* p){ * 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; +unsigned rhoRefC(RHO_HEST_REFC* p, /* Homography estimation context. */ + const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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. */ + return p->rhoRefC(src, dst, inl, N, maxD, maxI, rConvg, cfd, minInl, beta, + flags, guessH, finalH); } + /** * Allocate memory aligned to a boundary of MEMALIGN. */ @@ -433,6 +397,241 @@ static inline void alfree(void* ptr){ } } +/** + * Constructor for RHO_HEST_REFC. + * + * Does nothing. True initialization is done by initialize(). + */ + +RHO_HEST_REFC::RHO_HEST_REFC() : init(0){ + +} + +/** + * Private copy constructor for RHO_HEST_REFC. Disabled. + */ + +RHO_HEST_REFC::RHO_HEST_REFC(const RHO_HEST_REFC&) : init(0){ + +} + +/** + * Destructor for RHO_HEST_REFC. + */ + +RHO_HEST_REFC::~RHO_HEST_REFC(){ + if(init){ + finalize(); + } +} + + + +/** + * 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 + * + * Returns 0 if unsuccessful and non-0 otherwise. + */ + +inline int RHO_HEST_REFC::initialize(void){ + ctrl.smpl = (unsigned*)almalloc(SMPL_SIZE*sizeof(*ctrl.smpl)); + + curr.pkdPts = (float*) almalloc(SMPL_SIZE*2*2*sizeof(*curr.pkdPts)); + curr.H = (float*) almalloc(HSIZE); + curr.inl = NULL; + curr.numInl = 0; + + best.H = (float*) almalloc(HSIZE); + best.inl = NULL; + best.numInl = 0; + + nr.size = 0; + nr.beta = 0.0; + + lm.ws = NULL; + lm.JtJ = NULL; + lm.tmp1 = NULL; + lm.Jte = NULL; + + + int areAllAllocsSuccessful = ctrl.smpl && + curr.H && + best.H && + curr.pkdPts; + + if(!areAllAllocsSuccessful){ + finalize(); + }else{ + init = 1; + } + + return areAllAllocsSuccessful; +} + +/** + * Finalize. + * + * Finalize the estimator context, by freeing the aligned buffers used + * internally. + */ + +inline void RHO_HEST_REFC::finalize(void){ + if(init){ + alfree(ctrl.smpl); + alfree(curr.H); + alfree(best.H); + alfree(curr.pkdPts); + + memset(this, 0, sizeof(*this)); + + init = 0; + } +} + +/** + * 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] 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.* + */ + +inline int RHO_HEST_REFC::sacEnsureCapacity(unsigned N, double beta){ + if(N == 0){ + /* Clear. */ + nr.tbl.clear(); + nr.size = 0; + }else if(nr.beta != beta){ + /* Beta changed. Redo all the work. */ + nr.tbl.resize(N); + nr.beta = beta; + sacInitNonRand(nr.beta, 0, N, &nr.tbl[0]); + nr.size = N; + }else if(N > nr.size){ + /* Work is partially done. Do rest of it. */ + nr.tbl.resize(N); + sacInitNonRand(nr.beta, nr.size, N, &nr.tbl[nr.size]); + nr.size = N; + }else{ + /* Work is already done. Do nothing. */ + } + + return 1; +} + + +/** + * Estimates the homography using the given context, matches and parameters to + * PROSAC. + * + * @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 RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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 + */ + + arg.src = src; + arg.dst = dst; + arg.inl = inl; + arg.N = N; + arg.maxD = maxD; + arg.maxI = maxI; + arg.rConvg = rConvg; + arg.cfd = cfd; + arg.minInl = minInl; + arg.beta = beta; + arg.flags = flags; + arg.guessH = guessH; + arg.finalH = finalH; + if(!initRun()){ + outputZeroH(); + finiRun(); + return 0; + } + + /** + * Extrinsic Guess + */ + + if(haveExtrinsicGuess()){ + verify(); + } + + + /** + * PROSAC Loop + */ + + for(ctrl.i=0; ctrl.i < arg.maxI; ctrl.i++){ + hypothesize() && verify(); + } + + + /** + * Teardown + */ + + if(isFinalRefineEnabled() && canRefine()){ + refine(); + } + + outputModel(); + finiRun(); + return isBestModelGoodEnough() ? best.numInl : 0; +} + /** * Initialize SAC for a run given its arguments. @@ -446,7 +645,7 @@ static inline void alfree(void* ptr){ * Writes: curr.*, best.*, ctrl.*, eval.* */ -static inline int sacInitRun(RHO_HEST_REFC* p){ +inline int RHO_HEST_REFC::initRun(void){ /** * Sanitize arguments. * @@ -454,29 +653,29 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ * mean something or other. */ - if(!p->arg.src || !p->arg.dst){ + if(!arg.src || !arg.dst){ /* Arguments src or dst are insane, must be != NULL */ return 0; } - if(p->arg.N < m){ + if(arg.N < SMPL_SIZE){ /* Argument N is insane, must be >= 4. */ return 0; } - if(p->arg.maxD < 0){ + if(arg.maxD < 0){ /* Argument maxD is insane, must be >= 0. */ return 0; } - if(p->arg.cfd < 0 || p->arg.cfd > 1){ + if(arg.cfd < 0 || 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)){ + arg.minInl = arg.minInl < SMPL_SIZE ? SMPL_SIZE : arg.minInl; + if(isNREnabled() && (arg.beta <= 0 || arg.beta >= 1)){ /* Argument beta is insane, must be in (0, 1). */ return 0; } - if(!p->arg.finalH){ + if(!arg.finalH){ /* Argument finalH is insane, must be != NULL */ return 0; } @@ -491,7 +690,7 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ * substruct and the sanity-checked N and beta arguments from above. */ - if(sacIsNREnabled(p) && !rhoRefCEnsureCapacity(p, p->arg.N, p->arg.beta)){ + if(isNREnabled() && !sacEnsureCapacity(arg.N, arg.beta)){ return 0; } @@ -505,15 +704,15 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ * 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); + best.inl = arg.inl ? arg.inl : (char*)almalloc(arg.N); + curr.inl = (char*)almalloc(arg.N); - if(!p->curr.inl || !p->best.inl){ + if(!curr.inl || !best.inl){ return 0; } - memset(p->best.inl, 0, p->arg.N); - memset(p->curr.inl, 0, p->arg.N); + memset(best.inl, 0, arg.N); + memset(curr.inl, 0, arg.N); /** * LevMarq workspace alloc. @@ -522,17 +721,17 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ * we wish to quit before doing any serious work. */ - if(sacIsRefineEnabled(p) || sacIsFinalRefineEnabled(p)){ - p->lm.ws = (float*)almalloc(2*8*8*sizeof(float) + 1*8*sizeof(float)); - if(!p->lm.ws){ + if(isRefineEnabled() || isFinalRefineEnabled()){ + lm.ws = (float*)almalloc(2*8*8*sizeof(float) + 1*8*sizeof(float)); + if(!lm.ws){ return 0; } - p->lm.JtJ = (float(*)[8])(p->lm.ws + 0*8*8); - p->lm.tmp1 = (float(*)[8])(p->lm.ws + 1*8*8); - p->lm.Jte = (float*) (p->lm.ws + 2*8*8); + lm.JtJ = (float(*)[8])(lm.ws + 0*8*8); + lm.tmp1 = (float(*)[8])(lm.ws + 1*8*8); + lm.Jte = (float*) (lm.ws + 2*8*8); }else{ - p->lm.ws = NULL; + lm.ws = NULL; } /** @@ -542,32 +741,32 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ * 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; + ctrl.i = 0; + ctrl.phNum = SMPL_SIZE; + ctrl.phEndI = 1; + ctrl.phEndFpI = sacInitPEndFpI(arg.rConvg, arg.N, SMPL_SIZE); + ctrl.phMax = arg.N; + ctrl.phNumInl = 0; + ctrl.numModels = 0; - if(sacHaveExtrinsicGuess(p)){ - memcpy(p->curr.H, p->arg.guessH, HSIZE); + if(haveExtrinsicGuess()){ + memcpy(curr.H, arg.guessH, HSIZE); }else{ - memset(p->curr.H, 0, HSIZE); + memset(curr.H, 0, HSIZE); } - p->curr.numInl = 0; + curr.numInl = 0; - memset(p->best.H, 0, HSIZE); - p->best.numInl = 0; + memset(best.H, 0, HSIZE); + 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); + eval.Ntested = 0; + eval.Ntestedtotal = 0; + eval.good = 1; + eval.t_M = SPRT_T_M; + eval.m_S = SPRT_M_S; + eval.epsilon = SPRT_EPSILON; + eval.delta = SPRT_DELTA; + designSPRTTest(); return 1; } @@ -583,33 +782,33 @@ static inline int sacInitRun(RHO_HEST_REFC* p){ * Writes: curr.inl, best.inl */ -static inline void sacFiniRun(RHO_HEST_REFC* p){ +inline void RHO_HEST_REFC::finiRun(void){ /** * 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); + if(arg.inl){ + if(arg.inl == best.inl){ + alfree(curr.inl); }else{ - alfree(p->best.inl); + alfree(best.inl); } }else{ - alfree(p->best.inl); - alfree(p->curr.inl); + alfree(best.inl); + alfree(curr.inl); } - p->best.inl = NULL; - p->curr.inl = NULL; + best.inl = NULL; + curr.inl = NULL; /** * ₣ree the Levenberg-Marquardt workspace. */ - alfree(p->lm.ws); - p->lm.ws = NULL; + alfree(lm.ws); + lm.ws = NULL; } /** @@ -622,18 +821,18 @@ static inline void sacFiniRun(RHO_HEST_REFC* p){ * non-zero otherwise. */ -static inline int sacHypothesize(RHO_HEST_REFC* p){ - if(sacPROSACPhaseEndReached(p)){ - sacPROSACGoToNextPhase(p); +inline int RHO_HEST_REFC::hypothesize(void){ + if(PROSACPhaseEndReached()){ + PROSACGoToNextPhase(); } - sacGetPROSACSample(p); - if(sacIsSampleDegenerate(p)){ + getPROSACSample(); + if(isSampleDegenerate()){ return 0; } - sacGenerateModel(p); - if(sacIsModelDegenerate(p)){ + generateModel(); + if(isModelDegenerate()){ return 0; } @@ -649,21 +848,21 @@ static inline int sacHypothesize(RHO_HEST_REFC* p){ * Returns 1. */ -static inline int sacVerify(RHO_HEST_REFC* p){ - sacEvaluateModelSPRT(p); - sacUpdateSPRT(p); +inline int RHO_HEST_REFC::verify(void){ + evaluateModelSPRT(); + updateSPRT(); - if(sacIsBestModel(p)){ - sacSaveBestModel(p); + if(isBestModel()){ + saveBestModel(); - if(sacIsRefineEnabled(p) && sacCanRefine(p)){ - sacRefine(p); + if(isRefineEnabled() && canRefine()){ + refine(); } - sacUpdateBounds(p); + updateBounds(); - if(sacIsNREnabled(p)){ - sacNStarOptimize(p); + if(isNREnabled()){ + nStarOptimize(); } } @@ -676,8 +875,8 @@ static inline int sacVerify(RHO_HEST_REFC* p){ * @return Zero if no extrinsic guess was provided; non-zero otherwiseEE. */ -static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p){ - return !!p->arg.guessH; +inline int RHO_HEST_REFC::haveExtrinsicGuess(void){ + return !!arg.guessH; } /** @@ -686,8 +885,8 @@ static inline int sacHaveExtrinsicGuess(RHO_HEST_REFC* p){ * @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; +inline int RHO_HEST_REFC::isNREnabled(void){ + return arg.flags & RHO_FLAG_ENABLE_NR; } /** @@ -696,8 +895,8 @@ static inline int sacIsNREnabled(RHO_HEST_REFC* p){ * @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; +inline int RHO_HEST_REFC::isRefineEnabled(void){ + return arg.flags & RHO_FLAG_ENABLE_REFINEMENT; } /** @@ -706,8 +905,8 @@ static inline int sacIsRefineEnabled(RHO_HEST_REFC* p){ * @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; +inline int RHO_HEST_REFC::isFinalRefineEnabled(void){ + return arg.flags & RHO_FLAG_ENABLE_FINAL_REFINEMENT; } /** @@ -718,8 +917,8 @@ static inline int sacIsFinalRefineEnabled(RHO_HEST_REFC* p){ * 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; +inline int RHO_HEST_REFC::PROSACPhaseEndReached(void){ + return ctrl.i >= ctrl.phEndI && ctrl.phNum < ctrl.phMax; } /** @@ -733,13 +932,13 @@ static inline int sacPROSACPhaseEndReached(RHO_HEST_REFC* p){ * Write: phNum, phEndFpI, phEndI */ -static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p){ +inline void RHO_HEST_REFC::PROSACGoToNextPhase(void){ double next; - p->ctrl.phNum++; - next = (p->ctrl.phEndFpI * p->ctrl.phNum)/(p->ctrl.phNum - m); - p->ctrl.phEndI += (unsigned)ceil(next - p->ctrl.phEndFpI); - p->ctrl.phEndFpI = next; + ctrl.phNum++; + next = (ctrl.phEndFpI * ctrl.phNum)/(ctrl.phNum - SMPL_SIZE); + ctrl.phEndI += (unsigned)ceil(next - ctrl.phEndFpI); + ctrl.phEndFpI = next; } /** @@ -750,12 +949,13 @@ static inline void sacPROSACGoToNextPhase(RHO_HEST_REFC* p){ * 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 */ +inline void RHO_HEST_REFC::getPROSACSample(void){ + if(ctrl.i > ctrl.phEndI){ + /* FIXME: Dubious. Review. */ + sacRndSmpl(4, ctrl.smpl, ctrl.phNum);/* Used to be phMax */ }else{ - sacRndSmpl(3, p->ctrl.smpl, p->ctrl.phNum-1); - p->ctrl.smpl[3] = p->ctrl.phNum-1; + sacRndSmpl(3, ctrl.smpl, ctrl.phNum-1); + ctrl.smpl[3] = ctrl.phNum-1; } } @@ -767,10 +967,10 @@ static inline void sacGetPROSACSample(RHO_HEST_REFC* p){ * 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]; +inline int RHO_HEST_REFC::isSampleDegenerate(void){ + unsigned i0 = ctrl.smpl[0], i1 = ctrl.smpl[1], i2 = ctrl.smpl[2], i3 = 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; + MyPt2f* pkdPts = (MyPt2f*)curr.pkdPts, *src = (MyPt2f*)arg.src, *dst = (MyPt2f*)arg.dst; /** * Pack the matches selected by the SAC algorithm. @@ -860,8 +1060,8 @@ static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p){ * current homography. */ -static inline void sacGenerateModel(RHO_HEST_REFC* p){ - hFuncRefC(p->curr.pkdPts, p->curr.H); +inline void RHO_HEST_REFC::generateModel(void){ + hFuncRefC(curr.pkdPts, curr.H); } /** @@ -870,9 +1070,9 @@ static inline void sacGenerateModel(RHO_HEST_REFC* p){ * NaN the homography is rejected. */ -static inline int sacIsModelDegenerate(RHO_HEST_REFC* p){ +inline int RHO_HEST_REFC::isModelDegenerate(void){ int degenerate; - float* H = p->curr.H; + float* H = curr.H; float f=H[0]+H[1]+H[2]+H[3]+H[4]+H[5]+H[6]+H[7]; /* degenerate = isnan(f); */ @@ -890,26 +1090,26 @@ static inline int sacIsModelDegenerate(RHO_HEST_REFC* p){ * Writes: eval.*, curr.inl, curr.numInl */ -static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ +inline void RHO_HEST_REFC::evaluateModelSPRT(void){ 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; + float distSq = arg.maxD*arg.maxD; + const float* src = arg.src; + const float* dst = arg.dst; + char* inl = curr.inl; + const float* H = curr.H; - p->ctrl.numModels++; + ctrl.numModels++; - p->curr.numInl = 0; - p->eval.Ntested = 0; - p->eval.good = 1; + curr.numInl = 0; + eval.Ntested = 0; + eval.good = 1; /* SCALAR */ - for(i=0;iarg.N && p->eval.good;i++){ + for(i=0;icurr.numInl += isInlier; + curr.numInl += isInlier; *inl++ = (char)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 */ + lambda *= isInlier ? eval.lambdaAccept : eval.lambdaReject; + eval.good = lambda <= eval.A; + /* If !good, the threshold A was exceeded, so we're rejecting */ } - p->eval.Ntested = i; - p->eval.Ntestedtotal += i; + eval.Ntested = i; + eval.Ntestedtotal += i; } /** @@ -959,18 +1159,21 @@ static inline void sacEvaluateModelSPRT(RHO_HEST_REFC* p){ * eval.lambdaReject */ -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); +inline void RHO_HEST_REFC::updateSPRT(void){ + if(eval.good){ + if(isBestModel()){ + eval.epsilon = (double)curr.numInl/arg.N; + designSPRTTest(); } }else{ - double newDelta = (double)p->curr.numInl/p->eval.Ntested; + double newDelta = (double)curr.numInl/eval.Ntested; - if(newDelta > 0 && CHNG_SIGNIFICANT(p->eval.delta, newDelta)){ - p->eval.delta = newDelta; - sacDesignSPRTTest(p); + if(newDelta > 0){ + double relChange = fabs(eval.delta - newDelta)/ eval.delta; + if(relChange > MIN_DELTA_CHNG){ + eval.delta = newDelta; + designSPRTTest(); + } } } } @@ -1044,18 +1247,18 @@ static inline double designSPRTTest(double delta, double epsilon, double t_M, do * 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 )); +inline void RHO_HEST_REFC::designSPRTTest(void){ + eval.A = designSPRTTest(eval.delta, eval.epsilon, eval.t_M, eval.m_S); + eval.lambdaReject = ((1.0 - eval.delta) / (1.0 - eval.epsilon)); + eval.lambdaAccept = (( eval.delta ) / ( 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; +inline int RHO_HEST_REFC::isBestModel(void){ + return curr.numInl > best.numInl; } /** @@ -1064,8 +1267,8 @@ static inline int sacIsBestModel(RHO_HEST_REFC* p){ * number of inliers. */ -static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p){ - return p->best.numInl >= p->arg.minInl; +inline int RHO_HEST_REFC::isBestModelGoodEnough(void){ + return best.numInl >= arg.minInl; } /** @@ -1073,18 +1276,18 @@ static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p){ * 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; +inline void RHO_HEST_REFC::saveBestModel(void){ + float* H = curr.H; + char* inl = curr.inl; + unsigned numInl = curr.numInl; - p->curr.H = p->best.H; - p->curr.inl = p->best.inl; - p->curr.numInl = p->best.numInl; + curr.H = best.H; + curr.inl = best.inl; + curr.numInl = best.numInl; - p->best.H = H; - p->best.inl = inl; - p->best.numInl = numInl; + best.H = H; + best.inl = inl; + best.numInl = numInl; } /** @@ -1095,13 +1298,13 @@ static inline void sacInitNonRand(double beta, unsigned start, unsigned N, unsigned* nonRandMinInl){ - unsigned n = m+1 > start ? m+1 : start; + unsigned n = SMPL_SIZE+1 > start ? SMPL_SIZE+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 = (unsigned)ceil(m + mu + sigma); + unsigned i_min = (unsigned)ceil(SMPL_SIZE + mu + sigma); nonRandMinInl[n] = i_min; } @@ -1112,31 +1315,31 @@ static inline void sacInitNonRand(double beta, * 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; +inline void RHO_HEST_REFC::nStarOptimize(void){ + unsigned min_sample_length = 10*2; /*(N * INLIERS_RATIO) */ + unsigned best_n = arg.N; unsigned test_n = best_n; - unsigned bestNumInl = p->best.numInl; + unsigned bestNumInl = 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]){ + if(testNumInl < nr.tbl[test_n]){ break; } best_n = test_n; bestNumInl = testNumInl; } - testNumInl -= !!p->arg.inl[test_n-1]; + testNumInl -= !!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); + if(bestNumInl*ctrl.phMax > ctrl.phNumInl*best_n){ + ctrl.phMax = best_n; + ctrl.phNumInl = bestNumInl; + arg.maxI = sacCalcIterBound(arg.cfd, + (double)ctrl.phNumInl/ctrl.phMax, + SMPL_SIZE, + arg.maxI); } } @@ -1144,25 +1347,25 @@ static inline void sacNStarOptimize(RHO_HEST_REFC* p){ * 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); +inline void RHO_HEST_REFC::updateBounds(void){ + arg.maxI = sacCalcIterBound(arg.cfd, + (double)best.numInl/arg.N, + SMPL_SIZE, + 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); - if(p->arg.inl != p->best.inl){ - memcpy(p->arg.inl, p->best.inl, p->arg.N); +inline void RHO_HEST_REFC::outputModel(void){ + if(isBestModelGoodEnough()){ + memcpy(arg.finalH, best.H, HSIZE); + if(arg.inl != best.inl){ + memcpy(arg.inl, best.inl, arg.N); } }else{ - sacOutputZeroH(p); + outputZeroH(); } } @@ -1170,8 +1373,9 @@ static inline void sacOutputModel(RHO_HEST_REFC* p){ * Ouput a zeroed H to the output argument. */ -static inline void sacOutputZeroH(RHO_HEST_REFC* p){ - memset(p->arg.finalH, 0, HSIZE); +inline void RHO_HEST_REFC::outputZeroH(void){ + memset(arg.finalH, 0, HSIZE); + memset(arg.inl, 0, arg.N); } /** @@ -1572,20 +1776,20 @@ static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates) * NB This is separate from whether it is *enabled*. */ -static inline int sacCanRefine(RHO_HEST_REFC* p){ +inline int RHO_HEST_REFC::canRefine(void){ /** * If we only have 4 matches, GE's result is already optimal and cannot * be refined any further. */ - return p->best.numInl > m; + return best.numInl > SMPL_SIZE; } /** * Refines the best-so-far homography (p->best.H). */ -static inline void sacRefine(RHO_HEST_REFC* p){ +inline void RHO_HEST_REFC::refine(void){ int i; float S, newS; /* Sum of squared errors */ float gain; /* Gain-parameter. */ @@ -1597,8 +1801,8 @@ static inline void sacRefine(RHO_HEST_REFC* p){ */ /* Find initial conditions */ - sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, - p->lm.JtJ, p->lm.Jte, &S); + sacCalcJacobianErrors(best.H, arg.src, arg.dst, arg.inl, arg.N, + lm.JtJ, lm.Jte, &S); /*Levenberg-Marquardt Loop.*/ for(i=0;ilm.JtJ, L, p->lm.tmp1)){ + while(!sacChol8x8Damped(lm.JtJ, L, lm.tmp1)){ L *= 2.0f; } - sacTRInv8x8 (p->lm.tmp1, p->lm.tmp1); - sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH); - sacSub8x1 (newH, p->best.H, dH); - sacCalcJacobianErrors(newH, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, + sacTRInv8x8 (lm.tmp1, lm.tmp1); + sacTRISolve8x8(lm.tmp1, lm.Jte, dH); + sacSub8x1 (newH, best.H, dH); + sacCalcJacobianErrors(newH, arg.src, arg.dst, arg.inl, arg.N, NULL, NULL, &newS); - gain = sacLMGain(dH, p->lm.Jte, S, newS, L); + gain = sacLMGain(dH, lm.Jte, S, newS, L); /*printf("Lambda: %12.6f S: %12.6f newS: %12.6f Gain: %12.6f\n", L, S, newS, gain);*/ @@ -1656,9 +1860,9 @@ static inline void sacRefine(RHO_HEST_REFC* p){ if(gain > 0){ S = newS; - memcpy(p->best.H, newH, sizeof(newH)); - sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, - p->lm.JtJ, p->lm.Jte, &S); + memcpy(best.H, newH, sizeof(newH)); + sacCalcJacobianErrors(best.H, arg.src, arg.dst, arg.inl, arg.N, + lm.JtJ, lm.Jte, &S); } } } @@ -2165,7 +2369,5 @@ static inline void sacSub8x1(float* Hout, const float* H, const float* dH){ } - -#ifdef __cplusplus +/* End namespace cv */ } -#endif diff --git a/modules/calib3d/src/rhorefc.h b/modules/calib3d/src/rhorefc.h index be7012f00..49bc58a86 100644 --- a/modules/calib3d/src/rhorefc.h +++ b/modules/calib3d/src/rhorefc.h @@ -56,7 +56,6 @@ /* Defines */ -#ifdef __cplusplus /* C++ does not have the restrict keyword. */ #ifdef restrict @@ -64,15 +63,6 @@ #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 @@ -90,101 +80,17 @@ +/* Namespace cv */ +namespace cv{ + /* 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 Jte; /* Jte vector */ - } lm; -} RHO_HEST_REFC; - - - -/* Extern C */ -#ifdef __cplusplus -namespace cv{ -/* extern "C" { */ -#endif - +struct RHO_HEST_REFC; +typedef struct RHO_HEST_REFC RHO_HEST_REFC; /* Functions */ @@ -193,11 +99,10 @@ namespace cv{ * 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. + * @return A pointer to the context if successful; NULL if an error occured. */ -int rhoRefCInit(RHO_HEST_REFC* p); +RHO_HEST_REFC* rhoRefCInit(void); /** @@ -355,11 +260,8 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte -/* Extern C */ -#ifdef __cplusplus -/* } *//* End extern "C" */ +/* End Namespace cv */ } -#endif From ccd33a721e6b45ab2c392293200d02edf7923a7b Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Sat, 7 Feb 2015 01:19:40 -0500 Subject: [PATCH 013/171] Fixed build failures related to designSPRTTest(). --- modules/calib3d/src/rhorefc.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 167accd97..03ec79ebe 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -1192,7 +1192,7 @@ inline void RHO_HEST_REFC::updateSPRT(void){ * 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){ +static inline double sacDesignSPRTTest(double delta, double epsilon, double t_M, double m_S){ double An, C, K, prevAn; unsigned i; @@ -1248,7 +1248,7 @@ static inline double designSPRTTest(double delta, double epsilon, double t_M, do */ inline void RHO_HEST_REFC::designSPRTTest(void){ - eval.A = designSPRTTest(eval.delta, eval.epsilon, eval.t_M, eval.m_S); + eval.A = sacDesignSPRTTest(eval.delta, eval.epsilon, eval.t_M, eval.m_S); eval.lambdaReject = ((1.0 - eval.delta) / (1.0 - eval.epsilon)); eval.lambdaAccept = (( eval.delta ) / ( eval.epsilon )); } From e178294b49ce36938fd275d3309db6110698b492 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Thu, 12 Feb 2015 15:23:28 +0100 Subject: [PATCH 014/171] Refactoring in preparation for 16-bit implementation of fastNlMeansDenoising --- modules/photo/src/denoising.cpp | 12 +- .../src/fast_nlmeans_denoising_invoker.hpp | 86 +++---- ...fast_nlmeans_denoising_invoker_commons.hpp | 218 ++++++++++-------- .../fast_nlmeans_multi_denoising_invoker.hpp | 93 ++++---- 4 files changed, 222 insertions(+), 187 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index a074ac136..724ea0eb0 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -65,17 +65,17 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, switch (src.type()) { case CV_8U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC2: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC3: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; default: @@ -175,19 +175,19 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds { case CV_8U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC2: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC3: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index b8f5a0392..2ad0189ef 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -50,7 +50,7 @@ using namespace cv; -template +template struct FastNlMeansDenoisingInvoker : public ParallelLoopBody { @@ -75,20 +75,20 @@ private: int template_window_half_size_; int search_window_half_size_; - int fixed_point_mult_; + IT fixed_point_mult_; int almost_template_window_size_sq_bin_shift_; - std::vector almost_dist2weight_; + std::vector almost_dist2weight_; void calcDistSumsForFirstElementInRow( - int i, Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const; + int i, Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const; void calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, - Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const; + Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const; }; inline int getNearestPowerOf2(int value) @@ -99,8 +99,8 @@ inline int getNearestPowerOf2(int value) return p; } -template -FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( +template +FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const Mat& src, Mat& dst, int template_window_size, int search_window_size, @@ -117,8 +117,8 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( border_size_ = search_window_half_size_ + template_window_half_size_; copyMakeBorder(src_, extended_src_, border_size_, border_size_, border_size_, border_size_, BORDER_DEFAULT); - const int max_estimate_sum_value = search_window_size_ * search_window_size_ * 255; - fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; + const IT max_estimate_sum_value = (IT)search_window_size_ * (IT)search_window_size_ * 255; + fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift @@ -127,7 +127,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq); double almost_dist2actual_dist_multiplier = ((double)(1 << almost_template_window_size_sq_bin_shift_)) / template_window_size_sq; - int max_dist = 255 * 255 * sizeof(T); + IT max_dist = 255 * 255 * sizeof(T); int almost_max_dist = (int)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight_.resize(almost_max_dist); @@ -135,7 +135,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - int weight = cvRound(fixed_point_mult_ * std::exp(-dist / (h * h * sizeof(T)))); + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * sizeof(T)))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -149,21 +149,21 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( dst_ = Mat::zeros(src_.size(), src_.type()); } -template -void FastNlMeansDenoisingInvoker::operator() (const Range& range) const +template +void FastNlMeansDenoisingInvoker::operator() (const Range& range) const { int row_from = range.start; int row_to = range.end - 1; // sums of cols anf rows for current pixel p - Array2d dist_sums(search_window_size_, search_window_size_); + Array2d dist_sums(search_window_size_, search_window_size_); // for lazy calc optimization (sum of cols for current pixel) - Array3d col_dist_sums(template_window_size_, search_window_size_, search_window_size_); + Array3d col_dist_sums(template_window_size_, search_window_size_, search_window_size_); int first_col_num = -1; // last elements of column sum (for each element in row) - Array3d up_col_dist_sums(src_.cols, search_window_size_, search_window_size_); + Array3d up_col_dist_sums(src_.cols, search_window_size_, search_window_size_); for (int i = row_from; i <= row_to; i++) { @@ -202,9 +202,9 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) const for (int y = 0; y < search_window_size; y++) { - int * dist_sums_row = dist_sums.row_ptr(y); - int * col_dist_sums_row = col_dist_sums.row_ptr(first_col_num, y); - int * up_col_dist_sums_row = up_col_dist_sums.row_ptr(j, y); + IT * dist_sums_row = dist_sums.row_ptr(y); + IT * col_dist_sums_row = col_dist_sums.row_ptr(first_col_num, y); + IT * up_col_dist_sums_row = up_col_dist_sums.row_ptr(j, y); const T * b_up_ptr = extended_src_.ptr(start_by - template_window_half_size_ - 1 + y); const T * b_down_ptr = extended_src_.ptr(start_by + template_window_half_size_ + y); @@ -215,7 +215,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) const dist_sums_row[x] -= col_dist_sums_row[x]; int bx = start_bx + x; - col_dist_sums_row[x] = up_col_dist_sums_row[x] + calcUpDownDist(a_up, a_down, b_up_ptr[bx], b_down_ptr[bx]); + col_dist_sums_row[x] = up_col_dist_sums_row[x] + calcUpDownDist(a_up, a_down, b_up_ptr[bx], b_down_ptr[bx]); dist_sums_row[x] += col_dist_sums_row[x]; up_col_dist_sums_row[x] = col_dist_sums_row[x]; @@ -227,39 +227,39 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) const } // calc weights - int estimation[3], weights_sum = 0; + IT estimation[3], weights_sum = 0; for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) estimation[channel_num] = 0; for (int y = 0; y < search_window_size_; y++) { const T* cur_row_ptr = extended_src_.ptr(border_size_ + search_window_y + y); - int* dist_sums_row = dist_sums.row_ptr(y); + IT* dist_sums_row = dist_sums.row_ptr(y); for (int x = 0; x < search_window_size_; x++) { - int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_; - int weight = almost_dist2weight_[almostAvgDist]; + int almostAvgDist = (int)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_); + IT weight = almost_dist2weight_[almostAvgDist]; weights_sum += weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; - incWithWeight(estimation, weight, p); + incWithWeight(estimation, weight, p); } } for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) - estimation[channel_num] = ((unsigned)estimation[channel_num] + weights_sum/2) / weights_sum; + estimation[channel_num] = (static_cast(estimation[channel_num]) + weights_sum/2) / weights_sum; - dst_.at(i,j) = saturateCastFromArray(estimation); + dst_.at(i,j) = saturateCastFromArray(estimation); } } } -template -inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( +template +inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( int i, - Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const + Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const { int j = 0; @@ -276,7 +276,7 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) for (int tx = -template_window_half_size_; tx <= template_window_half_size_; tx++) { - int dist = calcDist(extended_src_, + int dist = calcDist(extended_src_, border_size_ + i + ty, border_size_ + j + tx, border_size_ + start_y + ty, border_size_ + start_x + tx); @@ -288,12 +288,12 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( } } -template -inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( +template +inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, - Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const + Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const { int ay = border_size_ + i; int ax = border_size_ + j + template_window_half_size_; @@ -312,7 +312,7 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( int by = start_by + y; int bx = start_bx + x; for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) - col_dist_sums[new_last_col_num][y][x] += calcDist(extended_src_, ay + ty, ax, by + ty, bx); + col_dist_sums[new_last_col_num][y][x] += calcDist(extended_src_, ay + ty, ax, by + ty, bx); dist_sums[y][x] += col_dist_sums[new_last_col_num][y][x]; up_col_dist_sums[j][y][x] = col_dist_sums[new_last_col_num][y][x]; diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index ab7db5d2d..e4e0a3a59 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -44,118 +44,152 @@ using namespace cv; -template static inline int calcDist(const T a, const T b); - -template <> inline int calcDist(const uchar a, const uchar b) +template struct calcDist_ { - return (a-b) * (a-b); + static inline IT f(const T a, const T b); +}; + +template struct calcDist_ +{ + static inline IT f(uchar a, uchar b) + { + return (IT)(a-b) * (IT)(a-b); + } +}; + +template struct calcDist_ +{ + static inline IT f(const Vec2b a, const Vec2b b) + { + return (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]); + } +}; + +template struct calcDist_ +{ + static inline IT f(const Vec3b a, const Vec3b b) + { + return + (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]) + + (IT)(a[2]-b[2])*(IT)(a[2]-b[2]); + } +}; + +template static inline IT calcDist(const T a, const T b) +{ + return calcDist_::f(a, b); } -template <> inline int calcDist(const Vec2b a, const Vec2b b) -{ - return (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]); -} - -template <> inline int calcDist(const Vec3b a, const Vec3b b) -{ - return (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]); -} - -template static inline int calcDist(const Mat& m, int i1, int j1, int i2, int j2) +template +static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) { const T a = m.at(i1, j1); const T b = m.at(i2, j2); - return calcDist(a,b); + return calcDist(a,b); } -template static inline int calcUpDownDist(T a_up, T a_down, T b_up, T b_down) +template struct calcUpDownDist_ { - return calcDist(a_down, b_down) - calcDist(a_up, b_up); + static inline IT f(T a_up, T a_down, T b_up, T b_down) + { + return calcDist(a_down, b_down) - calcDist(a_up, b_up); + } +}; + +template struct calcUpDownDist_ +{ + static inline IT f(uchar a_up, uchar a_down, uchar b_up, uchar b_down) + { + IT A = a_down - b_down; + IT B = a_up - b_up; + return (A-B)*(A+B); + } +}; + +template +static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) +{ + return calcUpDownDist_::f(a_up, a_down, b_up, b_down); +}; + +template struct incWithWeight_ +{ + static inline void f(IT* estimation, IT weight, T p); +}; + +template struct incWithWeight_ +{ + static inline void f(IT* estimation, IT weight, uchar p) + { + estimation[0] += weight * p; + } +}; + +template struct incWithWeight_ +{ + static inline void f(IT* estimation, IT weight, Vec2b p) + { + estimation[0] += weight * p[0]; + estimation[1] += weight * p[1]; + } +}; + +template struct incWithWeight_ +{ + static inline void f(IT* estimation, IT weight, Vec3b p) + { + estimation[0] += weight * p[0]; + estimation[1] += weight * p[1]; + estimation[2] += weight * p[2]; + } +}; + +template +static inline void incWithWeight(IT* estimation, IT weight, T p) +{ + return incWithWeight_::f(estimation, weight, p); } -template <> inline int calcUpDownDist(uchar a_up, uchar a_down, uchar b_up, uchar b_down) +template struct saturateCastFromArray_ { - int A = a_down - b_down; - int B = a_up - b_up; - return (A-B)*(A+B); -} + static inline T f(IT* estimation); +}; -template static inline void incWithWeight(int* estimation, int weight, T p); - -template <> inline void incWithWeight(int* estimation, int weight, uchar p) +template struct saturateCastFromArray_ { - estimation[0] += weight * p; -} + static inline uchar f(IT* estimation) + { + return saturate_cast(estimation[0]); + } +}; -template <> inline void incWithWeight(int* estimation, int weight, Vec2b p) +template struct saturateCastFromArray_ { - estimation[0] += weight * p[0]; - estimation[1] += weight * p[1]; -} + static inline Vec2b f(IT* estimation) + { + Vec2b res; + res[0] = saturate_cast(estimation[0]); + res[1] = saturate_cast(estimation[1]); + return res; + } +}; -template <> inline void incWithWeight(int* estimation, int weight, Vec3b p) +template struct saturateCastFromArray_ { - estimation[0] += weight * p[0]; - estimation[1] += weight * p[1]; - estimation[2] += weight * p[2]; -} + static inline Vec3b f(IT* estimation) + { + Vec3b res; + res[0] = saturate_cast(estimation[0]); + res[1] = saturate_cast(estimation[1]); + res[2] = saturate_cast(estimation[2]); + return res; + } +}; -template <> inline void incWithWeight(int* estimation, int weight, int p) +template static inline T saturateCastFromArray(IT* estimation) { - estimation[0] += weight * p; -} - -template <> inline void incWithWeight(int* estimation, int weight, Vec2i p) -{ - estimation[0] += weight * p[0]; - estimation[1] += weight * p[1]; -} - -template <> inline void incWithWeight(int* estimation, int weight, Vec3i p) -{ - estimation[0] += weight * p[0]; - estimation[1] += weight * p[1]; - estimation[2] += weight * p[2]; -} - -template static inline T saturateCastFromArray(int* estimation); - -template <> inline uchar saturateCastFromArray(int* estimation) -{ - return saturate_cast(estimation[0]); -} - -template <> inline Vec2b saturateCastFromArray(int* estimation) -{ - Vec2b res; - res[0] = saturate_cast(estimation[0]); - res[1] = saturate_cast(estimation[1]); - return res; -} - -template <> inline Vec3b saturateCastFromArray(int* estimation) -{ - Vec3b res; - res[0] = saturate_cast(estimation[0]); - res[1] = saturate_cast(estimation[1]); - res[2] = saturate_cast(estimation[2]); - return res; -} - -template <> inline int saturateCastFromArray(int* estimation) -{ - return estimation[0]; -} - -template <> inline Vec2i saturateCastFromArray(int* estimation) -{ - estimation[1] = 0; - return Vec2i(estimation); -} - -template <> inline Vec3i saturateCastFromArray(int* estimation) -{ - return Vec3i(estimation); + return saturateCastFromArray_::f(estimation); } #endif diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index 191a67127..392733c08 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -50,7 +50,7 @@ using namespace cv; -template +template struct FastNlMeansMultiDenoisingInvoker : ParallelLoopBody { @@ -81,21 +81,21 @@ private: int search_window_half_size_; int temporal_window_half_size_; - int fixed_point_mult_; + IT fixed_point_mult_; int almost_template_window_size_sq_bin_shift; - std::vector almost_dist2weight; + std::vector almost_dist2weight; - void calcDistSumsForFirstElementInRow(int i, Array3d& dist_sums, - Array4d& col_dist_sums, - Array4d& up_col_dist_sums) const; + void calcDistSumsForFirstElementInRow(int i, Array3d& dist_sums, + Array4d& col_dist_sums, + Array4d& up_col_dist_sums) const; void calcDistSumsForElementInFirstRow(int i, int j, int first_col_num, - Array3d& dist_sums, Array4d& col_dist_sums, - Array4d& up_col_dist_sums) const; + Array3d& dist_sums, Array4d& col_dist_sums, + Array4d& up_col_dist_sums) const; }; -template -FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( +template +FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( const std::vector& srcImgs, int imgToDenoiseIndex, int temporalWindowSize, @@ -125,8 +125,9 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( border_size_, border_size_, border_size_, border_size_, cv::BORDER_DEFAULT); main_extended_src_ = extended_srcs_[temporal_window_half_size_]; - const int max_estimate_sum_value = temporal_window_size_ * search_window_size_ * search_window_size_ * 255; - fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; + const IT max_estimate_sum_value = + (IT)temporal_window_size_ * (IT)search_window_size_ * (IT)search_window_size_ * 255; + fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift @@ -138,7 +139,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( int almost_template_window_size_sq = 1 << almost_template_window_size_sq_bin_shift; double almost_dist2actual_dist_multiplier = (double) almost_template_window_size_sq / template_window_size_sq; - int max_dist = 255 * 255 * sizeof(T); + IT max_dist = 255 * 255 * sizeof(T); int almost_max_dist = (int) (max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight.resize(almost_max_dist); @@ -146,7 +147,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - int weight = cvRound(fixed_point_mult_ * std::exp(-dist / (h * h * sizeof(T)))); + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * sizeof(T)))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -160,19 +161,19 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( dst_ = Mat::zeros(srcImgs[0].size(), srcImgs[0].type()); } -template -void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const +template +void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const { int row_from = range.start; int row_to = range.end - 1; - Array3d dist_sums(temporal_window_size_, search_window_size_, search_window_size_); + Array3d dist_sums(temporal_window_size_, search_window_size_, search_window_size_); // for lazy calc optimization - Array4d col_dist_sums(template_window_size_, temporal_window_size_, search_window_size_, search_window_size_); + Array4d col_dist_sums(template_window_size_, temporal_window_size_, search_window_size_, search_window_size_); int first_col_num = -1; - Array4d up_col_dist_sums(cols_, temporal_window_size_, search_window_size_, search_window_size_); + Array4d up_col_dist_sums(cols_, temporal_window_size_, search_window_size_, search_window_size_); for (int i = row_from; i <= row_to; i++) { @@ -216,15 +217,15 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const for (int d = 0; d < temporal_window_size_; d++) { Mat cur_extended_src = extended_srcs_[d]; - Array2d cur_dist_sums = dist_sums[d]; - Array2d cur_col_dist_sums = col_dist_sums[first_col_num][d]; - Array2d cur_up_col_dist_sums = up_col_dist_sums[j][d]; + Array2d cur_dist_sums = dist_sums[d]; + Array2d cur_col_dist_sums = col_dist_sums[first_col_num][d]; + Array2d cur_up_col_dist_sums = up_col_dist_sums[j][d]; for (int y = 0; y < search_window_size; y++) { - int* dist_sums_row = cur_dist_sums.row_ptr(y); + IT* dist_sums_row = cur_dist_sums.row_ptr(y); - int* col_dist_sums_row = cur_col_dist_sums.row_ptr(y); - int* up_col_dist_sums_row = cur_up_col_dist_sums.row_ptr(y); + IT* col_dist_sums_row = cur_col_dist_sums.row_ptr(y); + IT* up_col_dist_sums_row = cur_up_col_dist_sums.row_ptr(y); const T* b_up_ptr = cur_extended_src.ptr(start_by - template_window_half_size_ - 1 + y); const T* b_down_ptr = cur_extended_src.ptr(start_by + template_window_half_size_ + y); @@ -234,7 +235,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const dist_sums_row[x] -= col_dist_sums_row[x]; col_dist_sums_row[x] = up_col_dist_sums_row[x] + - calcUpDownDist(a_up, a_down, b_up_ptr[start_bx + x], b_down_ptr[start_bx + x]); + calcUpDownDist(a_up, a_down, b_up_ptr[start_bx + x], b_down_ptr[start_bx + x]); dist_sums_row[x] += col_dist_sums_row[x]; up_col_dist_sums_row[x] = col_dist_sums_row[x]; @@ -247,9 +248,9 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const } // calc weights - int weights_sum = 0; + IT weights_sum = 0; - int estimation[3]; + IT estimation[3]; for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) estimation[channel_num] = 0; @@ -260,33 +261,33 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const { const T* cur_row_ptr = esrc_d.ptr(border_size_ + search_window_y + y); - int* dist_sums_row = dist_sums.row_ptr(d, y); + IT* dist_sums_row = dist_sums.row_ptr(d, y); for (int x = 0; x < search_window_size_; x++) { - int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift; + int almostAvgDist = (int)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift); - int weight = almost_dist2weight[almostAvgDist]; + IT weight = almost_dist2weight[almostAvgDist]; weights_sum += weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; - incWithWeight(estimation, weight, p); + incWithWeight(estimation, weight, p); } } } for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) - estimation[channel_num] = ((unsigned)estimation[channel_num] + weights_sum / 2) / weights_sum; + estimation[channel_num] = (static_cast(estimation[channel_num]) + weights_sum / 2) / weights_sum; // ???? - dst_.at(i,j) = saturateCastFromArray(estimation); + dst_.at(i,j) = saturateCastFromArray(estimation); } } } -template -inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRow( - int i, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const +template +inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRow( + int i, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { int j = 0; @@ -303,14 +304,14 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRo int start_y = i + y - search_window_half_size_; int start_x = j + x - search_window_half_size_; - int* dist_sums_ptr = &dist_sums[d][y][x]; - int* col_dist_sums_ptr = &col_dist_sums[0][d][y][x]; + IT* dist_sums_ptr = &dist_sums[d][y][x]; + IT* col_dist_sums_ptr = &col_dist_sums[0][d][y][x]; int col_dist_sums_step = col_dist_sums.step_size(0); for (int tx = -template_window_half_size_; tx <= template_window_half_size_; tx++) { for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) { - int dist = calcDist( + IT dist = calcDist( main_extended_src_.at(border_size_ + i + ty, border_size_ + j + tx), cur_extended_src.at(border_size_ + start_y + ty, border_size_ + start_x + tx)); @@ -325,10 +326,10 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRo } } -template -inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRow( - int i, int j, int first_col_num, Array3d& dist_sums, - Array4d& col_dist_sums, Array4d& up_col_dist_sums) const +template +inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRow( + int i, int j, int first_col_num, Array3d& dist_sums, + Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { int ay = border_size_ + i; int ax = border_size_ + j + template_window_half_size_; @@ -350,10 +351,10 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRo int by = start_by + y; int bx = start_bx + x; - int* col_dist_sums_ptr = &col_dist_sums[new_last_col_num][d][y][x]; + IT* col_dist_sums_ptr = &col_dist_sums[new_last_col_num][d][y][x]; for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) { - *col_dist_sums_ptr += calcDist( + *col_dist_sums_ptr += calcDist( main_extended_src_.at(ay + ty, ax), cur_extended_src.at(by + ty, bx)); } From 8368fb9ea8dc03a5d09a3d701858ee272a9c818a Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Thu, 12 Feb 2015 18:45:09 +0100 Subject: [PATCH 015/171] Additional refactoring preparing for 16-bit implementation --- .../src/fast_nlmeans_denoising_invoker.hpp | 12 +- ...fast_nlmeans_denoising_invoker_commons.hpp | 113 +++++++++++------- .../fast_nlmeans_multi_denoising_invoker.hpp | 9 +- 3 files changed, 83 insertions(+), 51 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 2ad0189ef..202e36013 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -107,7 +107,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const float h) : src_(src), dst_(dst) { - CV_Assert(src.channels() == sizeof(T)); //T is Vec1b or Vec2b or Vec3b + CV_Assert(src.channels() == pixelInfo::channels); template_window_half_size_ = template_window_size / 2; search_window_half_size_ = search_window_size / 2; @@ -117,17 +117,21 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( border_size_ = search_window_half_size_ + template_window_half_size_; copyMakeBorder(src_, extended_src_, border_size_, border_size_, border_size_, border_size_, BORDER_DEFAULT); - const IT max_estimate_sum_value = (IT)search_window_size_ * (IT)search_window_size_ * 255; + const IT max_estimate_sum_value = + (IT)search_window_size_ * (IT)search_window_size_ * (IT)pixelInfo::sampleMax(); fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift + // squared distances are truncated to 16 bits to get a reasonable table size CV_Assert(template_window_size_ <= 46340); // sqrt(INT_MAX) int template_window_size_sq = template_window_size_ * template_window_size_; - almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq); + almost_template_window_size_sq_bin_shift_ = + getNearestPowerOf2(template_window_size_sq) + 2*pixelInfo::sampleBits() - 16; double almost_dist2actual_dist_multiplier = ((double)(1 << almost_template_window_size_sq_bin_shift_)) / template_window_size_sq; - IT max_dist = 255 * 255 * sizeof(T); + IT max_dist = + (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; int almost_max_dist = (int)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight_.resize(almost_max_dist); diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index e4e0a3a59..0a8713b91 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -44,30 +44,62 @@ using namespace cv; -template struct calcDist_ +template struct pixelInfo_ { - static inline IT f(const T a, const T b); + static const int channels = 1; + typedef T sampleType; }; -template struct calcDist_ +template struct pixelInfo_ > { - static inline IT f(uchar a, uchar b) + static const int channels = n; + typedef ET sampleType; +}; + +template struct pixelInfo: public pixelInfo_ +{ + using typename pixelInfo_::sampleType; + + static inline sampleType sampleMax() + { + return std::numeric_limits::max(); + } + + static inline sampleType sampleMin() + { + return std::numeric_limits::min(); + } + + static inline size_t sampleBytes() + { + return sizeof(sampleType); + } + + static inline size_t sampleBits() + { + return 8*sampleBytes(); + } +}; + +template struct calcDist_ +{ + static inline IT f(const T a, const T b) { return (IT)(a-b) * (IT)(a-b); } }; -template struct calcDist_ +template struct calcDist_, IT> { - static inline IT f(const Vec2b a, const Vec2b b) + static inline IT f(const Vec a, const Vec b) { return (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]); } }; -template struct calcDist_ +template struct calcDist_, IT> { - static inline IT f(const Vec3b a, const Vec3b b) + static inline IT f(const Vec a, const Vec b) { return (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + @@ -92,14 +124,6 @@ static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) template struct calcUpDownDist_ { static inline IT f(T a_up, T a_down, T b_up, T b_down) - { - return calcDist(a_down, b_down) - calcDist(a_up, b_up); - } -}; - -template struct calcUpDownDist_ -{ - static inline IT f(uchar a_up, uchar a_down, uchar b_up, uchar b_down) { IT A = a_down - b_down; IT B = a_up - b_up; @@ -107,6 +131,17 @@ template struct calcUpDownDist_ } }; +template struct calcUpDownDist_, IT> +{ +private: + typedef Vec T; +public: + static inline IT f(T a_up, T a_down, T b_up, T b_down) + { + return calcDist(a_down, b_down) - calcDist(a_up, b_up); + } +}; + template static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) { @@ -115,29 +150,24 @@ static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) template struct incWithWeight_ { - static inline void f(IT* estimation, IT weight, T p); -}; - -template struct incWithWeight_ -{ - static inline void f(IT* estimation, IT weight, uchar p) + static inline void f(IT* estimation, IT weight, T p) { estimation[0] += weight * p; } }; -template struct incWithWeight_ +template struct incWithWeight_, IT> { - static inline void f(IT* estimation, IT weight, Vec2b p) + static inline void f(IT* estimation, IT weight, Vec p) { estimation[0] += weight * p[0]; estimation[1] += weight * p[1]; } }; -template struct incWithWeight_ +template struct incWithWeight_, IT> { - static inline void f(IT* estimation, IT weight, Vec3b p) + static inline void f(IT* estimation, IT weight, Vec p) { estimation[0] += weight * p[0]; estimation[1] += weight * p[1]; @@ -153,36 +183,31 @@ static inline void incWithWeight(IT* estimation, IT weight, T p) template struct saturateCastFromArray_ { - static inline T f(IT* estimation); -}; - -template struct saturateCastFromArray_ -{ - static inline uchar f(IT* estimation) + static inline T f(IT* estimation) { - return saturate_cast(estimation[0]); + return saturate_cast(estimation[0]); } }; -template struct saturateCastFromArray_ +template struct saturateCastFromArray_, IT> { - static inline Vec2b f(IT* estimation) + static inline Vec f(IT* estimation) { - Vec2b res; - res[0] = saturate_cast(estimation[0]); - res[1] = saturate_cast(estimation[1]); + Vec res; + res[0] = saturate_cast(estimation[0]); + res[1] = saturate_cast(estimation[1]); return res; } }; -template struct saturateCastFromArray_ +template struct saturateCastFromArray_, IT> { - static inline Vec3b f(IT* estimation) + static inline Vec f(IT* estimation) { - Vec3b res; - res[0] = saturate_cast(estimation[0]); - res[1] = saturate_cast(estimation[1]); - res[2] = saturate_cast(estimation[2]); + Vec res; + res[0] = saturate_cast(estimation[0]); + res[1] = saturate_cast(estimation[1]); + res[2] = saturate_cast(estimation[2]); return res; } }; diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index 392733c08..48276b426 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -106,7 +106,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( dst_(dst), extended_srcs_(srcImgs.size()) { CV_Assert(srcImgs.size() > 0); - CV_Assert(srcImgs[0].channels() == sizeof(T)); + CV_Assert(srcImgs[0].channels() == pixelInfo::channels); rows_ = srcImgs[0].rows; cols_ = srcImgs[0].cols; @@ -126,20 +126,23 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( main_extended_src_ = extended_srcs_[temporal_window_half_size_]; const IT max_estimate_sum_value = - (IT)temporal_window_size_ * (IT)search_window_size_ * (IT)search_window_size_ * 255; + (IT)temporal_window_size_ * (IT)search_window_size_ * (IT)search_window_size_ * (IT)pixelInfo::sampleMax(); fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift + // squared distances are truncated to 16 bits to get a reasonable table size int template_window_size_sq = template_window_size_ * template_window_size_; almost_template_window_size_sq_bin_shift = 0; while (1 << almost_template_window_size_sq_bin_shift < template_window_size_sq) almost_template_window_size_sq_bin_shift++; + almost_template_window_size_sq_bin_shift += 2*pixelInfo::sampleBits() - 16; int almost_template_window_size_sq = 1 << almost_template_window_size_sq_bin_shift; double almost_dist2actual_dist_multiplier = (double) almost_template_window_size_sq / template_window_size_sq; - IT max_dist = 255 * 255 * sizeof(T); + IT max_dist = + (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; int almost_max_dist = (int) (max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight.resize(almost_max_dist); From ff2509af56bb694c07edd80eb102e5e1edff41c3 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Thu, 12 Feb 2015 14:42:37 -0500 Subject: [PATCH 016/171] Fixed printouts in testcase to blame the correct method for a failure. Previously, certain test failures by the method RHO would result in an error blaming RANSAC instead. The fix involves a parameter change to several functions in test_homography.cpp. --- modules/calib3d/test/test_homography.cpp | 34 ++++++++++++------------ 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/modules/calib3d/test/test_homography.cpp b/modules/calib3d/test/test_homography.cpp index 1199cd6cd..a31b75d2b 100644 --- a/modules/calib3d/test/test_homography.cpp +++ b/modules/calib3d/test/test_homography.cpp @@ -94,12 +94,12 @@ private: void print_information_1(int j, int N, int method, const Mat& H); void print_information_2(int j, int N, int method, const Mat& H, const Mat& H_res, int k, double diff); - void print_information_3(int j, int N, const Mat& mask); + void print_information_3(int method, int j, int N, const Mat& mask); void print_information_4(int method, int j, int N, int k, int l, double diff); void print_information_5(int method, int j, int N, int l, double diff); - void print_information_6(int j, int N, int k, double diff, bool value); - void print_information_7(int j, int N, int k, double diff, bool original_value, bool found_value); - void print_information_8(int j, int N, int k, int l, double diff); + void print_information_6(int method, int j, int N, int k, double diff, bool value); + void print_information_7(int method, int j, int N, int k, double diff, bool original_value, bool found_value); + void print_information_8(int method, int j, int N, int k, int l, double diff); }; CV_HomographyTest::CV_HomographyTest() : max_diff(1e-2f), max_2diff(2e-2f) @@ -166,13 +166,13 @@ void CV_HomographyTest::print_information_2(int j, int N, int _method, const Mat cout << "Maximum allowed difference: " << max_diff << endl; cout << endl; } -void CV_HomographyTest::print_information_3(int j, int N, const Mat& mask) +void CV_HomographyTest::print_information_3(int _method, int j, int N, const Mat& mask) { cout << endl; cout << "Checking for inliers/outliers mask..." << endl; cout << endl; 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: RANSAC" << endl; + cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl; cout << "Found mask:" << endl; cout << endl; cout << mask << endl; cout << endl; cout << "Number of rows: " << mask.rows << " Number of cols: " << mask.cols << endl; cout << endl; @@ -205,10 +205,10 @@ void CV_HomographyTest::print_information_5(int _method, int j, int N, int l, do cout << "Maxumum allowed difference: " << max_diff << endl; cout << endl; } -void CV_HomographyTest::print_information_6(int j, int N, int k, double diff, bool value) +void CV_HomographyTest::print_information_6(int _method, int j, int N, int k, double diff, bool value) { cout << endl; cout << "Checking for inliers/outliers mask..." << endl; cout << endl; - cout << "Method: RANSAC" << endl; + cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl; 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; @@ -218,10 +218,10 @@ void CV_HomographyTest::print_information_6(int j, int N, int k, double diff, bo cout << "Value of found mask: "<< value << endl; cout << endl; } -void CV_HomographyTest::print_information_7(int j, int N, int k, double diff, bool original_value, bool found_value) +void CV_HomographyTest::print_information_7(int _method, int j, int N, int k, double diff, bool original_value, bool found_value) { cout << endl; cout << "Checking for inliers/outliers mask..." << endl; cout << endl; - cout << "Method: RANSAC" << endl; + cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl; 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; @@ -231,10 +231,10 @@ void CV_HomographyTest::print_information_7(int j, int N, int k, double diff, bo cout << "Value of original mask: "<< original_value << " Value of found mask: " << found_value << endl; cout << endl; } -void CV_HomographyTest::print_information_8(int j, int N, int k, int l, double diff) +void CV_HomographyTest::print_information_8(int _method, int j, int N, int k, int l, double diff) { cout << endl; cout << "Checking for reprojection error of inlier..." << endl; cout << endl; - cout << "Method: RANSAC" << endl; + cout << "Method: "; if (_method == RANSAC) cout << "RANSAC" << endl; else if (_method == cv::RHO) cout << "RHO" << endl; else cout << _method << endl; cout << "Sigma of normal noise: " << sigma << endl; 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; @@ -371,7 +371,7 @@ void CV_HomographyTest::run(int) if (code) { - print_information_3(j, N, mask[j]); + print_information_3(method, j, N, mask[j]); switch (code) { @@ -490,7 +490,7 @@ void CV_HomographyTest::run(int) if (code) { - print_information_3(j, N, mask_res[j]); + print_information_3(method, j, N, mask_res[j]); switch (code) { @@ -522,14 +522,14 @@ void CV_HomographyTest::run(int) if (mask_res[j].at(k, 0) != (diff <= reproj_threshold)) { - print_information_6(j, N, k, diff, mask_res[j].at(k, 0)); + print_information_6(method, j, N, k, diff, mask_res[j].at(k, 0)); CV_Error(CALIB3D_HOMOGRAPHY_ERROR_RANSAC_MASK, MESSAGE_RANSAC_MASK_4); return; } if (mask.at(k, 0) && !mask_res[j].at(k, 0)) { - print_information_7(j, N, k, diff, mask.at(k, 0), mask_res[j].at(k, 0)); + print_information_7(method, j, N, k, diff, mask.at(k, 0), mask_res[j].at(k, 0)); CV_Error(CALIB3D_HOMOGRAPHY_ERROR_RANSAC_MASK, MESSAGE_RANSAC_MASK_5); return; } @@ -549,7 +549,7 @@ void CV_HomographyTest::run(int) if (diff - cv::norm(noise_2d, NORM_TYPE[l]) > max_2diff) { - print_information_8(j, N, k, l, diff - cv::norm(noise_2d, NORM_TYPE[l])); + print_information_8(method, j, N, k, l, diff - cv::norm(noise_2d, NORM_TYPE[l])); CV_Error(CALIB3D_HOMOGRAPHY_ERROR_RANSAC_DIFF, MESSAGE_RANSAC_DIFF); return; } From 49e93747b17cae65915c66b326e37a94ddc53190 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Thu, 12 Feb 2015 22:05:05 +0100 Subject: [PATCH 017/171] Added saturate_cast from int64 and uint64 --- modules/core/include/opencv2/core/base.hpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/modules/core/include/opencv2/core/base.hpp b/modules/core/include/opencv2/core/base.hpp index f2acaa3fb..73beb911f 100644 --- a/modules/core/include/opencv2/core/base.hpp +++ b/modules/core/include/opencv2/core/base.hpp @@ -442,6 +442,10 @@ template static inline _Tp saturate_cast(int v) { return _Tp( template static inline _Tp saturate_cast(float v) { return _Tp(v); } /** @overload */ template static inline _Tp saturate_cast(double v) { return _Tp(v); } +/** @overload */ +template static inline _Tp saturate_cast(int64 v) { return _Tp(v); } +/** @overload */ +template static inline _Tp saturate_cast(uint64 v) { return _Tp(v); } //! @cond IGNORED @@ -452,6 +456,8 @@ template<> inline uchar saturate_cast(short v) { return saturate_c template<> inline uchar saturate_cast(unsigned v) { return (uchar)std::min(v, (unsigned)UCHAR_MAX); } template<> inline uchar saturate_cast(float v) { int iv = cvRound(v); return saturate_cast(iv); } template<> inline uchar saturate_cast(double v) { int iv = cvRound(v); return saturate_cast(iv); } +template<> inline uchar saturate_cast(int64 v) { return (uchar)((uint64)v <= (uint64)UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0); } +template<> inline uchar saturate_cast(uint64 v) { return (uchar)std::min(v, (uint64)UCHAR_MAX); } template<> inline schar saturate_cast(uchar v) { return (schar)std::min((int)v, SCHAR_MAX); } template<> inline schar saturate_cast(ushort v) { return (schar)std::min((unsigned)v, (unsigned)SCHAR_MAX); } @@ -460,6 +466,8 @@ template<> inline schar saturate_cast(short v) { return saturate_c template<> inline schar saturate_cast(unsigned v) { return (schar)std::min(v, (unsigned)SCHAR_MAX); } template<> inline schar saturate_cast(float v) { int iv = cvRound(v); return saturate_cast(iv); } template<> inline schar saturate_cast(double v) { int iv = cvRound(v); return saturate_cast(iv); } +template<> inline schar saturate_cast(int64 v) { return (schar)((uint64)((int64)v-SCHAR_MIN) <= (uint64)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN); } +template<> inline schar saturate_cast(uint64 v) { return (schar)std::min(v, (uint64)SCHAR_MAX); } template<> inline ushort saturate_cast(schar v) { return (ushort)std::max((int)v, 0); } template<> inline ushort saturate_cast(short v) { return (ushort)std::max((int)v, 0); } @@ -467,12 +475,16 @@ template<> inline ushort saturate_cast(int v) { return (ushort)(( template<> inline ushort saturate_cast(unsigned v) { return (ushort)std::min(v, (unsigned)USHRT_MAX); } template<> inline ushort saturate_cast(float v) { int iv = cvRound(v); return saturate_cast(iv); } template<> inline ushort saturate_cast(double v) { int iv = cvRound(v); return saturate_cast(iv); } +template<> inline ushort saturate_cast(int64 v) { return (ushort)((uint64)v <= (uint64)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0); } +template<> inline ushort saturate_cast(uint64 v) { return (ushort)std::min(v, (uint64)USHRT_MAX); } template<> inline short saturate_cast(ushort v) { return (short)std::min((int)v, SHRT_MAX); } template<> inline short saturate_cast(int v) { return (short)((unsigned)(v - SHRT_MIN) <= (unsigned)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN); } template<> inline short saturate_cast(unsigned v) { return (short)std::min(v, (unsigned)SHRT_MAX); } template<> inline short saturate_cast(float v) { int iv = cvRound(v); return saturate_cast(iv); } template<> inline short saturate_cast(double v) { int iv = cvRound(v); return saturate_cast(iv); } +template<> inline short saturate_cast(int64 v) { return (short)((uint64)((int64)v - SHRT_MIN) <= (uint64)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN); } +template<> inline short saturate_cast(uint64 v) { return (short)std::min(v, (uint64)SHRT_MAX); } template<> inline int saturate_cast(float v) { return cvRound(v); } template<> inline int saturate_cast(double v) { return cvRound(v); } From 42db9e7153a6d10b429df0bc2108278251c11ebc Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Thu, 12 Feb 2015 22:14:01 +0100 Subject: [PATCH 018/171] Basic 16-bit implmentation of fastNlMeansDenoising. Table-based exponetiation leads to high memory footprint and loss of precision in 16-bit mode. --- modules/photo/src/denoising.cpp | 43 ++++++++++++++++--- .../src/fast_nlmeans_denoising_invoker.hpp | 14 +++--- .../fast_nlmeans_multi_denoising_invoker.hpp | 15 ++++--- 3 files changed, 55 insertions(+), 17 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 724ea0eb0..0abeefe5b 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -65,17 +65,32 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, switch (src.type()) { case CV_8U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC2: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC3: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16U: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC2: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64>( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC3: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64>( src, dst, templateWindowSize, searchWindowSize, h)); break; default: @@ -181,13 +196,31 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds break; case CV_8UC2: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC3: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16U: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC2: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC3: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 202e36013..27a016ae9 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -123,11 +123,13 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift - // squared distances are truncated to 16 bits to get a reasonable table size + // squared distances are truncated to 24 bits to avoid unreasonable table sizes + // TODO: uses lots of memory and loses precision wtih 16-bit images ???? + const size_t TABLE_MAX_BITS = 24; CV_Assert(template_window_size_ <= 46340); // sqrt(INT_MAX) int template_window_size_sq = template_window_size_ * template_window_size_; - almost_template_window_size_sq_bin_shift_ = - getNearestPowerOf2(template_window_size_sq) + 2*pixelInfo::sampleBits() - 16; + almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq) + + std::max(2*pixelInfo::sampleBits(), TABLE_MAX_BITS) - TABLE_MAX_BITS; double almost_dist2actual_dist_multiplier = ((double)(1 << almost_template_window_size_sq_bin_shift_)) / template_window_size_sq; IT max_dist = @@ -139,7 +141,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * sizeof(T)))); + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * pixelInfo::channels))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -232,7 +234,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co // calc weights IT estimation[3], weights_sum = 0; - for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) + for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) estimation[channel_num] = 0; for (int y = 0; y < search_window_size_; y++) @@ -250,7 +252,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co } } - for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) + for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) estimation[channel_num] = (static_cast(estimation[channel_num]) + weights_sum/2) / weights_sum; dst_.at(i,j) = saturateCastFromArray(estimation); diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index 48276b426..c90249b82 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -131,12 +131,15 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift - // squared distances are truncated to 16 bits to get a reasonable table size + // squared distances are truncated to 24 bits to avoid unreasonable table sizes + // TODO: uses lots of memory and loses precision wtih 16-bit images ???? + const size_t TABLE_MAX_BITS = 24; int template_window_size_sq = template_window_size_ * template_window_size_; almost_template_window_size_sq_bin_shift = 0; while (1 << almost_template_window_size_sq_bin_shift < template_window_size_sq) almost_template_window_size_sq_bin_shift++; - almost_template_window_size_sq_bin_shift += 2*pixelInfo::sampleBits() - 16; + almost_template_window_size_sq_bin_shift += + std::max(2*pixelInfo::sampleBits(), TABLE_MAX_BITS) - TABLE_MAX_BITS; int almost_template_window_size_sq = 1 << almost_template_window_size_sq_bin_shift; double almost_dist2actual_dist_multiplier = (double) almost_template_window_size_sq / template_window_size_sq; @@ -150,7 +153,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * sizeof(T)))); + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * pixelInfo::channels))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -254,7 +257,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang IT weights_sum = 0; IT estimation[3]; - for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) + for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) estimation[channel_num] = 0; for (int d = 0; d < temporal_window_size_; d++) @@ -279,8 +282,8 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang } } - for (size_t channel_num = 0; channel_num < sizeof(T); channel_num++) - estimation[channel_num] = (static_cast(estimation[channel_num]) + weights_sum / 2) / weights_sum; // ???? + for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) + estimation[channel_num] = (static_cast(estimation[channel_num]) + weights_sum / 2) / weights_sum; dst_.at(i,j) = saturateCastFromArray(estimation); From d588c717da1ad2b77e03b058a281da3c00ba0327 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 13 Feb 2015 00:11:30 +0100 Subject: [PATCH 019/171] Using WEIGHT_THRESHOLD to limit table size. Still problematic with 16-bit and big h-values. --- .../src/fast_nlmeans_denoising_invoker.hpp | 30 +++++++++---------- .../fast_nlmeans_multi_denoising_invoker.hpp | 29 +++++++++--------- 2 files changed, 29 insertions(+), 30 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 27a016ae9..c9689cabd 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -123,31 +123,28 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift - // squared distances are truncated to 24 bits to avoid unreasonable table sizes - // TODO: uses lots of memory and loses precision wtih 16-bit images ???? - const size_t TABLE_MAX_BITS = 24; CV_Assert(template_window_size_ <= 46340); // sqrt(INT_MAX) int template_window_size_sq = template_window_size_ * template_window_size_; - almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq) + - std::max(2*pixelInfo::sampleBits(), TABLE_MAX_BITS) - TABLE_MAX_BITS; + almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq); double almost_dist2actual_dist_multiplier = ((double)(1 << almost_template_window_size_sq_bin_shift_)) / template_window_size_sq; + const double WEIGHT_THRESHOLD = 0.001; + const size_t ALLOC_CHUNK = 65536; IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; - int almost_max_dist = (int)(max_dist / almost_dist2actual_dist_multiplier + 1); - almost_dist2weight_.resize(almost_max_dist); - - const double WEIGHT_THRESHOLD = 0.001; - for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) + int almost_max_dist = 0; + while (true) { - double dist = almost_dist * almost_dist2actual_dist_multiplier; + double dist = almost_max_dist * almost_dist2actual_dist_multiplier; IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * pixelInfo::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult_ || dist > max_dist) break; - if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) - weight = 0; + if (almost_max_dist >= almost_dist2weight_.size()) + almost_dist2weight_.resize(almost_max_dist + ALLOC_CHUNK); - almost_dist2weight_[almost_dist] = weight; + almost_dist2weight_[almost_max_dist++] = weight; } + almost_dist2weight_.resize(almost_max_dist); CV_Assert(almost_dist2weight_[0] == fixed_point_mult_); // additional optimization init end @@ -161,6 +158,8 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co int row_from = range.start; int row_to = range.end - 1; + int almost_max_dist = almost_dist2weight_.size(); + // sums of cols anf rows for current pixel p Array2d dist_sums(search_window_size_, search_window_size_); @@ -244,7 +243,8 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co for (int x = 0; x < search_window_size_; x++) { int almostAvgDist = (int)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_); - IT weight = almost_dist2weight_[almostAvgDist]; + IT weight = + almostAvgDist < almost_max_dist ? almost_dist2weight_[almostAvgDist] : 0; weights_sum += weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index c90249b82..b4bfc0c6c 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -131,35 +131,31 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift - // squared distances are truncated to 24 bits to avoid unreasonable table sizes - // TODO: uses lots of memory and loses precision wtih 16-bit images ???? - const size_t TABLE_MAX_BITS = 24; int template_window_size_sq = template_window_size_ * template_window_size_; almost_template_window_size_sq_bin_shift = 0; while (1 << almost_template_window_size_sq_bin_shift < template_window_size_sq) almost_template_window_size_sq_bin_shift++; - almost_template_window_size_sq_bin_shift += - std::max(2*pixelInfo::sampleBits(), TABLE_MAX_BITS) - TABLE_MAX_BITS; int almost_template_window_size_sq = 1 << almost_template_window_size_sq_bin_shift; double almost_dist2actual_dist_multiplier = (double) almost_template_window_size_sq / template_window_size_sq; + const double WEIGHT_THRESHOLD = 0.001; + const size_t ALLOC_CHUNK = 65536; IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; - int almost_max_dist = (int) (max_dist / almost_dist2actual_dist_multiplier + 1); - almost_dist2weight.resize(almost_max_dist); - - const double WEIGHT_THRESHOLD = 0.001; - for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) + int almost_max_dist = 0; + while (true) { - double dist = almost_dist * almost_dist2actual_dist_multiplier; + double dist = almost_max_dist * almost_dist2actual_dist_multiplier; IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * pixelInfo::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult_ || dist > max_dist) break; - if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) - weight = 0; + if (almost_max_dist >= almost_dist2weight.size()) + almost_dist2weight.resize(almost_max_dist + ALLOC_CHUNK); - almost_dist2weight[almost_dist] = weight; + almost_dist2weight[almost_max_dist++] = weight; } + almost_dist2weight.resize(almost_max_dist); CV_Assert(almost_dist2weight[0] == fixed_point_mult_); // additional optimization init end @@ -173,6 +169,8 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang int row_from = range.start; int row_to = range.end - 1; + int almost_max_dist = almost_dist2weight.size(); + Array3d dist_sums(temporal_window_size_, search_window_size_, search_window_size_); // for lazy calc optimization @@ -273,7 +271,8 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang { int almostAvgDist = (int)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift); - IT weight = almost_dist2weight[almostAvgDist]; + IT weight = + almostAvgDist < almost_max_dist ? almost_dist2weight[almostAvgDist] : 0; weights_sum += weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; From 6dbf13d7b5e017a8b16cf5f12a38000896e64583 Mon Sep 17 00:00:00 2001 From: ASUS Date: Thu, 12 Feb 2015 21:22:52 -0500 Subject: [PATCH 020/171] saveBestModel() is modified. accuracy test is passed. --- modules/calib3d/src/fundam.cpp | 13 +++++---- modules/calib3d/src/rhorefc.cpp | 47 +++++++++++++++++++++++++-------- 2 files changed, 44 insertions(+), 16 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 1366e9baa..2f6058c74 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -399,16 +399,18 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, { tempMask = Mat::ones(npoints, 1, CV_8U); result = cb->runKernel(src, dst, H) > 0; - } - else if( method == RANSAC ) + } + + else if( method == RANSAC) result = createRANSACPointSetRegistrator(cb, 4, ransacReprojThreshold, confidence, maxIters)->run(src, dst, H, tempMask); else if( method == LMEDS ) result = createLMeDSPointSetRegistrator(cb, 4, confidence, maxIters)->run(src, dst, H, tempMask); - else if( method == RHO ){ + else if( method == RHO ) result = createAndRunRHORegistrator(confidence, maxIters, ransacReprojThreshold, npoints, src, dst, H, tempMask); - }else + else CV_Error(Error::StsBadArg, "Unknown estimation method"); + if( result && npoints > 4 && method != RHO) { compressPoints( src.ptr(), tempMask.ptr(), 1, npoints ); @@ -419,8 +421,9 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, Mat dst1 = dst.rowRange(0, npoints); src = src1; dst = dst1; - if( method == RANSAC || method == LMEDS ) + if( method == RANSAC || method == LMEDS) cb->runKernel( src, dst, H ); + Mat H8(8, 1, CV_64F, H.ptr()); createLMSolver(makePtr(src, dst), 10)->run(H8); } diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 3cc9a8a1c..976f8d341 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -55,7 +55,6 @@ #include "rhorefc.h" - /* Defines */ #define MEM_ALIGN 32 #define HSIZE (3*3*sizeof(float)) @@ -383,7 +382,7 @@ unsigned rhoRefC(RHO_HEST_REFC* restrict p, /* Homography estimation conte * PROSAC Loop */ - for(p->ctrl.i=0; p->ctrl.i < p->arg.maxI; p->ctrl.i++){ + for(p->ctrl.i=0; p->ctrl.i < p->arg.maxI || p->ctrl.i<100; p->ctrl.i++){ sacHypothesize(p) && sacVerify(p); } @@ -861,7 +860,28 @@ static inline int sacIsSampleDegenerate(RHO_HEST_REFC* p){ */ static inline void sacGenerateModel(RHO_HEST_REFC* p){ +#if 1 hFuncRefC(p->curr.pkdPts, p->curr.H); +#else + int mm = 4; + cv::Mat _H(3,3,CV_32FC1); + std::vector _srcPoints(mm,cv::Point2f()); + std::vector _dstPoints(mm,cv::Point2f()); + for (int i=0 ; i< mm ; i++){ + cv::Point2f tempPoint2f(p->curr.pkdPts[2*i], p->curr.pkdPts[2*i+1]); + _srcPoints[i] = tempPoint2f; + tempPoint2f=cv::Point2f((float)p->curr.pkdPts[2*i+8],(float) p->curr.pkdPts[2*i+9]); + _dstPoints[i] = tempPoint2f; + } + + _H = cv::findHomography(_srcPoints, _dstPoints,0,3); + double* data = (double*) _H.data; + + p->curr.H[0]=data[0]; p->curr.H[1]=data[1]; p->curr.H[2]=data[2]; + p->curr.H[3]=data[3]; p->curr.H[4]=data[4]; p->curr.H[5]=data[5]; + p->curr.H[6]=data[6]; p->curr.H[7]=data[7]; p->curr.H[8]=1.0; + +#endif } /** @@ -1074,17 +1094,22 @@ static inline int sacIsBestModelGoodEnough(RHO_HEST_REFC* p){ */ 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; + memcpy(p->best.H, p->curr.H, HSIZE); + memcpy(p->best.inl, p->curr.inl, p->arg.N); + p->best.numInl = p->curr.numInl; - p->best.H = H; - p->best.inl = inl; - p->best.numInl = numInl; +// 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; } /** From 584372bbf297c386ce71357d70b65068551b9466 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 13 Feb 2015 04:33:29 +0100 Subject: [PATCH 021/171] Fixed bounds checking --- modules/photo/src/fast_nlmeans_denoising_invoker.hpp | 6 +++--- modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index c9689cabd..2de50a77b 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -132,7 +132,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const size_t ALLOC_CHUNK = 65536; IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; - int almost_max_dist = 0; + size_t almost_max_dist = 0; while (true) { double dist = almost_max_dist * almost_dist2actual_dist_multiplier; @@ -158,7 +158,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co int row_from = range.start; int row_to = range.end - 1; - int almost_max_dist = almost_dist2weight_.size(); + size_t almost_max_dist = almost_dist2weight_.size(); // sums of cols anf rows for current pixel p Array2d dist_sums(search_window_size_, search_window_size_); @@ -242,7 +242,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co IT* dist_sums_row = dist_sums.row_ptr(y); for (int x = 0; x < search_window_size_; x++) { - int almostAvgDist = (int)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_); + size_t almostAvgDist = (size_t)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_); IT weight = almostAvgDist < almost_max_dist ? almost_dist2weight_[almostAvgDist] : 0; weights_sum += weight; diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index b4bfc0c6c..e0f06c68f 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -143,7 +143,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( const size_t ALLOC_CHUNK = 65536; IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; - int almost_max_dist = 0; + size_t almost_max_dist = 0; while (true) { double dist = almost_max_dist * almost_dist2actual_dist_multiplier; @@ -169,7 +169,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang int row_from = range.start; int row_to = range.end - 1; - int almost_max_dist = almost_dist2weight.size(); + size_t almost_max_dist = almost_dist2weight.size(); Array3d dist_sums(temporal_window_size_, search_window_size_, search_window_size_); @@ -269,7 +269,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang for (int x = 0; x < search_window_size_; x++) { - int almostAvgDist = (int)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift); + size_t almostAvgDist = (size_t)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift); IT weight = almostAvgDist < almost_max_dist ? almost_dist2weight[almostAvgDist] : 0; From 9a555063e86747323cd10fd64e39102991ab0cd8 Mon Sep 17 00:00:00 2001 From: ASUS Date: Thu, 12 Feb 2015 23:34:48 -0500 Subject: [PATCH 022/171] Fix sacCalcJacobianErrors arguments. (curr.inl replaced with best.inl) Fix the issue given NULL inlMask --- modules/calib3d/src/rhorefc.cpp | 78 ++++++--------------------------- 1 file changed, 14 insertions(+), 64 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 00802e602..272995cee 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -56,7 +56,6 @@ #include #include "rhorefc.h" - /* Defines */ const int MEM_ALIGN = 32; const size_t HSIZE = (3*3*sizeof(float)); @@ -613,15 +612,10 @@ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ * PROSAC Loop */ -<<<<<<< HEAD - for(p->ctrl.i=0; p->ctrl.i < p->arg.maxI || p->ctrl.i<100; p->ctrl.i++){ - sacHypothesize(p) && sacVerify(p); -======= - for(ctrl.i=0; ctrl.i < arg.maxI; ctrl.i++){ + for(ctrl.i=0; ctrl.i < arg.maxI || ctrl.i < 100; ctrl.i++){ hypothesize() && verify(); ->>>>>>> ff2509af56bb694c07edd80eb102e5e1edff41c3 - } + } /** * Teardown @@ -631,9 +625,10 @@ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ refine(); } - outputModel(); - finiRun(); + outputModel(); + finiRun(); return isBestModelGoodEnough() ? best.numInl : 0; + } @@ -661,7 +656,7 @@ inline int RHO_HEST_REFC::initRun(void){ /* Arguments src or dst are insane, must be != NULL */ return 0; } - if(arg.N < SMPL_SIZE){ + if(arg.N < (unsigned)SMPL_SIZE){ /* Argument N is insane, must be >= 4. */ return 0; } @@ -674,7 +669,7 @@ inline int RHO_HEST_REFC::initRun(void){ return 0; } /* Clamp minInl to 4 or higher. */ - arg.minInl = arg.minInl < SMPL_SIZE ? SMPL_SIZE : arg.minInl; + arg.minInl = arg.minInl < (unsigned)SMPL_SIZE ? SMPL_SIZE : arg.minInl; if(isNREnabled() && (arg.beta <= 0 || arg.beta >= 1)){ /* Argument beta is insane, must be in (0, 1). */ return 0; @@ -868,6 +863,7 @@ inline int RHO_HEST_REFC::verify(void){ if(isNREnabled()){ nStarOptimize(); } + } return 1; @@ -1064,34 +1060,8 @@ inline int RHO_HEST_REFC::isSampleDegenerate(void){ * current homography. */ -<<<<<<< HEAD -static inline void sacGenerateModel(RHO_HEST_REFC* p){ -#if 1 - hFuncRefC(p->curr.pkdPts, p->curr.H); -#else - int mm = 4; - cv::Mat _H(3,3,CV_32FC1); - std::vector _srcPoints(mm,cv::Point2f()); - std::vector _dstPoints(mm,cv::Point2f()); - for (int i=0 ; i< mm ; i++){ - cv::Point2f tempPoint2f(p->curr.pkdPts[2*i], p->curr.pkdPts[2*i+1]); - _srcPoints[i] = tempPoint2f; - tempPoint2f=cv::Point2f((float)p->curr.pkdPts[2*i+8],(float) p->curr.pkdPts[2*i+9]); - _dstPoints[i] = tempPoint2f; - } - - _H = cv::findHomography(_srcPoints, _dstPoints,0,3); - double* data = (double*) _H.data; - - p->curr.H[0]=data[0]; p->curr.H[1]=data[1]; p->curr.H[2]=data[2]; - p->curr.H[3]=data[3]; p->curr.H[4]=data[4]; p->curr.H[5]=data[5]; - p->curr.H[6]=data[6]; p->curr.H[7]=data[7]; p->curr.H[8]=1.0; - -#endif -======= inline void RHO_HEST_REFC::generateModel(void){ hFuncRefC(curr.pkdPts, curr.H); ->>>>>>> ff2509af56bb694c07edd80eb102e5e1edff41c3 } /** @@ -1306,25 +1276,6 @@ inline int RHO_HEST_REFC::isBestModelGoodEnough(void){ * and count of inliers between the current and best models. */ -<<<<<<< HEAD -static inline void sacSaveBestModel(RHO_HEST_REFC* p){ - - memcpy(p->best.H, p->curr.H, HSIZE); - memcpy(p->best.inl, p->curr.inl, p->arg.N); - p->best.numInl = p->curr.numInl; - -// 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; -======= inline void RHO_HEST_REFC::saveBestModel(void){ float* H = curr.H; char* inl = curr.inl; @@ -1337,7 +1288,6 @@ inline void RHO_HEST_REFC::saveBestModel(void){ best.H = H; best.inl = inl; best.numInl = numInl; ->>>>>>> ff2509af56bb694c07edd80eb102e5e1edff41c3 } /** @@ -1398,7 +1348,7 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ */ inline void RHO_HEST_REFC::updateBounds(void){ - arg.maxI = sacCalcIterBound(arg.cfd, + arg.maxI = sacCalcIterBound(arg.cfd, (double)best.numInl/arg.N, SMPL_SIZE, arg.maxI); @@ -1411,7 +1361,7 @@ inline void RHO_HEST_REFC::updateBounds(void){ inline void RHO_HEST_REFC::outputModel(void){ if(isBestModelGoodEnough()){ memcpy(arg.finalH, best.H, HSIZE); - if(arg.inl != best.inl){ + if(arg.inl && arg.inl != best.inl){ memcpy(arg.inl, best.inl, arg.N); } }else{ @@ -1832,7 +1782,7 @@ inline int RHO_HEST_REFC::canRefine(void){ * be refined any further. */ - return best.numInl > SMPL_SIZE; + return best.numInl > (unsigned)SMPL_SIZE; } /** @@ -1851,7 +1801,7 @@ inline void RHO_HEST_REFC::refine(void){ */ /* Find initial conditions */ - sacCalcJacobianErrors(best.H, arg.src, arg.dst, arg.inl, arg.N, + sacCalcJacobianErrors(best.H, arg.src, arg.dst, best.inl, arg.N, lm.JtJ, lm.Jte, &S); /*Levenberg-Marquardt Loop.*/ @@ -1883,7 +1833,7 @@ inline void RHO_HEST_REFC::refine(void){ sacTRInv8x8 (lm.tmp1, lm.tmp1); sacTRISolve8x8(lm.tmp1, lm.Jte, dH); sacSub8x1 (newH, best.H, dH); - sacCalcJacobianErrors(newH, arg.src, arg.dst, arg.inl, arg.N, + sacCalcJacobianErrors(newH, arg.src, arg.dst, best.inl, arg.N, NULL, NULL, &newS); gain = sacLMGain(dH, lm.Jte, S, newS, L); /*printf("Lambda: %12.6f S: %12.6f newS: %12.6f Gain: %12.6f\n", @@ -1911,7 +1861,7 @@ inline void RHO_HEST_REFC::refine(void){ if(gain > 0){ S = newS; memcpy(best.H, newH, sizeof(newH)); - sacCalcJacobianErrors(best.H, arg.src, arg.dst, arg.inl, arg.N, + sacCalcJacobianErrors(best.H, arg.src, arg.dst, best.inl, arg.N, lm.JtJ, lm.Jte, &S); } } From 42176f8eb1145abd4b046769e9ad1b7abda9d22c Mon Sep 17 00:00:00 2001 From: Hamid Bazargani Date: Fri, 13 Feb 2015 00:09:37 -0500 Subject: [PATCH 023/171] Fix sacCalcJacobianErrors arguments. (curr.inl replaced with best.inl) Fix the issue given NULL inlMask --- modules/calib3d/src/rhorefc.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 272995cee..bc36e0bd8 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -1799,7 +1799,6 @@ inline void RHO_HEST_REFC::refine(void){ /** * Iteratively refine the homography. */ - /* Find initial conditions */ sacCalcJacobianErrors(best.H, arg.src, arg.dst, best.inl, arg.N, lm.JtJ, lm.Jte, &S); From e22678018b084e8c0e17ece43114894aaea8fae8 Mon Sep 17 00:00:00 2001 From: Hamid Bazargani Date: Fri, 13 Feb 2015 00:30:18 -0500 Subject: [PATCH 024/171] Fix sacCalcJacobianErrors arguments. (curr.inl replaced with best.inl) Fix the issue given NULL inlMask --- modules/calib3d/src/fundam.cpp | 1 - modules/calib3d/src/rhorefc.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index beba572c9..af7c4dfb0 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -399,7 +399,6 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, tempMask = Mat::ones(npoints, 1, CV_8U); result = cb->runKernel(src, dst, H) > 0; } - else if( method == RANSAC) result = createRANSACPointSetRegistrator(cb, 4, ransacReprojThreshold, confidence, maxIters)->run(src, dst, H, tempMask); else if( method == LMEDS ) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index bc36e0bd8..2fffadac8 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -624,11 +624,9 @@ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ if(isFinalRefineEnabled() && canRefine()){ refine(); } - outputModel(); finiRun(); return isBestModelGoodEnough() ? best.numInl : 0; - } From 44f906eb356f1de71517a82904d897186dbf6e96 Mon Sep 17 00:00:00 2001 From: Hamid Bazargani Date: Fri, 13 Feb 2015 00:38:19 -0500 Subject: [PATCH 025/171] Fix sacCalcJacobianErrors arguments. (curr.inl replaced with best.inl) Fix the issue given NULL inlMask --- modules/calib3d/src/fundam.cpp | 2 +- modules/calib3d/src/rhorefc.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index af7c4dfb0..584ba5af1 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -398,7 +398,7 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, { tempMask = Mat::ones(npoints, 1, CV_8U); result = cb->runKernel(src, dst, H) > 0; - } + } else if( method == RANSAC) result = createRANSACPointSetRegistrator(cb, 4, ransacReprojThreshold, confidence, maxIters)->run(src, dst, H, tempMask); else if( method == LMEDS ) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 2fffadac8..8a7b57fea 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -624,8 +624,8 @@ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ if(isFinalRefineEnabled() && canRefine()){ refine(); } - outputModel(); - finiRun(); + outputModel(); + finiRun(); return isBestModelGoodEnough() ? best.numInl : 0; } @@ -1346,7 +1346,7 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ */ inline void RHO_HEST_REFC::updateBounds(void){ - arg.maxI = sacCalcIterBound(arg.cfd, + arg.maxI = sacCalcIterBound(arg.cfd, (double)best.numInl/arg.N, SMPL_SIZE, arg.maxI); From 5070f2a33418e22e9b63f7254b458b07c3305d04 Mon Sep 17 00:00:00 2001 From: Hamid Bazargani Date: Fri, 13 Feb 2015 00:44:19 -0500 Subject: [PATCH 026/171] Fix sacCalcJacobianErrors arguments. (curr.inl replaced with best.inl) Fix the issue given NULL inlMask --- modules/calib3d/src/rhorefc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 8a7b57fea..6214813ca 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -1346,7 +1346,7 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ */ inline void RHO_HEST_REFC::updateBounds(void){ - arg.maxI = sacCalcIterBound(arg.cfd, + arg.maxI = sacCalcIterBound(arg.cfd, (double)best.numInl/arg.N, SMPL_SIZE, arg.maxI); From c339720af9cf93ee0c130c55b2a7d2621bca72dc Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 13 Feb 2015 13:38:37 +0100 Subject: [PATCH 027/171] Preparation for 16-bit colored denoising. Currently not working due to cvtColor not supportint 16-bit Lab conversion. --- modules/photo/src/denoising.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 0abeefe5b..8f9d1f84a 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -80,7 +80,7 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, break; case CV_16U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_16UC2: @@ -95,7 +95,7 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, break; default: CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8UC1, CV_8UC2 and CV_8UC3 are supported"); + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_16U, CV_16UC2, and CV_16UC3 are supported"); } } @@ -105,9 +105,9 @@ void cv::fastNlMeansDenoisingColored( InputArray _src, OutputArray _dst, { int type = _src.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type); Size src_size = _src.size(); - if (type != CV_8UC3 && type != CV_8UC4) + if (type != CV_8UC3 && type != CV_16UC3 && type != CV_8UC4 && type != CV_16UC4) { - CV_Error(Error::StsBadArg, "Type of input image should be CV_8UC3!"); + CV_Error(Error::StsBadArg, "Type of input image should be CV_8UC3, CV_16UC3, CV_8UC4, or CV_16UC4"); return; } @@ -123,8 +123,8 @@ void cv::fastNlMeansDenoisingColored( InputArray _src, OutputArray _dst, Mat src_lab; cvtColor(src, src_lab, COLOR_LBGR2Lab); - Mat l(src_size, CV_8U); - Mat ab(src_size, CV_8UC2); + Mat l(src_size, CV_MAKE_TYPE(depth, 1)); + Mat ab(src_size, CV_MAKE_TYPE(depth, 2)); Mat l_ab[] = { l, ab }; int from_to[] = { 0,0, 1,1, 2,2 }; mixChannels(&src_lab, 1, l_ab, 2, from_to, 3); @@ -190,7 +190,7 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds { case CV_8U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; @@ -226,7 +226,7 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds break; default: CV_Error(Error::StsBadArg, - "Unsupported matrix format! Only uchar, Vec2b, Vec3b are supported"); + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_16U, CV_16UC2, and CV_16UC3 are supported"); } } @@ -245,11 +245,12 @@ void cv::fastNlMeansDenoisingColoredMulti( InputArrayOfArrays _srcImgs, OutputAr _dst.create(srcImgs[0].size(), srcImgs[0].type()); Mat dst = _dst.getMat(); + int type = srcImgs[0].type(), depth = CV_MAT_DEPTH(type); int src_imgs_size = static_cast(srcImgs.size()); - if (srcImgs[0].type() != CV_8UC3) + if (type != CV_8UC3 && type != CV_16UC3) { - CV_Error(Error::StsBadArg, "Type of input images should be CV_8UC3!"); + CV_Error(Error::StsBadArg, "Type of input images should be CV_8UC3 or CV_16UC3!"); return; } @@ -261,9 +262,9 @@ void cv::fastNlMeansDenoisingColoredMulti( InputArrayOfArrays _srcImgs, OutputAr std::vector ab(src_imgs_size); for (int i = 0; i < src_imgs_size; i++) { - src_lab[i] = Mat::zeros(srcImgs[0].size(), CV_8UC3); - l[i] = Mat::zeros(srcImgs[0].size(), CV_8UC1); - ab[i] = Mat::zeros(srcImgs[0].size(), CV_8UC2); + src_lab[i] = Mat::zeros(srcImgs[0].size(), type); + l[i] = Mat::zeros(srcImgs[0].size(), CV_MAKE_TYPE(depth, 1)); + ab[i] = Mat::zeros(srcImgs[0].size(), CV_MAKE_TYPE(depth, 2)); cvtColor(srcImgs[i], src_lab[i], COLOR_LBGR2Lab); Mat l_ab[] = { l[i], ab[i] }; From e5696bc5e6042752f8bce7207ad39e516a923d02 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Fri, 13 Feb 2015 08:01:57 -0500 Subject: [PATCH 028/171] Whitespace change reverts to minimize delta w.r.t master. --- modules/calib3d/src/fundam.cpp | 8 +++----- modules/calib3d/src/rhorefc.cpp | 10 +++++----- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index af7c4dfb0..5ec12161d 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -398,8 +398,8 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, { tempMask = Mat::ones(npoints, 1, CV_8U); result = cb->runKernel(src, dst, H) > 0; - } - else if( method == RANSAC) + } + else if( method == RANSAC ) result = createRANSACPointSetRegistrator(cb, 4, ransacReprojThreshold, confidence, maxIters)->run(src, dst, H, tempMask); else if( method == LMEDS ) result = createLMeDSPointSetRegistrator(cb, 4, confidence, maxIters)->run(src, dst, H, tempMask); @@ -408,7 +408,6 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, else CV_Error(Error::StsBadArg, "Unknown estimation method"); - if( result && npoints > 4 && method != RHO) { compressPoints( src.ptr(), tempMask.ptr(), 1, npoints ); @@ -419,9 +418,8 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, Mat dst1 = dst.rowRange(0, npoints); src = src1; dst = dst1; - if( method == RANSAC || method == LMEDS) + if( method == RANSAC || method == LMEDS ) cb->runKernel( src, dst, H ); - Mat H8(8, 1, CV_64F, H.ptr()); createLMSolver(makePtr(src, dst), 10)->run(H8); } diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 2fffadac8..f60f68a04 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -614,9 +614,9 @@ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ for(ctrl.i=0; ctrl.i < arg.maxI || ctrl.i < 100; ctrl.i++){ hypothesize() && verify(); - } + /** * Teardown */ @@ -624,8 +624,9 @@ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ if(isFinalRefineEnabled() && canRefine()){ refine(); } - outputModel(); - finiRun(); + + outputModel(); + finiRun(); return isBestModelGoodEnough() ? best.numInl : 0; } @@ -861,7 +862,6 @@ inline int RHO_HEST_REFC::verify(void){ if(isNREnabled()){ nStarOptimize(); } - } return 1; @@ -1346,7 +1346,7 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ */ inline void RHO_HEST_REFC::updateBounds(void){ - arg.maxI = sacCalcIterBound(arg.cfd, + arg.maxI = sacCalcIterBound(arg.cfd, (double)best.numInl/arg.N, SMPL_SIZE, arg.maxI); From baf266c29eea897d13ae9ce0f85539a74f264b5f Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 17 Feb 2015 21:30:52 +0100 Subject: [PATCH 029/171] Changed from sum of squared differences to sum of abs differences --- .../src/fast_nlmeans_denoising_invoker.hpp | 5 ++- ...fast_nlmeans_denoising_invoker_commons.hpp | 32 +++---------------- .../fast_nlmeans_multi_denoising_invoker.hpp | 5 ++- 3 files changed, 8 insertions(+), 34 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 2de50a77b..cbf9d259f 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -130,13 +130,12 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const double WEIGHT_THRESHOLD = 0.001; const size_t ALLOC_CHUNK = 65536; - IT max_dist = - (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; + IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; size_t almost_max_dist = 0; while (true) { double dist = almost_max_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * pixelInfo::channels))); + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_ || dist > max_dist) break; if (almost_max_dist >= almost_dist2weight_.size()) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index 0a8713b91..4ca63d652 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -85,7 +85,7 @@ template struct calcDist_ { static inline IT f(const T a, const T b) { - return (IT)(a-b) * (IT)(a-b); + return std::abs((IT)(a-b)); } }; @@ -93,7 +93,7 @@ template struct calcDist_, IT> { static inline IT f(const Vec a, const Vec b) { - return (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]); + return std::abs((IT)(a[0]-b[0])) + std::abs((IT)(a[1]-b[1])); } }; @@ -101,10 +101,7 @@ template struct calcDist_, IT> { static inline IT f(const Vec a, const Vec b) { - return - (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + - (IT)(a[1]-b[1])*(IT)(a[1]-b[1]) + - (IT)(a[2]-b[2])*(IT)(a[2]-b[2]); + return std::abs((IT)(a[0]-b[0])) + std::abs((IT)(a[1]-b[1])) + std::abs((IT)(a[2]-b[2])); } }; @@ -121,31 +118,10 @@ static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) return calcDist(a,b); } -template struct calcUpDownDist_ -{ - static inline IT f(T a_up, T a_down, T b_up, T b_down) - { - IT A = a_down - b_down; - IT B = a_up - b_up; - return (A-B)*(A+B); - } -}; - -template struct calcUpDownDist_, IT> -{ -private: - typedef Vec T; -public: - static inline IT f(T a_up, T a_down, T b_up, T b_down) - { - return calcDist(a_down, b_down) - calcDist(a_up, b_up); - } -}; - template static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) { - return calcUpDownDist_::f(a_up, a_down, b_up, b_down); + return calcDist(a_down, b_down) - calcDist(a_up, b_up); }; template struct incWithWeight_ diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index e0f06c68f..f12a0ef50 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -141,13 +141,12 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( const double WEIGHT_THRESHOLD = 0.001; const size_t ALLOC_CHUNK = 65536; - IT max_dist = - (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; + IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; size_t almost_max_dist = 0; while (true) { double dist = almost_max_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist / (h * h * pixelInfo::channels))); + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_ || dist > max_dist) break; if (almost_max_dist >= almost_dist2weight.size()) From e647b7c7e8a15765f7a18ed496fd2313338b900f Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 17 Feb 2015 23:08:36 +0100 Subject: [PATCH 030/171] Calculating almost_dist2weight at full size to avoid bounds checking --- .../src/fast_nlmeans_denoising_invoker.hpp | 31 ++++++++----------- .../fast_nlmeans_multi_denoising_invoker.hpp | 31 ++++++++----------- 2 files changed, 26 insertions(+), 36 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index cbf9d259f..a641c990e 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -128,22 +128,20 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq); double almost_dist2actual_dist_multiplier = ((double)(1 << almost_template_window_size_sq_bin_shift_)) / template_window_size_sq; - const double WEIGHT_THRESHOLD = 0.001; - const size_t ALLOC_CHUNK = 65536; IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; - size_t almost_max_dist = 0; - while (true) - { - double dist = almost_max_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult_ || dist > max_dist) break; - - if (almost_max_dist >= almost_dist2weight_.size()) - almost_dist2weight_.resize(almost_max_dist + ALLOC_CHUNK); - - almost_dist2weight_[almost_max_dist++] = weight; - } + size_t almost_max_dist = (size_t)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight_.resize(almost_max_dist); + + const double WEIGHT_THRESHOLD = 0.001; + for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) + { + double dist = almost_dist * almost_dist2actual_dist_multiplier; + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) + weight = 0; + + almost_dist2weight_[almost_dist] = weight; + } CV_Assert(almost_dist2weight_[0] == fixed_point_mult_); // additional optimization init end @@ -157,8 +155,6 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co int row_from = range.start; int row_to = range.end - 1; - size_t almost_max_dist = almost_dist2weight_.size(); - // sums of cols anf rows for current pixel p Array2d dist_sums(search_window_size_, search_window_size_); @@ -242,8 +238,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co for (int x = 0; x < search_window_size_; x++) { size_t almostAvgDist = (size_t)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_); - IT weight = - almostAvgDist < almost_max_dist ? almost_dist2weight_[almostAvgDist] : 0; + IT weight = almost_dist2weight_[almostAvgDist]; weights_sum += weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index f12a0ef50..808b01f50 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -139,22 +139,20 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( int almost_template_window_size_sq = 1 << almost_template_window_size_sq_bin_shift; double almost_dist2actual_dist_multiplier = (double) almost_template_window_size_sq / template_window_size_sq; - const double WEIGHT_THRESHOLD = 0.001; - const size_t ALLOC_CHUNK = 65536; IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; - size_t almost_max_dist = 0; - while (true) - { - double dist = almost_max_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult_ || dist > max_dist) break; - - if (almost_max_dist >= almost_dist2weight.size()) - almost_dist2weight.resize(almost_max_dist + ALLOC_CHUNK); - - almost_dist2weight[almost_max_dist++] = weight; - } + int almost_max_dist = (int) (max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight.resize(almost_max_dist); + + const double WEIGHT_THRESHOLD = 0.001; + for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) + { + double dist = almost_dist * almost_dist2actual_dist_multiplier; + IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) + weight = 0; + + almost_dist2weight[almost_dist] = weight; + } CV_Assert(almost_dist2weight[0] == fixed_point_mult_); // additional optimization init end @@ -168,8 +166,6 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang int row_from = range.start; int row_to = range.end - 1; - size_t almost_max_dist = almost_dist2weight.size(); - Array3d dist_sums(temporal_window_size_, search_window_size_, search_window_size_); // for lazy calc optimization @@ -270,8 +266,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang { size_t almostAvgDist = (size_t)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift); - IT weight = - almostAvgDist < almost_max_dist ? almost_dist2weight[almostAvgDist] : 0; + IT weight = almost_dist2weight[almostAvgDist]; weights_sum += weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; From 8e7aff44869439f04ef9c0f3ae43b7c6f143c715 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Wed, 18 Feb 2015 14:59:52 +0100 Subject: [PATCH 031/171] Changed fastNlMeansDenoising and fastNlMeansDenoisingMulti back to sum of sq distances. Moved sq sum of abs distances to fastNlMeansDenoisingAbs and fastNlMeansDenoisingMultiAbs --- modules/photo/include/opencv2/photo.hpp | 56 ++++++ modules/photo/src/denoising.cpp | 102 +++++++++-- .../src/fast_nlmeans_denoising_invoker.hpp | 28 +-- ...fast_nlmeans_denoising_invoker_commons.hpp | 159 +++++++++++++++--- .../fast_nlmeans_multi_denoising_invoker.hpp | 28 +-- 5 files changed, 301 insertions(+), 72 deletions(-) diff --git a/modules/photo/include/opencv2/photo.hpp b/modules/photo/include/opencv2/photo.hpp index 2d1087e89..c25a35e6d 100644 --- a/modules/photo/include/opencv2/photo.hpp +++ b/modules/photo/include/opencv2/photo.hpp @@ -138,6 +138,31 @@ parameter. CV_EXPORTS_W void fastNlMeansDenoising( InputArray src, OutputArray dst, float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); +/** @brief Perform image denoising using Non-local Means Denoising +algorithm +with several computational optimizations. Noise expected to be a +gaussian white noise. Uses squared sum of absolute value distances +instead of sum of squared distances for weight calculation + +@param src Input 8-bit or 16-bit 1-channel, 2-channel or 3-channel image. +@param dst Output image with the same size and type as src . +@param templateWindowSize Size in pixels of the template patch that is used to compute weights. +Should be odd. Recommended value 7 pixels +@param searchWindowSize Size in pixels of the window that is used to compute weighted average for +given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater +denoising time. Recommended value 21 pixels +@param h Parameter regulating filter strength. Big h value perfectly removes noise but also +removes image details, smaller h value preserves details but also preserves some noise + +This function expected to be applied to grayscale images. For colored images look at +fastNlMeansDenoisingColored. Advanced usage of this functions can be manual denoising of colored +image in different colorspaces. Such approach is used in fastNlMeansDenoisingColored by converting +image to CIELAB colorspace and then separately denoise L and AB components with different h +parameter. + */ +CV_EXPORTS_W void fastNlMeansDenoisingAbs( InputArray src, OutputArray dst, float h = 3, + int templateWindowSize = 7, int searchWindowSize = 21); + /** @brief Modification of fastNlMeansDenoising function for colored images @param src Input 8-bit 3-channel image. @@ -186,6 +211,37 @@ CV_EXPORTS_W void fastNlMeansDenoisingMulti( InputArrayOfArrays srcImgs, OutputA int imgToDenoiseIndex, int temporalWindowSize, float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); +/** @brief Modification of fastNlMeansDenoising function for images +sequence where consequtive images have been captured in small period +of time. For example video. This version of the function is for +grayscale images or for manual manipulation with colorspaces. For more +details see +. Uses +squared sum of absolute value distances instead of sum of squared +distances for weight calculation + +@param srcImgs Input 8-bit or 16-bit 1-channel, 2-channel or 3-channel +images sequence. All images should +have the same type and size. +@param imgToDenoiseIndex Target image to denoise index in srcImgs sequence +@param temporalWindowSize Number of surrounding images to use for target image denoising. Should +be odd. Images from imgToDenoiseIndex - temporalWindowSize / 2 to +imgToDenoiseIndex - temporalWindowSize / 2 from srcImgs will be used to denoise +srcImgs[imgToDenoiseIndex] image. +@param dst Output image with the same size and type as srcImgs images. +@param templateWindowSize Size in pixels of the template patch that is used to compute weights. +Should be odd. Recommended value 7 pixels +@param searchWindowSize Size in pixels of the window that is used to compute weighted average for +given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater +denoising time. Recommended value 21 pixels +@param h Parameter regulating filter strength for luminance component. Bigger h value perfectly +removes noise but also removes image details, smaller h value preserves details but also preserves +some noise + */ +CV_EXPORTS_W void fastNlMeansDenoisingMultiAbs( InputArrayOfArrays srcImgs, OutputArray dst, + int imgToDenoiseIndex, int temporalWindowSize, + float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); + /** @brief Modification of fastNlMeansDenoisingMulti function for colored images sequences @param srcImgs Input 8-bit 3-channel images sequence. All images should have the same type and diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 8f9d1f84a..52065b5f6 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -65,32 +65,62 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, switch (src.type()) { case CV_8U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC2: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC3: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + default: + CV_Error(Error::StsBadArg, + "Unsupported image format! Only CV_8U, CV_8UC2, and CV_8UC3 are supported"); + } +} + +void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, + int templateWindowSize, int searchWindowSize) +{ + Size src_size = _src.size(); + Mat src = _src.getMat(); + _dst.create(src_size, src.type()); + Mat dst = _dst.getMat(); + + switch (src.type()) { + case CV_8U: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC2: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC3: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_16U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_16UC2: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs>( src, dst, templateWindowSize, searchWindowSize, h)); break; case CV_16UC3: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs>( src, dst, templateWindowSize, searchWindowSize, h)); break; default: @@ -105,9 +135,9 @@ void cv::fastNlMeansDenoisingColored( InputArray _src, OutputArray _dst, { int type = _src.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type); Size src_size = _src.size(); - if (type != CV_8UC3 && type != CV_16UC3 && type != CV_8UC4 && type != CV_16UC4) + if (type != CV_8UC3 && type != CV_8UC4) { - CV_Error(Error::StsBadArg, "Type of input image should be CV_8UC3, CV_16UC3, CV_8UC4, or CV_16UC4"); + CV_Error(Error::StsBadArg, "Type of input image should be CV_8UC3 or CV_8UC4!"); return; } @@ -190,37 +220,77 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds { case CV_8U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC2: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_8UC3: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + default: + CV_Error(Error::StsBadArg, + "Unsupported image format! Only CV_8U, CV_8UC2, and CV_8UC3 are supported"); + } +} + +void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray _dst, + int imgToDenoiseIndex, int temporalWindowSize, + float h, int templateWindowSize, int searchWindowSize) +{ + std::vector srcImgs; + _srcImgs.getMatVector(srcImgs); + + fastNlMeansDenoisingMultiCheckPreconditions( + srcImgs, imgToDenoiseIndex, + temporalWindowSize, templateWindowSize, searchWindowSize); + + _dst.create(srcImgs[0].size(), srcImgs[0].type()); + Mat dst = _dst.getMat(); + + switch (srcImgs[0].type()) + { + case CV_8U: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC2: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC3: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_16U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_16UC2: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; case CV_16UC3: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; @@ -248,9 +318,9 @@ void cv::fastNlMeansDenoisingColoredMulti( InputArrayOfArrays _srcImgs, OutputAr int type = srcImgs[0].type(), depth = CV_MAT_DEPTH(type); int src_imgs_size = static_cast(srcImgs.size()); - if (type != CV_8UC3 && type != CV_16UC3) + if (type != CV_8UC3) { - CV_Error(Error::StsBadArg, "Type of input images should be CV_8UC3 or CV_16UC3!"); + CV_Error(Error::StsBadArg, "Type of input images should be CV_8UC3!"); return; } diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index a641c990e..468fa82f7 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -50,7 +50,7 @@ using namespace cv; -template +template struct FastNlMeansDenoisingInvoker : public ParallelLoopBody { @@ -99,8 +99,8 @@ inline int getNearestPowerOf2(int value) return p; } -template -FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( +template +FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const Mat& src, Mat& dst, int template_window_size, int search_window_size, @@ -128,7 +128,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq); double almost_dist2actual_dist_multiplier = ((double)(1 << almost_template_window_size_sq_bin_shift_)) / template_window_size_sq; - IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; + IT max_dist = D::template maxDist(); size_t almost_max_dist = (size_t)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight_.resize(almost_max_dist); @@ -136,7 +136,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); + IT weight = (IT)round(fixed_point_mult_ * D::template calcWeight(dist, h)); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -149,8 +149,8 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( dst_ = Mat::zeros(src_.size(), src_.type()); } -template -void FastNlMeansDenoisingInvoker::operator() (const Range& range) const +template +void FastNlMeansDenoisingInvoker::operator() (const Range& range) const { int row_from = range.start; int row_to = range.end - 1; @@ -215,7 +215,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co dist_sums_row[x] -= col_dist_sums_row[x]; int bx = start_bx + x; - col_dist_sums_row[x] = up_col_dist_sums_row[x] + calcUpDownDist(a_up, a_down, b_up_ptr[bx], b_down_ptr[bx]); + col_dist_sums_row[x] = up_col_dist_sums_row[x] + D::template calcUpDownDist(a_up, a_down, b_up_ptr[bx], b_down_ptr[bx]); dist_sums_row[x] += col_dist_sums_row[x]; up_col_dist_sums_row[x] = col_dist_sums_row[x]; @@ -254,8 +254,8 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) co } } -template -inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( +template +inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( int i, Array2d& dist_sums, Array3d& col_dist_sums, @@ -276,7 +276,7 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElement for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) for (int tx = -template_window_half_size_; tx <= template_window_half_size_; tx++) { - int dist = calcDist(extended_src_, + int dist = D::template calcDist(extended_src_, border_size_ + i + ty, border_size_ + j + tx, border_size_ + start_y + ty, border_size_ + start_x + tx); @@ -288,8 +288,8 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElement } } -template -inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( +template +inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, Array2d& dist_sums, Array3d& col_dist_sums, @@ -312,7 +312,7 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFir int by = start_by + y; int bx = start_bx + x; for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) - col_dist_sums[new_last_col_num][y][x] += calcDist(extended_src_, ay + ty, ax, by + ty, bx); + col_dist_sums[new_last_col_num][y][x] += D::template calcDist(extended_src_, ay + ty, ax, by + ty, bx); dist_sums[y][x] += col_dist_sums[new_last_col_num][y][x]; up_col_dist_sums[j][y][x] = col_dist_sums[new_last_col_num][y][x]; diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index 4ca63d652..d55d93ce7 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -81,47 +81,150 @@ template struct pixelInfo: public pixelInfo_ } }; -template struct calcDist_ +class DistAbs { - static inline IT f(const T a, const T b) + template struct calcDist_ { - return std::abs((IT)(a-b)); + static inline IT f(const T a, const T b) + { + return std::abs((IT)(a-b)); + } + }; + + template struct calcDist_, IT> + { + static inline IT f(const Vec a, const Vec b) + { + return std::abs((IT)(a[0]-b[0])) + std::abs((IT)(a[1]-b[1])); + } + }; + + template struct calcDist_, IT> + { + static inline IT f(const Vec a, const Vec b) + { + return + std::abs((IT)(a[0]-b[0])) + + std::abs((IT)(a[1]-b[1])) + + std::abs((IT)(a[2]-b[2])); + } + }; + +public: + template static inline IT calcDist(const T a, const T b) + { + return calcDist_::f(a, b); + } + + template + static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) + { + const T a = m.at(i1, j1); + const T b = m.at(i2, j2); + return calcDist(a,b); + } + + template + static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) + { + return calcDist(a_down, b_down) - calcDist(a_up, b_up); + }; + + template + static double calcWeight(double dist, double h) + { + return std::exp(-dist*dist / (h * h * pixelInfo::channels)); + } + + template + static double maxDist() + { + return (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; } }; -template struct calcDist_, IT> +class DistSquared { - static inline IT f(const Vec a, const Vec b) + template struct calcDist_ { - return std::abs((IT)(a[0]-b[0])) + std::abs((IT)(a[1]-b[1])); - } -}; + static inline IT f(const T a, const T b) + { + return (IT)(a-b) * (IT)(a-b); + } + }; -template struct calcDist_, IT> -{ - static inline IT f(const Vec a, const Vec b) + template struct calcDist_, IT> { - return std::abs((IT)(a[0]-b[0])) + std::abs((IT)(a[1]-b[1])) + std::abs((IT)(a[2]-b[2])); + static inline IT f(const Vec a, const Vec b) + { + return (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]); + } + }; + + template struct calcDist_, IT> + { + static inline IT f(const Vec a, const Vec b) + { + return + (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]) + + (IT)(a[2]-b[2])*(IT)(a[2]-b[2]); + } + }; + + template struct calcUpDownDist_ + { + static inline IT f(T a_up, T a_down, T b_up, T b_down) + { + IT A = a_down - b_down; + IT B = a_up - b_up; + return (A-B)*(A+B); + } + }; + + template struct calcUpDownDist_, IT> + { + private: + typedef Vec T; + public: + static inline IT f(T a_up, T a_down, T b_up, T b_down) + { + return calcDist(a_down, b_down) - calcDist(a_up, b_up); + } + }; + +public: + template static inline IT calcDist(const T a, const T b) + { + return calcDist_::f(a, b); } -}; -template static inline IT calcDist(const T a, const T b) -{ - return calcDist_::f(a, b); -} + template + static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) + { + const T a = m.at(i1, j1); + const T b = m.at(i2, j2); + return calcDist(a,b); + } -template -static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) -{ - const T a = m.at(i1, j1); - const T b = m.at(i2, j2); - return calcDist(a,b); -} + template + static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) + { + return calcUpDownDist_::f(a_up, a_down, b_up, b_down); + }; -template -static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) -{ - return calcDist(a_down, b_down) - calcDist(a_up, b_up); + template + static double calcWeight(double dist, double h) + { + return std::exp(-dist / (h * h * pixelInfo::channels)); + } + + template + static double maxDist() + { + return (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * + (IT)pixelInfo::channels; + } }; template struct incWithWeight_ diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index 808b01f50..0a2bdd739 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -50,7 +50,7 @@ using namespace cv; -template +template struct FastNlMeansMultiDenoisingInvoker : ParallelLoopBody { @@ -94,8 +94,8 @@ private: Array4d& up_col_dist_sums) const; }; -template -FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( +template +FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( const std::vector& srcImgs, int imgToDenoiseIndex, int temporalWindowSize, @@ -139,7 +139,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( int almost_template_window_size_sq = 1 << almost_template_window_size_sq_bin_shift; double almost_dist2actual_dist_multiplier = (double) almost_template_window_size_sq / template_window_size_sq; - IT max_dist = (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; + IT max_dist = D::template maxDist(); int almost_max_dist = (int) (max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight.resize(almost_max_dist); @@ -147,7 +147,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * std::exp(-dist*dist / (h * h * pixelInfo::channels))); + IT weight = (IT)round(fixed_point_mult_ * D::template calcWeight(dist, h)); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -160,8 +160,8 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( dst_ = Mat::zeros(srcImgs[0].size(), srcImgs[0].type()); } -template -void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const +template +void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const { int row_from = range.start; int row_to = range.end - 1; @@ -234,7 +234,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang dist_sums_row[x] -= col_dist_sums_row[x]; col_dist_sums_row[x] = up_col_dist_sums_row[x] + - calcUpDownDist(a_up, a_down, b_up_ptr[start_bx + x], b_down_ptr[start_bx + x]); + D::template calcUpDownDist(a_up, a_down, b_up_ptr[start_bx + x], b_down_ptr[start_bx + x]); dist_sums_row[x] += col_dist_sums_row[x]; up_col_dist_sums_row[x] = col_dist_sums_row[x]; @@ -284,8 +284,8 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& rang } } -template -inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRow( +template +inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRow( int i, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { int j = 0; @@ -310,7 +310,7 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstEl { for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) { - IT dist = calcDist( + IT dist = D::template calcDist( main_extended_src_.at(border_size_ + i + ty, border_size_ + j + tx), cur_extended_src.at(border_size_ + start_y + ty, border_size_ + start_x + tx)); @@ -325,8 +325,8 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstEl } } -template -inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRow( +template +inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { @@ -353,7 +353,7 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElement IT* col_dist_sums_ptr = &col_dist_sums[new_last_col_num][d][y][x]; for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) { - *col_dist_sums_ptr += calcDist( + *col_dist_sums_ptr += D::template calcDist( main_extended_src_.at(ay + ty, ax), cur_extended_src.at(by + ty, bx)); } From 5d97dd0ea5d2d752cc0c96156ed310f4897443c7 Mon Sep 17 00:00:00 2001 From: Simon Heinen Date: Fri, 20 Feb 2015 12:54:54 +0100 Subject: [PATCH 032/171] Update android+AsyncServiceHelper.java --- .../generator/src/java/android+AsyncServiceHelper.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/java/generator/src/java/android+AsyncServiceHelper.java b/modules/java/generator/src/java/android+AsyncServiceHelper.java index 568f3da17..b7c57f28c 100644 --- a/modules/java/generator/src/java/android+AsyncServiceHelper.java +++ b/modules/java/generator/src/java/android+AsyncServiceHelper.java @@ -21,9 +21,10 @@ class AsyncServiceHelper final LoaderCallbackInterface Callback) { AsyncServiceHelper helper = new AsyncServiceHelper(Version, AppContext, Callback); - if (AppContext.bindService(new Intent("org.opencv.engine.BIND"), - helper.mServiceConnection, Context.BIND_AUTO_CREATE)) - { + Intent intent = new Intent("org.opencv.engine.BIND"); + intent.setPackage("org.opencv.engine"); + if (AppContext.bindService(intent, helper.mServiceConnection, + Context.BIND_AUTO_CREATE)) { return true; } else From f454929d9c2f96f74cee0c8006458fd40e20da46 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Sat, 21 Feb 2015 12:31:55 -0500 Subject: [PATCH 033/171] PRNG changes: xorshift128+ algorithm, and seeding API. - Switched to the extremely fast, while simple and high-quality, xorshift128+ PRNG algorithm by Sebastiano Vigna in "Further scramblings of Marsaglia's xorshift generators. CoRR, abs/1402.6246, 2014" (2^128-1 period, passes BigCrush tests). Performance improved by 10% over random(). - Added an API to allow seeding with a specified seed, rather than using rand() or random(). This allows deterministic, reproducible results in tests using our algorithm (although findHomography() does not yet support passing an entropy source on its own end). --- modules/calib3d/src/rhorefc.cpp | 214 ++++++++++++++++++++------------ modules/calib3d/src/rhorefc.h | 14 +++ 2 files changed, 149 insertions(+), 79 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index f60f68a04..86d2f0977 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -161,6 +161,11 @@ struct RHO_HEST_REFC{ float* Jte; /* Jte vector */ } lm; + /* PRNG XORshift128+ */ + struct{ + uint64_t s[2]; /* PRNG state */ + } prng; + /* Initialized? */ int init; @@ -205,6 +210,11 @@ struct RHO_HEST_REFC{ inline int PROSACPhaseEndReached(void); inline void PROSACGoToNextPhase(void); inline void getPROSACSample(void); + inline void rndSmpl(unsigned sampleSize, + unsigned* currentSample, + unsigned dataSetSize); + inline double fastRandom(void); + inline void fastSeed(uint64_t seed); inline int isSampleDegenerate(void); inline void generateModel(void); inline int isModelDegenerate(void); @@ -239,10 +249,6 @@ static inline void sacInitNonRand (double beta, 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, @@ -304,6 +310,15 @@ int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta){ } +/** + * Seeds the internal PRNG with the given seed. + */ + +void rhoRefCSeed(RHO_HEST_REFC* p, unsigned long long seed){ + p->fastSeed((uint64_t)seed); +} + + /** * External access to context destructor. * @@ -459,6 +474,12 @@ inline int RHO_HEST_REFC::initialize(void){ lm.tmp1 = NULL; lm.Jte = NULL; +#ifdef _WIN32 + fastSeed(rand()); +#else + fastSeed(random()); +#endif + int areAllAllocsSuccessful = ctrl.smpl && curr.H && @@ -950,13 +971,121 @@ inline void RHO_HEST_REFC::PROSACGoToNextPhase(void){ inline void RHO_HEST_REFC::getPROSACSample(void){ if(ctrl.i > ctrl.phEndI){ /* FIXME: Dubious. Review. */ - sacRndSmpl(4, ctrl.smpl, ctrl.phNum);/* Used to be phMax */ + rndSmpl(4, ctrl.smpl, ctrl.phNum);/* Used to be phMax */ }else{ - sacRndSmpl(3, ctrl.smpl, ctrl.phNum-1); + rndSmpl(3, ctrl.smpl, ctrl.phNum-1); ctrl.smpl[3] = ctrl.phNum-1; } } +/** + * Choose, without repetition, sampleSize integers in the range [0, numDataPoints). + */ + +inline void RHO_HEST_REFC::rndSmpl(unsigned sampleSize, + unsigned* currentSample, + unsigned dataSetSize){ + /** + * If sampleSize is very close to dataSetSize, we use selection sampling. + * Otherwise we use the naive sampling technique wherein we select random + * indexes until sampleSize of them are distinct. + */ + + if(sampleSize*2>dataSetSize){ + /** + * 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> 17; // b + x ^= y ^ (y >> 26); // c + prng.s[0] = y; + prng.s[1] = x; + uint64_t s = x + y; + + return s * 5.421010862427522e-20;/* 2^-64 */ +} + +/** + * Seeds the PRNG. + * + * The seed should not be zero, since the state must be initialized to non-zero. + */ + +inline void RHO_HEST_REFC::fastSeed(uint64_t seed){ + int i; + + prng.s[0] = seed; + prng.s[1] = ~seed;/* Guarantees one of the elements will be non-zero. */ + + /** + * Escape from zero-land (see xorshift128+ paper). Approximately 20 + * iterations required according to the graph. + */ + + for(i=0;i<20;i++){ + fastRandom(); + } +} + /** * Checks whether the *sample* is degenerate prior to model generation. * - First, the extremely cheap numerical degeneracy test is run, which weeds @@ -1395,79 +1524,6 @@ static inline double sacInitPEndFpI(const unsigned ransacConvg, return ransacConvg*numer/denom; } -/** - * Choose, without repetition, sampleSize integers in the range [0, numDataPoints). - */ - -static inline void sacRndSmpl(unsigned sampleSize, - unsigned* currentSample, - unsigned dataSetSize){ - /** - * If sampleSize is very close to dataSetSize, we use selection sampling. - * Otherwise we use the naive sampling technique wherein we select random - * indexes until sampleSize of them are distinct. - */ - - if(sampleSize*2>dataSetSize){ - /** - * 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 Date: Thu, 26 Feb 2015 21:47:29 -0500 Subject: [PATCH 034/171] Removed unnecessary precision in damped Cholesky decomposition. Cholesky decomposition is stable; It is not necessary to carry it out internally at double precision if the result will be truncated to single precision when stored. --- modules/calib3d/src/rhorefc.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 86d2f0977..8372d251a 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -2129,28 +2129,28 @@ static inline int sacChol8x8Damped(const float (*A)[8], const register int N = 8; int i, j, k; float lambdap1 = lambda + 1.0f; - double x; + float x; for(i=0;i Date: Sun, 1 Mar 2015 20:59:34 +0100 Subject: [PATCH 035/171] Refactoring and addition of CV_8UC3 to ocl_fastNlMeansDenoising --- .../src/fast_nlmeans_denoising_opencl.hpp | 44 +++++++++--- modules/photo/src/opencl/nlmeans.cl | 72 ++++++++++++------- 2 files changed, 78 insertions(+), 38 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index 1cdd8fa49..cd7dde385 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -70,11 +70,11 @@ static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindow static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, int templateWindowSize, int searchWindowSize) { - int type = _src.type(), cn = CV_MAT_CN(type); + int type = _src.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type); int ctaSize = ocl::Device::getDefault().isIntel() ? CTA_SIZE_INTEL : CTA_SIZE_DEFAULT; Size size = _src.size(); - if ( type != CV_8UC1 && type != CV_8UC2 && type != CV_8UC4 ) + if ( type != CV_8UC1 && type != CV_8UC2 && type != CV_8UC3 ) return false; int templateWindowHalfWize = templateWindowSize / 2; @@ -86,13 +86,15 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, char cvt[2][40]; String opts = format("-D OP_CALC_FASTNLMEANS -D TEMPLATE_SIZE=%d -D SEARCH_SIZE=%d" - " -D uchar_t=%s -D int_t=%s -D BLOCK_COLS=%d -D BLOCK_ROWS=%d" + " -D sample_t=%s -D pixel_t=%s -D int_t=%s" + " -D BLOCK_COLS=%d -D BLOCK_ROWS=%d" " -D CTA_SIZE=%d -D TEMPLATE_SIZE2=%d -D SEARCH_SIZE2=%d" - " -D convert_int_t=%s -D cn=%d -D convert_uchar_t=%s", - templateWindowSize, searchWindowSize, ocl::typeToStr(type), - ocl::typeToStr(CV_32SC(cn)), BLOCK_COLS, BLOCK_ROWS, ctaSize, - templateWindowHalfWize, searchWindowHalfSize, - ocl::convertTypeStr(CV_8U, CV_32S, cn, cvt[0]), cn, + " -D convert_int_t=%s -D cn=%d -D convert_pixel_t=%s", + templateWindowSize, searchWindowSize, + ocl::typeToStr(depth), ocl::typeToStr(type), ocl::typeToStr(CV_32SC(cn)), + BLOCK_COLS, BLOCK_ROWS, + ctaSize, templateWindowHalfWize, searchWindowHalfSize, + ocl::convertTypeStr(CV_8U, CV_32S, cn, cvt[0]), type == CV_8UC3 ? 4 : cn, ocl::convertTypeStr(CV_32S, CV_8U, cn, cvt[1])); ocl::Kernel k("fastNlMeansDenoising", ocl::photo::nlmeans_oclsrc, opts); @@ -107,10 +109,22 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, UMat srcex; int borderSize = searchWindowHalfSize + templateWindowHalfWize; - copyMakeBorder(_src, srcex, borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); + if (type == CV_8UC3) { + Mat src_rgb = _src.getMat(), src_rgba(size, CV_8UC4); + int from_to[] = { 0,0, 1,1, 2,2 }; + mixChannels(&src_rgb, 1, &src_rgba, 1, from_to, 3); + copyMakeBorder(src_rgba, srcex, + borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); + } + else + copyMakeBorder(_src, srcex, borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); _dst.create(size, type); - UMat dst = _dst.getUMat(); + UMat dst; + if (type == CV_8UC3) + dst.create(size, CV_8UC4); + else + dst = _dst.getUMat(); int searchWindowSizeSq = searchWindowSize * searchWindowSize; Size upColSumSize(size.width, searchWindowSizeSq * nblocksy); @@ -123,7 +137,15 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, ocl::KernelArg::PtrReadOnly(buffer), almostTemplateWindowSizeSqBinShift); size_t globalsize[2] = { nblocksx * ctaSize, nblocksy }, localsize[2] = { ctaSize, 1 }; - return k.run(2, globalsize, localsize, false); + if (!k.run(2, globalsize, localsize, false)) return false; + + if (type == CV_8UC3) { + Mat dst_rgba = dst.getMat(ACCESS_READ), dst_rgb = _dst.getMat(); + int from_to[] = { 0,0, 1,1, 2,2 }; + mixChannels(&dst_rgba, 1, &dst_rgb, 1, from_to, 3); + } + + return true; } static bool ocl_fastNlMeansDenoisingColored( InputArray _src, OutputArray _dst, diff --git a/modules/photo/src/opencl/nlmeans.cl b/modules/photo/src/opencl/nlmeans.cl index af3fb1f9b..c48adda0b 100644 --- a/modules/photo/src/opencl/nlmeans.cl +++ b/modules/photo/src/opencl/nlmeans.cl @@ -29,8 +29,11 @@ __kernel void calcAlmostDist2Weight(__global int * almostDist2Weight, int almost if (almostDist < almostMaxDist) { FT dist = almostDist * almostDist2ActualDistMultiplier; +#ifdef ABS + int weight = convert_int_sat_rte(fixedPointMult * exp(-dist*dist * den)); +#else int weight = convert_int_sat_rte(fixedPointMult * exp(-dist * den)); - +#endif if (weight < WEIGHT_THRESHOLD * fixedPointMult) weight = 0; @@ -44,21 +47,33 @@ __kernel void calcAlmostDist2Weight(__global int * almostDist2Weight, int almost #define SEARCH_SIZE_SQ (SEARCH_SIZE * SEARCH_SIZE) -inline int calcDist(uchar_t a, uchar_t b) +inline int calcDist(pixel_t a, pixel_t b) { +#ifdef ABS + int_t retval = convert_int_t(abs_diff(a, b)); +#else int_t diff = convert_int_t(a) - convert_int_t(b); int_t retval = diff * diff; +#endif #if cn == 1 return retval; #elif cn == 2 return retval.x + retval.y; +#elif cn == 3 || cn == 4 /* A is ignored */ + return retval.x + retval.y + retval.z; #else -#error "cn should be either 1 or 2" +#error "cn should be either 1, 2, 3 or 4" #endif } -inline int calcDistUpDown(uchar_t down_value, uchar_t down_value_t, uchar_t up_value, uchar_t up_value_t) +#ifdef ABS +inline int calcDistUpDown(pixel_t down_value, pixel_t down_value_t, pixel_t up_value, pixel_t up_value_t) +{ + return calcDist(down_value, down_value_t) - calcDist(up_value, up_value_t); +} +#else +inline int calcDistUpDown(pixel_t down_value, pixel_t down_value_t, pixel_t up_value, pixel_t up_value_t) { int_t A = convert_int_t(down_value) - convert_int_t(down_value_t); int_t B = convert_int_t(up_value) - convert_int_t(up_value_t); @@ -68,14 +83,17 @@ inline int calcDistUpDown(uchar_t down_value, uchar_t down_value_t, uchar_t up_v return retval; #elif cn == 2 return retval.x + retval.y; +#elif cn == 3 || cn == 4 /* A is ignored */ + return retval.x + retval.y + retval.z; #else -#error "cn should be either 1 or 2" +#error "cn should be either 1, 2, 3 or 4" #endif } +#endif #define COND if (x == 0 && y == 0) -inline void calcFirstElementInRow(__global const uchar * src, int src_step, int src_offset, +inline void calcFirstElementInRow(__global const sample_t * src, int src_step, int src_offset, __local int * dists, int y, int x, int id, __global int * col_dists, __global int * up_col_dists) { @@ -87,9 +105,9 @@ inline void calcFirstElementInRow(__global const uchar * src, int src_step, int { int dist = 0, value; - __global const uchar_t * src_template = (__global const uchar_t *)(src + + __global const pixel_t * src_template = (__global const pixel_t *)(src + mad24(sy + i / SEARCH_SIZE, src_step, mad24(cn, sx + i % SEARCH_SIZE, src_offset))); - __global const uchar_t * src_current = (__global const uchar_t *)(src + mad24(y, src_step, mad24(cn, x, src_offset))); + __global const pixel_t * src_current = (__global const pixel_t *)(src + mad24(y, src_step, mad24(cn, x, src_offset))); __global int * col_dists_current = col_dists + i * TEMPLATE_SIZE; #pragma unroll @@ -107,8 +125,8 @@ inline void calcFirstElementInRow(__global const uchar * src, int src_step, int dist += value; } - src_current = (__global const uchar_t *)((__global const uchar *)src_current + src_step); - src_template = (__global const uchar_t *)((__global const uchar *)src_template + src_step); + src_current = (__global const pixel_t *)((__global const sample_t *)src_current + src_step); + src_template = (__global const pixel_t *)((__global const sample_t *)src_template + src_step); } #pragma unroll @@ -120,7 +138,7 @@ inline void calcFirstElementInRow(__global const uchar * src, int src_step, int } } -inline void calcElementInFirstRow(__global const uchar * src, int src_step, int src_offset, +inline void calcElementInFirstRow(__global const sample_t * src, int src_step, int src_offset, __local int * dists, int y, int x0, int x, int id, int first, __global int * col_dists, __global int * up_col_dists) { @@ -130,8 +148,8 @@ inline void calcElementInFirstRow(__global const uchar * src, int src_step, int for (int i = id; i < SEARCH_SIZE_SQ; i += CTA_SIZE) { - __global const uchar_t * src_current = (__global const uchar_t *)(src + mad24(y, src_step, mad24(cn, x, src_offset))); - __global const uchar_t * src_template = (__global const uchar_t *)(src + + __global const pixel_t * src_current = (__global const pixel_t *)(src + mad24(y, src_step, mad24(cn, x, src_offset))); + __global const pixel_t * src_template = (__global const pixel_t *)(src + mad24(sy + i / SEARCH_SIZE, src_step, mad24(cn, sx + i % SEARCH_SIZE, src_offset))); __global int * col_dists_current = col_dists + TEMPLATE_SIZE * i; @@ -142,8 +160,8 @@ inline void calcElementInFirstRow(__global const uchar * src, int src_step, int { col_dist += calcDist(src_current[0], src_template[0]); - src_current = (__global const uchar_t *)((__global const uchar *)src_current + src_step); - src_template = (__global const uchar_t *)((__global const uchar *)src_template + src_step); + src_current = (__global const pixel_t *)((__global const sample_t *)src_current + src_step); + src_template = (__global const pixel_t *)((__global const sample_t *)src_template + src_step); } dists[i] += col_dist - col_dists_current[first]; @@ -152,7 +170,7 @@ inline void calcElementInFirstRow(__global const uchar * src, int src_step, int } } -inline void calcElement(__global const uchar * src, int src_step, int src_offset, +inline void calcElement(__global const sample_t * src, int src_step, int src_offset, __local int * dists, int y, int x0, int x, int id, int first, __global int * col_dists, __global int * up_col_dists) { @@ -160,8 +178,8 @@ inline void calcElement(__global const uchar * src, int src_step, int src_offset int sy_up = y - TEMPLATE_SIZE2 - 1; int sy_down = y + TEMPLATE_SIZE2; - uchar_t up_value = *(__global const uchar_t *)(src + mad24(sy_up, src_step, mad24(cn, sx, src_offset))); - uchar_t down_value = *(__global const uchar_t *)(src + mad24(sy_down, src_step, mad24(cn, sx, src_offset))); + pixel_t up_value = *(__global const pixel_t *)(src + mad24(sy_up, src_step, mad24(cn, sx, src_offset))); + pixel_t down_value = *(__global const pixel_t *)(src + mad24(sy_down, src_step, mad24(cn, sx, src_offset))); sx -= SEARCH_SIZE2; sy_up -= SEARCH_SIZE2; @@ -171,8 +189,8 @@ inline void calcElement(__global const uchar * src, int src_step, int src_offset { int wx = i % SEARCH_SIZE, wy = i / SEARCH_SIZE; - uchar_t up_value_t = *(__global const uchar_t *)(src + mad24(sy_up + wy, src_step, mad24(cn, sx + wx, src_offset))); - uchar_t down_value_t = *(__global const uchar_t *)(src + mad24(sy_down + wy, src_step, mad24(cn, sx + wx, src_offset))); + pixel_t up_value_t = *(__global const pixel_t *)(src + mad24(sy_up + wy, src_step, mad24(cn, sx + wx, src_offset))); + pixel_t down_value_t = *(__global const pixel_t *)(src + mad24(sy_down + wy, src_step, mad24(cn, sx + wx, src_offset))); __global int * col_dists_current = col_dists + mad24(i, TEMPLATE_SIZE, first); __global int * up_col_dists_current = up_col_dists + mad24(x0, SEARCH_SIZE_SQ, i); @@ -185,9 +203,9 @@ inline void calcElement(__global const uchar * src, int src_step, int src_offset } } -inline void convolveWindow(__global const uchar * src, int src_step, int src_offset, +inline void convolveWindow(__global const sample_t * src, int src_step, int src_offset, __local int * dists, __global const int * almostDist2Weight, - __global uchar * dst, int dst_step, int dst_offset, + __global sample_t * dst, int dst_step, int dst_offset, int y, int x, int id, __local int * weights_local, __local int_t * weighted_sum_local, int almostTemplateWindowSizeSqBinShift) { @@ -197,7 +215,7 @@ inline void convolveWindow(__global const uchar * src, int src_step, int src_off for (int i = id; i < SEARCH_SIZE_SQ; i += CTA_SIZE) { int src_index = mad24(sy + i / SEARCH_SIZE, src_step, mad24(i % SEARCH_SIZE + sx, cn, src_offset)); - int_t src_value = convert_int_t(*(__global const uchar_t *)(src + src_index)); + int_t src_value = convert_int_t(*(__global const pixel_t *)(src + src_index)); int almostAvgDist = dists[i] >> almostTemplateWindowSizeSqBinShift; int weight = almostDist2Weight[almostAvgDist]; @@ -228,13 +246,13 @@ inline void convolveWindow(__global const uchar * src, int src_step, int src_off weighted_sum_local[2] + weighted_sum_local[3]; int weights_local_0 = weights_local[0] + weights_local[1] + weights_local[2] + weights_local[3]; - *(__global uchar_t *)(dst + dst_index) = convert_uchar_t(weighted_sum_local_0 / (int_t)(weights_local_0)); + *(__global pixel_t *)(dst + dst_index) = convert_pixel_t(weighted_sum_local_0 / (int_t)(weights_local_0)); } } -__kernel void fastNlMeansDenoising(__global const uchar * src, int src_step, int src_offset, - __global uchar * dst, int dst_step, int dst_offset, int dst_rows, int dst_cols, - __global const int * almostDist2Weight, __global uchar * buffer, +__kernel void fastNlMeansDenoising(__global const sample_t * src, int src_step, int src_offset, + __global sample_t * dst, int dst_step, int dst_offset, int dst_rows, int dst_cols, + __global const int * almostDist2Weight, __global sample_t * buffer, int almostTemplateWindowSizeSqBinShift) { int block_x = get_group_id(0), nblocks_x = get_num_groups(0); From 9f7cac8c5933df74bf953227368b1bcd181d1b12 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Sun, 1 Mar 2015 21:01:57 +0100 Subject: [PATCH 036/171] Addtion of test cases for CV_8UC3 --- modules/photo/test/ocl/test_denoising.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/photo/test/ocl/test_denoising.cpp b/modules/photo/test/ocl/test_denoising.cpp index cb2d74f85..48efc8ab5 100644 --- a/modules/photo/test/ocl/test_denoising.cpp +++ b/modules/photo/test/ocl/test_denoising.cpp @@ -87,7 +87,7 @@ OCL_TEST_P(FastNlMeansDenoisingColored, Mat) } } -OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoising, Combine(Values(1, 2), Bool())); +OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoising, Combine(Values(1, 2, 3), Bool())); OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoisingColored, Combine(Values(3, 4), Bool())); } } // namespace cvtest::ocl From a9ff335a8923c92e9dc86ddac3571aeaae6f0fbf Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Sun, 1 Mar 2015 22:21:36 +0100 Subject: [PATCH 037/171] Added OpenCL support for FastNlMeansDenoisingAbs --- modules/photo/src/denoising.cpp | 6 +++++- .../src/fast_nlmeans_denoising_opencl.hpp | 18 +++++++++--------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 52065b5f6..3fe1f2b90 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -51,7 +51,7 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, Size src_size = _src.size(); CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes - ocl_fastNlMeansDenoising(_src, _dst, h, templateWindowSize, searchWindowSize)) + ocl_fastNlMeansDenoising(_src, _dst, h, templateWindowSize, searchWindowSize, false)) Mat src = _src.getMat(); _dst.create(src_size, src.type()); @@ -88,6 +88,10 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, int templateWindowSize, int searchWindowSize) { Size src_size = _src.size(); + CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && + src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes + ocl_fastNlMeansDenoising(_src, _dst, h, templateWindowSize, searchWindowSize, true)) + Mat src = _src.getMat(); _dst.create(src_size, src.type()); Mat dst = _dst.getMat(); diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index cd7dde385..5e96533fb 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -30,7 +30,7 @@ static int divUp(int a, int b) template static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindowSize, int templateWindowSize, FT h, int cn, - int & almostTemplateWindowSizeSqBinShift) + int & almostTemplateWindowSizeSqBinShift, bool abs) { const int maxEstimateSumValue = searchWindowSize * searchWindowSize * 255; int fixedPointMult = std::numeric_limits::max() / maxEstimateSumValue; @@ -48,15 +48,15 @@ static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindow FT almostDist2ActualDistMultiplier = (FT)(1 << almostTemplateWindowSizeSqBinShift) / templateWindowSizeSq; const FT WEIGHT_THRESHOLD = 1e-3f; - int maxDist = 255 * 255 * cn; + int maxDist = abs ? 255 * cn : 255 * 255 * cn; int almostMaxDist = (int)(maxDist / almostDist2ActualDistMultiplier + 1); FT den = 1.0f / (h * h * cn); almostDist2Weight.create(1, almostMaxDist, CV_32SC1); ocl::Kernel k("calcAlmostDist2Weight", ocl::photo::nlmeans_oclsrc, - format("-D OP_CALC_WEIGHTS -D FT=%s%s", ocl::typeToStr(depth), - doubleSupport ? " -D DOUBLE_SUPPORT" : "")); + format("-D OP_CALC_WEIGHTS -D FT=%s%s%s", ocl::typeToStr(depth), + doubleSupport ? " -D DOUBLE_SUPPORT" : "", abs ? " -D ABS" : "")); if (k.empty()) return false; @@ -68,7 +68,7 @@ static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindow } static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, - int templateWindowSize, int searchWindowSize) + int templateWindowSize, int searchWindowSize, bool abs) { int type = _src.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type); int ctaSize = ocl::Device::getDefault().isIntel() ? CTA_SIZE_INTEL : CTA_SIZE_DEFAULT; @@ -89,21 +89,21 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, " -D sample_t=%s -D pixel_t=%s -D int_t=%s" " -D BLOCK_COLS=%d -D BLOCK_ROWS=%d" " -D CTA_SIZE=%d -D TEMPLATE_SIZE2=%d -D SEARCH_SIZE2=%d" - " -D convert_int_t=%s -D cn=%d -D convert_pixel_t=%s", + " -D convert_int_t=%s -D cn=%d -D convert_pixel_t=%s%s", templateWindowSize, searchWindowSize, ocl::typeToStr(depth), ocl::typeToStr(type), ocl::typeToStr(CV_32SC(cn)), BLOCK_COLS, BLOCK_ROWS, ctaSize, templateWindowHalfWize, searchWindowHalfSize, ocl::convertTypeStr(CV_8U, CV_32S, cn, cvt[0]), type == CV_8UC3 ? 4 : cn, - ocl::convertTypeStr(CV_32S, CV_8U, cn, cvt[1])); + ocl::convertTypeStr(CV_32S, CV_8U, cn, cvt[1]), abs ? " -D ABS" : ""); ocl::Kernel k("fastNlMeansDenoising", ocl::photo::nlmeans_oclsrc, opts); if (k.empty()) return false; UMat almostDist2Weight; - if (!ocl_calcAlmostDist2Weight(almostDist2Weight, searchWindowSize, templateWindowSize, h, cn, - almostTemplateWindowSizeSqBinShift)) + if (!ocl_calcAlmostDist2Weight(almostDist2Weight, searchWindowSize, templateWindowSize, + h, cn, almostTemplateWindowSizeSqBinShift, abs)) return false; CV_Assert(almostTemplateWindowSizeSqBinShift >= 0); From 3bde9e93651a0d2f388ba3b5be7e3c5d9de9820c Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Sun, 1 Mar 2015 22:22:09 +0100 Subject: [PATCH 038/171] Added test cases --- modules/photo/test/ocl/test_denoising.cpp | 51 +++++++++++++++++------ 1 file changed, 39 insertions(+), 12 deletions(-) diff --git a/modules/photo/test/ocl/test_denoising.cpp b/modules/photo/test/ocl/test_denoising.cpp index 48efc8ab5..30dc680c8 100644 --- a/modules/photo/test/ocl/test_denoising.cpp +++ b/modules/photo/test/ocl/test_denoising.cpp @@ -13,11 +13,11 @@ namespace cvtest { namespace ocl { -PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool) +PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool, bool) { int cn, templateWindowSize, searchWindowSize; float h; - bool use_roi; + bool use_roi, use_image; TEST_DECLARE_INPUT_PARAMETER(src); TEST_DECLARE_OUTPUT_PARAMETER(dst); @@ -26,6 +26,7 @@ PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool) { cn = GET_PARAM(0); use_roi = GET_PARAM(1); + use_image = GET_PARAM(2); templateWindowSize = 7; searchWindowSize = 21; @@ -34,20 +35,27 @@ PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool) virtual void generateTestData() { + const int type = CV_8UC(cn); Mat image; - if (cn == 1) - { - image = readImage("denoising/lena_noised_gaussian_sigma=10.png", IMREAD_GRAYSCALE); + + if (use_image) { + image = readImage("denoising/lena_noised_gaussian_sigma=10.png", + cn == 1 ? IMREAD_GRAYSCALE : IMREAD_COLOR); ASSERT_FALSE(image.empty()); } - const int type = CV_8UC(cn); - - Size roiSize = cn == 1 ? image.size() : randomSize(1, MAX_VALUE); + Size roiSize = use_image ? image.size() : randomSize(1, MAX_VALUE); Border srcBorder = randomBorder(0, use_roi ? MAX_VALUE : 0); randomSubMat(src, src_roi, roiSize, srcBorder, type, 0, 255); - if (cn == 1) - image.copyTo(src_roi); + if (use_image) { + ASSERT_TRUE(cn == 1 || cn == 2 || cn == 3); + if (cn == 2) { + int from_to[] = { 0,0, 1,1 }; + src_roi.create(roiSize, type); + mixChannels(&image, 1, &src_roi, 1, from_to, 2); + } + else image.copyTo(src_roi); + } Border dstBorder = randomBorder(0, use_roi ? MAX_VALUE : 0); randomSubMat(dst, dst_roi, roiSize, dstBorder, type, 0, 255); @@ -72,6 +80,21 @@ OCL_TEST_P(FastNlMeansDenoising, Mat) } } +typedef FastNlMeansDenoisingTestBase FastNlMeansDenoisingAbs; + +OCL_TEST_P(FastNlMeansDenoisingAbs, Mat) +{ + for (int j = 0; j < test_loop_times; j++) + { + generateTestData(); + + OCL_OFF(cv::fastNlMeansDenoisingAbs(src_roi, dst_roi, h, templateWindowSize, searchWindowSize)); + OCL_ON(cv::fastNlMeansDenoisingAbs(usrc_roi, udst_roi, h, templateWindowSize, searchWindowSize)); + + OCL_EXPECT_MATS_NEAR(dst, 1); + } +} + typedef FastNlMeansDenoisingTestBase FastNlMeansDenoisingColored; OCL_TEST_P(FastNlMeansDenoisingColored, Mat) @@ -87,8 +110,12 @@ OCL_TEST_P(FastNlMeansDenoisingColored, Mat) } } -OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoising, Combine(Values(1, 2, 3), Bool())); -OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoisingColored, Combine(Values(3, 4), Bool())); +OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoising, + Combine(Values(1, 2, 3), Bool(), Bool())); +OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoisingAbs, + Combine(Values(1, 2, 3), Bool(), Bool())); +OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoisingColored, + Combine(Values(3, 4), Bool(), Values(false))); } } // namespace cvtest::ocl From 73663dcdd1f0f06a0567f266c4f9ebeb9b74a2b2 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Mon, 2 Mar 2015 03:29:17 +0100 Subject: [PATCH 039/171] Added support for 16-bit input --- .../src/fast_nlmeans_denoising_opencl.hpp | 57 ++++++++++++------- modules/photo/src/opencl/nlmeans.cl | 31 ++++++---- 2 files changed, 56 insertions(+), 32 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index 5e96533fb..a88b5cfd7 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -28,12 +28,14 @@ static int divUp(int a, int b) return (a + b - 1) / b; } -template +template static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindowSize, int templateWindowSize, FT h, int cn, int & almostTemplateWindowSizeSqBinShift, bool abs) { - const int maxEstimateSumValue = searchWindowSize * searchWindowSize * 255; - int fixedPointMult = std::numeric_limits::max() / maxEstimateSumValue; + const WT maxEstimateSumValue = searchWindowSize * searchWindowSize * + std::numeric_limits::max(); + int fixedPointMult = (int)std::min(std::numeric_limits::max() / maxEstimateSumValue, + std::numeric_limits::max()); int depth = DataType::depth; bool doubleSupport = ocl::Device::getDefault().doubleFPConfig() > 0; @@ -48,7 +50,8 @@ static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindow FT almostDist2ActualDistMultiplier = (FT)(1 << almostTemplateWindowSizeSqBinShift) / templateWindowSizeSq; const FT WEIGHT_THRESHOLD = 1e-3f; - int maxDist = abs ? 255 * cn : 255 * 255 * cn; + int maxDist = abs ? std::numeric_limits::max() * cn : + std::numeric_limits::max() * std::numeric_limits::max() * cn; int almostMaxDist = (int)(maxDist / almostDist2ActualDistMultiplier + 1); FT den = 1.0f / (h * h * cn); @@ -74,7 +77,7 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, int ctaSize = ocl::Device::getDefault().isIntel() ? CTA_SIZE_INTEL : CTA_SIZE_DEFAULT; Size size = _src.size(); - if ( type != CV_8UC1 && type != CV_8UC2 && type != CV_8UC3 ) + if (cn != 1 && cn != 2 && cn != 3 && depth != CV_8U && (!abs || depth != CV_16U)) return false; int templateWindowHalfWize = templateWindowSize / 2; @@ -84,45 +87,60 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, int nblocksx = divUp(size.width, BLOCK_COLS), nblocksy = divUp(size.height, BLOCK_ROWS); int almostTemplateWindowSizeSqBinShift = -1; - char cvt[2][40]; + char buf[4][40]; String opts = format("-D OP_CALC_FASTNLMEANS -D TEMPLATE_SIZE=%d -D SEARCH_SIZE=%d" " -D sample_t=%s -D pixel_t=%s -D int_t=%s" + " -D weight_t=%s -D sum_t=%s -D convert_sum_t=%s" " -D BLOCK_COLS=%d -D BLOCK_ROWS=%d" " -D CTA_SIZE=%d -D TEMPLATE_SIZE2=%d -D SEARCH_SIZE2=%d" " -D convert_int_t=%s -D cn=%d -D convert_pixel_t=%s%s", templateWindowSize, searchWindowSize, ocl::typeToStr(depth), ocl::typeToStr(type), ocl::typeToStr(CV_32SC(cn)), + depth == CV_8U ? ocl::typeToStr(CV_32S) : "long", + depth == CV_8U ? ocl::typeToStr(CV_32SC(cn)) : + (sprintf(buf[0], "long%d", cn), buf[0]), + depth == CV_8U ? ocl::convertTypeStr(depth, CV_32S, cn, buf[1]) : + (sprintf(buf[1], "convert_long%d", cn), buf[1]), BLOCK_COLS, BLOCK_ROWS, ctaSize, templateWindowHalfWize, searchWindowHalfSize, - ocl::convertTypeStr(CV_8U, CV_32S, cn, cvt[0]), type == CV_8UC3 ? 4 : cn, - ocl::convertTypeStr(CV_32S, CV_8U, cn, cvt[1]), abs ? " -D ABS" : ""); + ocl::convertTypeStr(depth, CV_32S, cn, buf[2]), cn == 3 ? 4 : cn, + ocl::convertTypeStr(CV_32S, depth, cn, buf[3]), abs ? " -D ABS" : ""); ocl::Kernel k("fastNlMeansDenoising", ocl::photo::nlmeans_oclsrc, opts); if (k.empty()) return false; UMat almostDist2Weight; - if (!ocl_calcAlmostDist2Weight(almostDist2Weight, searchWindowSize, templateWindowSize, - h, cn, almostTemplateWindowSizeSqBinShift, abs)) + if ((depth == CV_8U && + !ocl_calcAlmostDist2Weight(almostDist2Weight, + searchWindowSize, templateWindowSize, + h, cn, + almostTemplateWindowSizeSqBinShift, + abs)) || + (depth == CV_16U && + !ocl_calcAlmostDist2Weight(almostDist2Weight, + searchWindowSize, templateWindowSize, + h, cn, + almostTemplateWindowSizeSqBinShift, + abs))) return false; CV_Assert(almostTemplateWindowSizeSqBinShift >= 0); UMat srcex; int borderSize = searchWindowHalfSize + templateWindowHalfWize; - if (type == CV_8UC3) { - Mat src_rgb = _src.getMat(), src_rgba(size, CV_8UC4); + if (cn == 3) { + UMat tmp(size, CV_MAKE_TYPE(depth, 4)); int from_to[] = { 0,0, 1,1, 2,2 }; - mixChannels(&src_rgb, 1, &src_rgba, 1, from_to, 3); - copyMakeBorder(src_rgba, srcex, - borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); + mixChannels(std::vector(1, _src.getUMat()), std::vector(1, tmp), from_to, 3); + copyMakeBorder(tmp, srcex, borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); } else copyMakeBorder(_src, srcex, borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); _dst.create(size, type); UMat dst; - if (type == CV_8UC3) - dst.create(size, CV_8UC4); + if (cn == 3) + dst.create(size, CV_MAKE_TYPE(depth, 4)); else dst = _dst.getUMat(); @@ -139,10 +157,9 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, size_t globalsize[2] = { nblocksx * ctaSize, nblocksy }, localsize[2] = { ctaSize, 1 }; if (!k.run(2, globalsize, localsize, false)) return false; - if (type == CV_8UC3) { - Mat dst_rgba = dst.getMat(ACCESS_READ), dst_rgb = _dst.getMat(); + if (cn == 3) { int from_to[] = { 0,0, 1,1, 2,2 }; - mixChannels(&dst_rgba, 1, &dst_rgb, 1, from_to, 3); + mixChannels(std::vector(1, dst), std::vector(1, _dst.getUMat()), from_to, 3); } return true; diff --git a/modules/photo/src/opencl/nlmeans.cl b/modules/photo/src/opencl/nlmeans.cl index c48adda0b..3a104c42a 100644 --- a/modules/photo/src/opencl/nlmeans.cl +++ b/modules/photo/src/opencl/nlmeans.cl @@ -206,22 +206,23 @@ inline void calcElement(__global const sample_t * src, int src_step, int src_off inline void convolveWindow(__global const sample_t * src, int src_step, int src_offset, __local int * dists, __global const int * almostDist2Weight, __global sample_t * dst, int dst_step, int dst_offset, - int y, int x, int id, __local int * weights_local, - __local int_t * weighted_sum_local, int almostTemplateWindowSizeSqBinShift) + int y, int x, int id, __local weight_t * weights_local, + __local sum_t * weighted_sum_local, int almostTemplateWindowSizeSqBinShift) { - int sx = x - SEARCH_SIZE2, sy = y - SEARCH_SIZE2, weights = 0; - int_t weighted_sum = (int_t)(0); + int sx = x - SEARCH_SIZE2, sy = y - SEARCH_SIZE2; + weight_t weights = 0; + sum_t weighted_sum = (sum_t)(0); for (int i = id; i < SEARCH_SIZE_SQ; i += CTA_SIZE) { int src_index = mad24(sy + i / SEARCH_SIZE, src_step, mad24(i % SEARCH_SIZE + sx, cn, src_offset)); - int_t src_value = convert_int_t(*(__global const pixel_t *)(src + src_index)); + sum_t src_value = convert_sum_t(*(__global const pixel_t *)(src + src_index)); int almostAvgDist = dists[i] >> almostTemplateWindowSizeSqBinShift; int weight = almostDist2Weight[almostAvgDist]; - weights += weight; - weighted_sum += (int_t)(weight) * src_value; + weights += (weight_t)weight; + weighted_sum += (sum_t)(weight) * src_value; } weights_local[id] = weights; @@ -242,11 +243,11 @@ inline void convolveWindow(__global const sample_t * src, int src_step, int src_ if (id == 0) { int dst_index = mad24(y, dst_step, mad24(cn, x, dst_offset)); - int_t weighted_sum_local_0 = weighted_sum_local[0] + weighted_sum_local[1] + + sum_t weighted_sum_local_0 = weighted_sum_local[0] + weighted_sum_local[1] + weighted_sum_local[2] + weighted_sum_local[3]; - int weights_local_0 = weights_local[0] + weights_local[1] + weights_local[2] + weights_local[3]; + weight_t weights_local_0 = weights_local[0] + weights_local[1] + weights_local[2] + weights_local[3]; - *(__global pixel_t *)(dst + dst_index) = convert_pixel_t(weighted_sum_local_0 / (int_t)(weights_local_0)); + *(__global pixel_t *)(dst + dst_index) = convert_pixel_t(weighted_sum_local_0 / (sum_t)(weights_local_0)); } } @@ -259,8 +260,9 @@ __kernel void fastNlMeansDenoising(__global const sample_t * src, int src_step, int block_y = get_group_id(1); int id = get_local_id(0), first; - __local int dists[SEARCH_SIZE_SQ], weights[CTA_SIZE]; - __local int_t weighted_sum[CTA_SIZE]; + __local int dists[SEARCH_SIZE_SQ]; + __local weight_t weights[CTA_SIZE]; + __local sum_t weighted_sum[CTA_SIZE]; int x0 = block_x * BLOCK_COLS, x1 = min(x0 + BLOCK_COLS, dst_cols); int y0 = block_y * BLOCK_ROWS, y1 = min(y0 + BLOCK_ROWS, dst_rows); @@ -271,6 +273,11 @@ __kernel void fastNlMeansDenoising(__global const sample_t * src, int src_step, __global int * col_dists = (__global int *)(buffer + block_data_start * sizeof(int)); __global int * up_col_dists = col_dists + SEARCH_SIZE_SQ * TEMPLATE_SIZE; + src_step /= sizeof(sample_t); + src_offset /= sizeof(sample_t); + dst_step /= sizeof(sample_t); + dst_offset /= sizeof(sample_t); + for (int y = y0; y < y1; ++y) for (int x = x0; x < x1; ++x) { From 50bb14a0a8642ffdf71969c78226ddd236bf97b9 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Mon, 2 Mar 2015 15:48:00 +0100 Subject: [PATCH 040/171] Avoiding unnecessary copy by creating borders in place after RGB -> RGBA conversion --- modules/photo/src/fast_nlmeans_denoising_opencl.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index a88b5cfd7..9c0e40401 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -129,10 +129,12 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, UMat srcex; int borderSize = searchWindowHalfSize + templateWindowHalfWize; if (cn == 3) { - UMat tmp(size, CV_MAKE_TYPE(depth, 4)); + srcex.create(size.height + 2*borderSize, size.width + 2*borderSize, CV_MAKE_TYPE(depth, 4)); + UMat src(srcex, Rect(borderSize, borderSize, size.width, size.height)); int from_to[] = { 0,0, 1,1, 2,2 }; - mixChannels(std::vector(1, _src.getUMat()), std::vector(1, tmp), from_to, 3); - copyMakeBorder(tmp, srcex, borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); + mixChannels(std::vector(1, _src.getUMat()), std::vector(1, src), from_to, 3); + copyMakeBorder(src, srcex, borderSize, borderSize, borderSize, borderSize, + BORDER_DEFAULT|BORDER_ISOLATED); // create borders in place } else copyMakeBorder(_src, srcex, borderSize, borderSize, borderSize, borderSize, BORDER_DEFAULT); From 87760d13fbee8b005800c55246fa59a3e4cc8685 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Mon, 2 Mar 2015 22:33:14 +0100 Subject: [PATCH 041/171] Cleanup and addition of 4-component support for ocl_fastNlMeansDenoising --- .../src/fast_nlmeans_denoising_opencl.hpp | 6 ++-- modules/photo/src/opencl/nlmeans.cl | 28 +++++++++++-------- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index 9c0e40401..41264045c 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -77,7 +77,7 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, int ctaSize = ocl::Device::getDefault().isIntel() ? CTA_SIZE_INTEL : CTA_SIZE_DEFAULT; Size size = _src.size(); - if (cn != 1 && cn != 2 && cn != 3 && depth != CV_8U && (!abs || depth != CV_16U)) + if (cn != 1 && cn != 2 && cn != 3 && cn != 4 && depth != CV_8U && (!abs || depth != CV_16U)) return false; int templateWindowHalfWize = templateWindowSize / 2; @@ -93,7 +93,7 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, " -D weight_t=%s -D sum_t=%s -D convert_sum_t=%s" " -D BLOCK_COLS=%d -D BLOCK_ROWS=%d" " -D CTA_SIZE=%d -D TEMPLATE_SIZE2=%d -D SEARCH_SIZE2=%d" - " -D convert_int_t=%s -D cn=%d -D convert_pixel_t=%s%s", + " -D convert_int_t=%s -D cn=%d -D psz=%d -D convert_pixel_t=%s%s", templateWindowSize, searchWindowSize, ocl::typeToStr(depth), ocl::typeToStr(type), ocl::typeToStr(CV_32SC(cn)), depth == CV_8U ? ocl::typeToStr(CV_32S) : "long", @@ -103,7 +103,7 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, (sprintf(buf[1], "convert_long%d", cn), buf[1]), BLOCK_COLS, BLOCK_ROWS, ctaSize, templateWindowHalfWize, searchWindowHalfSize, - ocl::convertTypeStr(depth, CV_32S, cn, buf[2]), cn == 3 ? 4 : cn, + ocl::convertTypeStr(depth, CV_32S, cn, buf[2]), cn, cn == 3 ? 4 : cn, ocl::convertTypeStr(CV_32S, depth, cn, buf[3]), abs ? " -D ABS" : ""); ocl::Kernel k("fastNlMeansDenoising", ocl::photo::nlmeans_oclsrc, opts); diff --git a/modules/photo/src/opencl/nlmeans.cl b/modules/photo/src/opencl/nlmeans.cl index 3a104c42a..91b012354 100644 --- a/modules/photo/src/opencl/nlmeans.cl +++ b/modules/photo/src/opencl/nlmeans.cl @@ -60,8 +60,10 @@ inline int calcDist(pixel_t a, pixel_t b) return retval; #elif cn == 2 return retval.x + retval.y; -#elif cn == 3 || cn == 4 /* A is ignored */ +#elif cn == 3 return retval.x + retval.y + retval.z; +#elif cn == 4 + return retval.x + retval.y + retval.z + retval.w; #else #error "cn should be either 1, 2, 3 or 4" #endif @@ -83,8 +85,10 @@ inline int calcDistUpDown(pixel_t down_value, pixel_t down_value_t, pixel_t up_v return retval; #elif cn == 2 return retval.x + retval.y; -#elif cn == 3 || cn == 4 /* A is ignored */ +#elif cn == 3 return retval.x + retval.y + retval.z; +#elif cn == 4 + return retval.x + retval.y + retval.z + retval.w; #else #error "cn should be either 1, 2, 3 or 4" #endif @@ -106,8 +110,8 @@ inline void calcFirstElementInRow(__global const sample_t * src, int src_step, i int dist = 0, value; __global const pixel_t * src_template = (__global const pixel_t *)(src + - mad24(sy + i / SEARCH_SIZE, src_step, mad24(cn, sx + i % SEARCH_SIZE, src_offset))); - __global const pixel_t * src_current = (__global const pixel_t *)(src + mad24(y, src_step, mad24(cn, x, src_offset))); + mad24(sy + i / SEARCH_SIZE, src_step, mad24(psz, sx + i % SEARCH_SIZE, src_offset))); + __global const pixel_t * src_current = (__global const pixel_t *)(src + mad24(y, src_step, mad24(psz, x, src_offset))); __global int * col_dists_current = col_dists + i * TEMPLATE_SIZE; #pragma unroll @@ -148,9 +152,9 @@ inline void calcElementInFirstRow(__global const sample_t * src, int src_step, i for (int i = id; i < SEARCH_SIZE_SQ; i += CTA_SIZE) { - __global const pixel_t * src_current = (__global const pixel_t *)(src + mad24(y, src_step, mad24(cn, x, src_offset))); + __global const pixel_t * src_current = (__global const pixel_t *)(src + mad24(y, src_step, mad24(psz, x, src_offset))); __global const pixel_t * src_template = (__global const pixel_t *)(src + - mad24(sy + i / SEARCH_SIZE, src_step, mad24(cn, sx + i % SEARCH_SIZE, src_offset))); + mad24(sy + i / SEARCH_SIZE, src_step, mad24(psz, sx + i % SEARCH_SIZE, src_offset))); __global int * col_dists_current = col_dists + TEMPLATE_SIZE * i; int col_dist = 0; @@ -178,8 +182,8 @@ inline void calcElement(__global const sample_t * src, int src_step, int src_off int sy_up = y - TEMPLATE_SIZE2 - 1; int sy_down = y + TEMPLATE_SIZE2; - pixel_t up_value = *(__global const pixel_t *)(src + mad24(sy_up, src_step, mad24(cn, sx, src_offset))); - pixel_t down_value = *(__global const pixel_t *)(src + mad24(sy_down, src_step, mad24(cn, sx, src_offset))); + pixel_t up_value = *(__global const pixel_t *)(src + mad24(sy_up, src_step, mad24(psz, sx, src_offset))); + pixel_t down_value = *(__global const pixel_t *)(src + mad24(sy_down, src_step, mad24(psz, sx, src_offset))); sx -= SEARCH_SIZE2; sy_up -= SEARCH_SIZE2; @@ -189,8 +193,8 @@ inline void calcElement(__global const sample_t * src, int src_step, int src_off { int wx = i % SEARCH_SIZE, wy = i / SEARCH_SIZE; - pixel_t up_value_t = *(__global const pixel_t *)(src + mad24(sy_up + wy, src_step, mad24(cn, sx + wx, src_offset))); - pixel_t down_value_t = *(__global const pixel_t *)(src + mad24(sy_down + wy, src_step, mad24(cn, sx + wx, src_offset))); + pixel_t up_value_t = *(__global const pixel_t *)(src + mad24(sy_up + wy, src_step, mad24(psz, sx + wx, src_offset))); + pixel_t down_value_t = *(__global const pixel_t *)(src + mad24(sy_down + wy, src_step, mad24(psz, sx + wx, src_offset))); __global int * col_dists_current = col_dists + mad24(i, TEMPLATE_SIZE, first); __global int * up_col_dists_current = up_col_dists + mad24(x0, SEARCH_SIZE_SQ, i); @@ -215,7 +219,7 @@ inline void convolveWindow(__global const sample_t * src, int src_step, int src_ for (int i = id; i < SEARCH_SIZE_SQ; i += CTA_SIZE) { - int src_index = mad24(sy + i / SEARCH_SIZE, src_step, mad24(i % SEARCH_SIZE + sx, cn, src_offset)); + int src_index = mad24(sy + i / SEARCH_SIZE, src_step, mad24(i % SEARCH_SIZE + sx, psz, src_offset)); sum_t src_value = convert_sum_t(*(__global const pixel_t *)(src + src_index)); int almostAvgDist = dists[i] >> almostTemplateWindowSizeSqBinShift; @@ -242,7 +246,7 @@ inline void convolveWindow(__global const sample_t * src, int src_step, int src_ if (id == 0) { - int dst_index = mad24(y, dst_step, mad24(cn, x, dst_offset)); + int dst_index = mad24(y, dst_step, mad24(psz, x, dst_offset)); sum_t weighted_sum_local_0 = weighted_sum_local[0] + weighted_sum_local[1] + weighted_sum_local[2] + weighted_sum_local[3]; weight_t weights_local_0 = weights_local[0] + weights_local[1] + weights_local[2] + weights_local[3]; From ae08884854a7b46db96eef489b6a943d1bb04f56 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 3 Mar 2015 01:19:34 +0100 Subject: [PATCH 042/171] Added support for 4-component input for fastNlMeansDenoising[Multi][Abs] --- modules/photo/src/denoising.cpp | 41 ++++++++++++++-- .../src/fast_nlmeans_denoising_invoker.hpp | 2 +- ...fast_nlmeans_denoising_invoker_commons.hpp | 48 +++++++++++++++++++ .../fast_nlmeans_multi_denoising_invoker.hpp | 2 +- 4 files changed, 87 insertions(+), 6 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 3fe1f2b90..b41f83ec9 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -78,9 +78,14 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; + case CV_8UC4: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; default: CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, and CV_8UC3 are supported"); + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3 and CV_8UC4 are supported"); } } @@ -112,6 +117,11 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, h)); break; + case CV_8UC4: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; case CV_16U: parallel_for_(cv::Range(0, src.rows), FastNlMeansDenoisingInvoker( @@ -127,9 +137,14 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, FastNlMeansDenoisingInvoker, int64, uint64, DistAbs>( src, dst, templateWindowSize, searchWindowSize, h)); break; + case CV_16UC4: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs>( + src, dst, templateWindowSize, searchWindowSize, h)); + break; default: CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_16U, CV_16UC2, and CV_16UC3 are supported"); + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_8UC4, CV_16U, CV_16UC2, CV_16UC3 and CV_16UC4 are supported"); } } @@ -240,9 +255,15 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; + case CV_8UC4: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; default: CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, and CV_8UC3 are supported"); + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3 and CV_8UC4 are supported"); } } @@ -280,6 +301,12 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; + case CV_8UC4: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; case CV_16U: parallel_for_(cv::Range(0, srcImgs[0].rows), FastNlMeansMultiDenoisingInvoker( @@ -298,9 +325,15 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, h)); break; + case CV_16UC4: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; default: CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_16U, CV_16UC2, and CV_16UC3 are supported"); + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_8UC4, CV_16U, CV_16UC2, CV_16UC3 and CV_16UC4 are supported"); } } diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 468fa82f7..01588b03d 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -227,7 +227,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) } // calc weights - IT estimation[3], weights_sum = 0; + IT estimation[pixelInfo::channels], weights_sum = 0; for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) estimation[channel_num] = 0; diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index d55d93ce7..d77ca3e1f 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -110,6 +110,18 @@ class DistAbs } }; + template struct calcDist_, IT> + { + static inline IT f(const Vec a, const Vec b) + { + return + std::abs((IT)(a[0]-b[0])) + + std::abs((IT)(a[1]-b[1])) + + std::abs((IT)(a[2]-b[2])) + + std::abs((IT)(a[3]-b[3])); + } + }; + public: template static inline IT calcDist(const T a, const T b) { @@ -172,6 +184,18 @@ class DistSquared } }; + template struct calcDist_, IT> + { + static inline IT f(const Vec a, const Vec b) + { + return + (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]) + + (IT)(a[2]-b[2])*(IT)(a[2]-b[2]) + + (IT)(a[3]-b[3])*(IT)(a[3]-b[3]); + } + }; + template struct calcUpDownDist_ { static inline IT f(T a_up, T a_down, T b_up, T b_down) @@ -254,6 +278,17 @@ template struct incWithWeight_, IT> } }; +template struct incWithWeight_, IT> +{ + static inline void f(IT* estimation, IT weight, Vec p) + { + estimation[0] += weight * p[0]; + estimation[1] += weight * p[1]; + estimation[2] += weight * p[2]; + estimation[3] += weight * p[3]; + } +}; + template static inline void incWithWeight(IT* estimation, IT weight, T p) { @@ -291,6 +326,19 @@ template struct saturateCastFromArray_, IT } }; +template struct saturateCastFromArray_, IT> +{ + static inline Vec f(IT* estimation) + { + Vec res; + res[0] = saturate_cast(estimation[0]); + res[1] = saturate_cast(estimation[1]); + res[2] = saturate_cast(estimation[2]); + res[3] = saturate_cast(estimation[3]); + return res; + } +}; + template static inline T saturateCastFromArray(IT* estimation) { return saturateCastFromArray_::f(estimation); diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index 0a2bdd739..eb2078643 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -249,7 +249,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r // calc weights IT weights_sum = 0; - IT estimation[3]; + IT estimation[pixelInfo::channels]; for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) estimation[channel_num] = 0; From ac6771f975144c00c153431687dce6ecc45303cf Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 3 Mar 2015 01:20:33 +0100 Subject: [PATCH 043/171] Added test cases --- modules/photo/test/ocl/test_denoising.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/modules/photo/test/ocl/test_denoising.cpp b/modules/photo/test/ocl/test_denoising.cpp index 30dc680c8..4aba4b51e 100644 --- a/modules/photo/test/ocl/test_denoising.cpp +++ b/modules/photo/test/ocl/test_denoising.cpp @@ -48,12 +48,17 @@ PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool, bool) Border srcBorder = randomBorder(0, use_roi ? MAX_VALUE : 0); randomSubMat(src, src_roi, roiSize, srcBorder, type, 0, 255); if (use_image) { - ASSERT_TRUE(cn == 1 || cn == 2 || cn == 3); + ASSERT_TRUE(cn == 1 || cn == 2 || cn == 3 || cn == 4); if (cn == 2) { int from_to[] = { 0,0, 1,1 }; src_roi.create(roiSize, type); mixChannels(&image, 1, &src_roi, 1, from_to, 2); } + else if (cn == 4) { + int from_to[] = { 0,0, 1,1, 2,2, 1,3}; + src_roi.create(roiSize, type); + mixChannels(&image, 1, &src_roi, 1, from_to, 4); + } else image.copyTo(src_roi); } @@ -111,9 +116,9 @@ OCL_TEST_P(FastNlMeansDenoisingColored, Mat) } OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoising, - Combine(Values(1, 2, 3), Bool(), Bool())); + Combine(Values(1, 2, 3, 4), Bool(), Bool())); OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoisingAbs, - Combine(Values(1, 2, 3), Bool(), Bool())); + Combine(Values(1, 2, 3, 4), Bool(), Bool())); OCL_INSTANTIATE_TEST_CASE_P(Photo, FastNlMeansDenoisingColored, Combine(Values(3, 4), Bool(), Values(false))); From d56d04e41bfc88dd26aa4b9799e0f6922266183a Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 3 Mar 2015 01:34:29 +0100 Subject: [PATCH 044/171] Updated documentation --- modules/photo/include/opencv2/photo.hpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/modules/photo/include/opencv2/photo.hpp b/modules/photo/include/opencv2/photo.hpp index c25a35e6d..446e81750 100644 --- a/modules/photo/include/opencv2/photo.hpp +++ b/modules/photo/include/opencv2/photo.hpp @@ -119,7 +119,7 @@ CV_EXPORTS_W void inpaint( InputArray src, InputArray inpaintMask, with several computational optimizations. Noise expected to be a gaussian white noise -@param src Input 8-bit 1-channel, 2-channel or 3-channel image. +@param src Input 8-bit 1-channel, 2-channel, 3-channel or 4-channel image. @param dst Output image with the same size and type as src . @param templateWindowSize Size in pixels of the template patch that is used to compute weights. Should be odd. Recommended value 7 pixels @@ -144,7 +144,7 @@ with several computational optimizations. Noise expected to be a gaussian white noise. Uses squared sum of absolute value distances instead of sum of squared distances for weight calculation -@param src Input 8-bit or 16-bit 1-channel, 2-channel or 3-channel image. +@param src Input 8-bit or 16-bit 1-channel, 2-channel, 3-channel or 4-channel image. @param dst Output image with the same size and type as src . @param templateWindowSize Size in pixels of the template patch that is used to compute weights. Should be odd. Recommended value 7 pixels @@ -190,8 +190,9 @@ captured in small period of time. For example video. This version of the functio images or for manual manipulation with colorspaces. For more details see -@param srcImgs Input 8-bit 1-channel, 2-channel or 3-channel images sequence. All images should -have the same type and size. +@param srcImgs Input 8-bit 1-channel, 2-channel, 3-channel or +4-channel images sequence. All images should have the same type and +size. @param imgToDenoiseIndex Target image to denoise index in srcImgs sequence @param temporalWindowSize Number of surrounding images to use for target image denoising. Should be odd. Images from imgToDenoiseIndex - temporalWindowSize / 2 to @@ -220,9 +221,9 @@ details see squared sum of absolute value distances instead of sum of squared distances for weight calculation -@param srcImgs Input 8-bit or 16-bit 1-channel, 2-channel or 3-channel -images sequence. All images should -have the same type and size. +@param srcImgs Input 8-bit or 16-bit 1-channel, 2-channel, 3-channel +or 4-channel images sequence. All images should have the same type and +size. @param imgToDenoiseIndex Target image to denoise index in srcImgs sequence @param temporalWindowSize Number of surrounding images to use for target image denoising. Should be odd. Images from imgToDenoiseIndex - temporalWindowSize / 2 to From 69eae13ff3f6115b6716c2dc1927b679580f9ced Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 3 Mar 2015 03:02:44 +0100 Subject: [PATCH 045/171] Changed pointers from sample_t * to uchar *. Rescaling psz accordingly. --- .../src/fast_nlmeans_denoising_opencl.hpp | 7 +++-- modules/photo/src/opencl/nlmeans.cl | 29 ++++++++----------- 2 files changed, 16 insertions(+), 20 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index 41264045c..2fa11a351 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -89,13 +89,13 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, char buf[4][40]; String opts = format("-D OP_CALC_FASTNLMEANS -D TEMPLATE_SIZE=%d -D SEARCH_SIZE=%d" - " -D sample_t=%s -D pixel_t=%s -D int_t=%s" + " -D pixel_t=%s -D int_t=%s" " -D weight_t=%s -D sum_t=%s -D convert_sum_t=%s" " -D BLOCK_COLS=%d -D BLOCK_ROWS=%d" " -D CTA_SIZE=%d -D TEMPLATE_SIZE2=%d -D SEARCH_SIZE2=%d" " -D convert_int_t=%s -D cn=%d -D psz=%d -D convert_pixel_t=%s%s", templateWindowSize, searchWindowSize, - ocl::typeToStr(depth), ocl::typeToStr(type), ocl::typeToStr(CV_32SC(cn)), + ocl::typeToStr(type), ocl::typeToStr(CV_32SC(cn)), depth == CV_8U ? ocl::typeToStr(CV_32S) : "long", depth == CV_8U ? ocl::typeToStr(CV_32SC(cn)) : (sprintf(buf[0], "long%d", cn), buf[0]), @@ -103,7 +103,8 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, (sprintf(buf[1], "convert_long%d", cn), buf[1]), BLOCK_COLS, BLOCK_ROWS, ctaSize, templateWindowHalfWize, searchWindowHalfSize, - ocl::convertTypeStr(depth, CV_32S, cn, buf[2]), cn, cn == 3 ? 4 : cn, + ocl::convertTypeStr(depth, CV_32S, cn, buf[2]), cn, + (depth == CV_8U ? sizeof(uchar) : sizeof(ushort)) * (cn == 3 ? 4 : cn), ocl::convertTypeStr(CV_32S, depth, cn, buf[3]), abs ? " -D ABS" : ""); ocl::Kernel k("fastNlMeansDenoising", ocl::photo::nlmeans_oclsrc, opts); diff --git a/modules/photo/src/opencl/nlmeans.cl b/modules/photo/src/opencl/nlmeans.cl index 91b012354..11837a5fc 100644 --- a/modules/photo/src/opencl/nlmeans.cl +++ b/modules/photo/src/opencl/nlmeans.cl @@ -97,7 +97,7 @@ inline int calcDistUpDown(pixel_t down_value, pixel_t down_value_t, pixel_t up_v #define COND if (x == 0 && y == 0) -inline void calcFirstElementInRow(__global const sample_t * src, int src_step, int src_offset, +inline void calcFirstElementInRow(__global const uchar * src, int src_step, int src_offset, __local int * dists, int y, int x, int id, __global int * col_dists, __global int * up_col_dists) { @@ -129,8 +129,8 @@ inline void calcFirstElementInRow(__global const sample_t * src, int src_step, i dist += value; } - src_current = (__global const pixel_t *)((__global const sample_t *)src_current + src_step); - src_template = (__global const pixel_t *)((__global const sample_t *)src_template + src_step); + src_current = (__global const pixel_t *)((__global const uchar *)src_current + src_step); + src_template = (__global const pixel_t *)((__global const uchar *)src_template + src_step); } #pragma unroll @@ -142,7 +142,7 @@ inline void calcFirstElementInRow(__global const sample_t * src, int src_step, i } } -inline void calcElementInFirstRow(__global const sample_t * src, int src_step, int src_offset, +inline void calcElementInFirstRow(__global const uchar * src, int src_step, int src_offset, __local int * dists, int y, int x0, int x, int id, int first, __global int * col_dists, __global int * up_col_dists) { @@ -164,8 +164,8 @@ inline void calcElementInFirstRow(__global const sample_t * src, int src_step, i { col_dist += calcDist(src_current[0], src_template[0]); - src_current = (__global const pixel_t *)((__global const sample_t *)src_current + src_step); - src_template = (__global const pixel_t *)((__global const sample_t *)src_template + src_step); + src_current = (__global const pixel_t *)((__global const uchar *)src_current + src_step); + src_template = (__global const pixel_t *)((__global const uchar *)src_template + src_step); } dists[i] += col_dist - col_dists_current[first]; @@ -174,7 +174,7 @@ inline void calcElementInFirstRow(__global const sample_t * src, int src_step, i } } -inline void calcElement(__global const sample_t * src, int src_step, int src_offset, +inline void calcElement(__global const uchar * src, int src_step, int src_offset, __local int * dists, int y, int x0, int x, int id, int first, __global int * col_dists, __global int * up_col_dists) { @@ -207,9 +207,9 @@ inline void calcElement(__global const sample_t * src, int src_step, int src_off } } -inline void convolveWindow(__global const sample_t * src, int src_step, int src_offset, +inline void convolveWindow(__global const uchar * src, int src_step, int src_offset, __local int * dists, __global const int * almostDist2Weight, - __global sample_t * dst, int dst_step, int dst_offset, + __global uchar * dst, int dst_step, int dst_offset, int y, int x, int id, __local weight_t * weights_local, __local sum_t * weighted_sum_local, int almostTemplateWindowSizeSqBinShift) { @@ -255,9 +255,9 @@ inline void convolveWindow(__global const sample_t * src, int src_step, int src_ } } -__kernel void fastNlMeansDenoising(__global const sample_t * src, int src_step, int src_offset, - __global sample_t * dst, int dst_step, int dst_offset, int dst_rows, int dst_cols, - __global const int * almostDist2Weight, __global sample_t * buffer, +__kernel void fastNlMeansDenoising(__global const uchar * src, int src_step, int src_offset, + __global uchar * dst, int dst_step, int dst_offset, int dst_rows, int dst_cols, + __global const int * almostDist2Weight, __global uchar * buffer, int almostTemplateWindowSizeSqBinShift) { int block_x = get_group_id(0), nblocks_x = get_num_groups(0); @@ -277,11 +277,6 @@ __kernel void fastNlMeansDenoising(__global const sample_t * src, int src_step, __global int * col_dists = (__global int *)(buffer + block_data_start * sizeof(int)); __global int * up_col_dists = col_dists + SEARCH_SIZE_SQ * TEMPLATE_SIZE; - src_step /= sizeof(sample_t); - src_offset /= sizeof(sample_t); - dst_step /= sizeof(sample_t); - dst_offset /= sizeof(sample_t); - for (int y = y0; y < y1; ++y) for (int x = x0; x < x1; ++x) { From b229d6b7ac90b9084fb11247fe3a1cb833534a8f Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 08:05:52 -0500 Subject: [PATCH 046/171] Moved constants to cv:: namespace. --- modules/calib3d/src/rhorefc.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 8372d251a..c829bb15a 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -56,7 +56,15 @@ #include #include "rhorefc.h" -/* Defines */ + + + + +/* For the sake of cv:: namespace ONLY: */ +namespace cv{/* For C support, replace with extern "C" { */ + + +/* Constants */ const int MEM_ALIGN = 32; const size_t HSIZE = (3*3*sizeof(float)); const double MIN_DELTA_CHNG = 0.1; @@ -74,12 +82,6 @@ const double LM_GAIN_LO = 0.25; /* See sacLMGain(). */ const double LM_GAIN_HI = 0.75; /* See sacLMGain(). */ - - - -/* For the sake of cv:: namespace ONLY: */ -namespace cv{/* For C support, replace with extern "C" { */ - /* Data Structures */ struct RHO_HEST_REFC{ /** From a2affe70d923818fb1556366e70c263c1885cfac Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 11:40:44 -0500 Subject: [PATCH 047/171] Deleted last remnants of restrict keyword. --- modules/calib3d/src/rhorefc.cpp | 32 +++++++++++++++---------------- modules/calib3d/src/rhorefc.h | 34 ++++++++++++++------------------- 2 files changed, 30 insertions(+), 36 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index c829bb15a..4abbd4ec3 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -256,14 +256,14 @@ static inline unsigned sacCalcIterBound (double confidence, unsigned sampleSize, unsigned maxIterBound); static inline void hFuncRefC (float* packedPoints, float* H); -static inline void sacCalcJacobianErrors(const float* restrict H, - const float* restrict src, - const float* restrict dst, - const char* restrict inl, - unsigned N, - float (* restrict JtJ)[8], - float* restrict Jte, - float* restrict Sp); +static inline void sacCalcJacobianErrors(const float* H, + const float* src, + const float* dst, + const char* inl, + unsigned N, + float (* JtJ)[8], + float* Jte, + float* Sp); static inline float sacLMGain (const float* dH, const float* Jte, const float S, @@ -1935,14 +1935,14 @@ inline void RHO_HEST_REFC::refine(void){ * nevertheless be vectorized if need be. */ -static inline void sacCalcJacobianErrors(const float* restrict H, - const float* restrict src, - const float* restrict dst, - const char* restrict inl, - unsigned N, - float (* restrict JtJ)[8], - float* restrict Jte, - float* restrict Sp){ +static inline void sacCalcJacobianErrors(const float* H, + const float* src, + const float* dst, + const char* inl, + unsigned N, + float (* JtJ)[8], + float* Jte, + float* Sp){ unsigned i; float S; diff --git a/modules/calib3d/src/rhorefc.h b/modules/calib3d/src/rhorefc.h index e6c324dcc..9fcb3677b 100644 --- a/modules/calib3d/src/rhorefc.h +++ b/modules/calib3d/src/rhorefc.h @@ -57,12 +57,6 @@ /* Defines */ -/* C++ does not have the restrict keyword. */ -#ifdef restrict -#undef restrict -#endif -#define restrict - /* Flags */ #ifndef RHO_FLAG_NONE @@ -256,20 +250,20 @@ void rhoRefCFini(RHO_HEST_REFC* p); * 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. */ +unsigned rhoRefC(RHO_HEST_REFC* p, /* Homography estimation context. */ + const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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. */ From 0f6ea38eac79d0931eb1cd03b8c11b17a899aae4 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 11:42:04 -0500 Subject: [PATCH 048/171] Added OPENCV_ prefix to #include guard. --- modules/calib3d/src/rhorefc.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/calib3d/src/rhorefc.h b/modules/calib3d/src/rhorefc.h index 9fcb3677b..8ebc277ad 100644 --- a/modules/calib3d/src/rhorefc.h +++ b/modules/calib3d/src/rhorefc.h @@ -44,8 +44,8 @@ */ /* Include Guards */ -#ifndef __RHOREFC_H__ -#define __RHOREFC_H__ +#ifndef __OPENCV_RHOREFC_H__ +#define __OPENCV_RHOREFC_H__ From e1abc416cfc25ddeaf11d919fc10afdadbfa0670 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 11:48:41 -0500 Subject: [PATCH 049/171] Changed seeding in initialization. The call to rand()/random() is now a call to (unsigned)cv::theRNG(). --- modules/calib3d/src/rhorefc.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 4abbd4ec3..7b3082506 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -45,6 +45,7 @@ /* Includes */ #include +#include #include #include #include @@ -476,11 +477,7 @@ inline int RHO_HEST_REFC::initialize(void){ lm.tmp1 = NULL; lm.Jte = NULL; -#ifdef _WIN32 - fastSeed(rand()); -#else - fastSeed(random()); -#endif + fastSeed((unsigned)cv::theRNG()); int areAllAllocsSuccessful = ctrl.smpl && From 16f36a5fda13ffc313619e9b2b08c4224ad25995 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 11:55:36 -0500 Subject: [PATCH 050/171] Replaced division by reciprocal + multiply in a few places. --- modules/calib3d/src/rhorefc.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 7b3082506..2b5b79d98 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -1756,21 +1756,21 @@ static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates) * 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=1.0f/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=1.0f/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]; From bb01231990bf7b500c6564dacc46e44f761c00ff Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 13:06:36 -0500 Subject: [PATCH 051/171] Substituted the NaN check with OpenCV's implementation. --- modules/calib3d/src/rhorefc.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 2b5b79d98..a4a63dc6e 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -1202,7 +1202,8 @@ inline int RHO_HEST_REFC::isModelDegenerate(void){ 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 = f!=f;// Only NaN is not equal to itself. */ + degenerate = cvIsNaN(f); /* degenerate = 0; */ From f592321771296acf662dbdcf0032f2401887d764 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 22:57:40 -0500 Subject: [PATCH 052/171] Internal buffers converted to use OpenCV dynamic memory allocation. --- modules/calib3d/src/rhorefc.cpp | 281 ++++++++++++++++++-------------- 1 file changed, 159 insertions(+), 122 deletions(-) diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index a4a63dc6e..0aab9a8b7 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -158,7 +158,6 @@ struct RHO_HEST_REFC{ /* Levenberg-Marquardt Refinement */ struct{ - float* ws; /* Levenberg-Marqhard Workspace */ float (* JtJ)[8]; /* JtJ matrix */ float (* tmp1)[8]; /* Temporary 1 */ float* Jte; /* Jte vector */ @@ -169,8 +168,14 @@ struct RHO_HEST_REFC{ uint64_t s[2]; /* PRNG state */ } prng; + /* Memory Management */ + struct{ + cv::Mat perObj; + cv::Mat perRun; + } mem; + /* Initialized? */ - int init; + int initialized; /* Empty constructors and destructors */ @@ -202,6 +207,10 @@ struct RHO_HEST_REFC{ /* Methods to implement internals */ + inline void allocatePerObj(void); + inline void allocatePerRun(void); + inline void deallocatePerRun(void); + inline void deallocatePerObj(void); inline int initRun(void); inline void finiRun(void); inline int haveExtrinsicGuess(void); @@ -242,9 +251,6 @@ struct RHO_HEST_REFC{ * Prototypes for purely-computational code. */ -static inline void* almalloc(size_t nBytes); -static inline void alfree (void* ptr); - static inline void sacInitNonRand (double beta, unsigned start, unsigned N, @@ -382,36 +388,6 @@ unsigned rhoRefC(RHO_HEST_REFC* p, /* Homography estimation context. */ -/** - * 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] = (unsigned char)(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); - } -} /** * Constructor for RHO_HEST_REFC. @@ -419,7 +395,7 @@ static inline void alfree(void* ptr){ * Does nothing. True initialization is done by initialize(). */ -RHO_HEST_REFC::RHO_HEST_REFC() : init(0){ +RHO_HEST_REFC::RHO_HEST_REFC() : initialized(0){ } @@ -427,7 +403,7 @@ RHO_HEST_REFC::RHO_HEST_REFC() : init(0){ * Private copy constructor for RHO_HEST_REFC. Disabled. */ -RHO_HEST_REFC::RHO_HEST_REFC(const RHO_HEST_REFC&) : init(0){ +RHO_HEST_REFC::RHO_HEST_REFC(const RHO_HEST_REFC&) : initialized(0){ } @@ -436,7 +412,7 @@ RHO_HEST_REFC::RHO_HEST_REFC(const RHO_HEST_REFC&) : init(0){ */ RHO_HEST_REFC::~RHO_HEST_REFC(){ - if(init){ + if(initialized){ finalize(); } } @@ -458,37 +434,30 @@ RHO_HEST_REFC::~RHO_HEST_REFC(){ */ inline int RHO_HEST_REFC::initialize(void){ - ctrl.smpl = (unsigned*)almalloc(SMPL_SIZE*sizeof(*ctrl.smpl)); + initialized = 0; + + + allocatePerObj(); - curr.pkdPts = (float*) almalloc(SMPL_SIZE*2*2*sizeof(*curr.pkdPts)); - curr.H = (float*) almalloc(HSIZE); curr.inl = NULL; curr.numInl = 0; - best.H = (float*) almalloc(HSIZE); best.inl = NULL; best.numInl = 0; nr.size = 0; nr.beta = 0.0; - lm.ws = NULL; - lm.JtJ = NULL; - lm.tmp1 = NULL; - lm.Jte = NULL; fastSeed((unsigned)cv::theRNG()); - int areAllAllocsSuccessful = ctrl.smpl && - curr.H && - best.H && - curr.pkdPts; + int areAllAllocsSuccessful = !mem.perObj.empty(); if(!areAllAllocsSuccessful){ finalize(); }else{ - init = 1; + initialized = 1; } return areAllAllocsSuccessful; @@ -502,15 +471,10 @@ inline int RHO_HEST_REFC::initialize(void){ */ inline void RHO_HEST_REFC::finalize(void){ - if(init){ - alfree(ctrl.smpl); - alfree(curr.H); - alfree(best.H); - alfree(curr.pkdPts); + if(initialized){ + deallocatePerObj(); - memset(this, 0, sizeof(*this)); - - init = 0; + initialized = 0; } } @@ -651,6 +615,128 @@ unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ } +/** + * Allocate per-object dynamic storage. + * + * This includes aligned, fixed-size internal buffers, but excludes any buffers + * whose size cannot be determined ahead-of-time (before the number of matches + * is known). + * + * All buffer memory is allocated in one single shot, and all pointers are + * initialized. + */ + +inline void RHO_HEST_REFC::allocatePerObj(void){ + /* We have known sizes */ + size_t ctrl_smpl_sz = SMPL_SIZE*sizeof(*ctrl.smpl); + size_t curr_pkdPts_sz = SMPL_SIZE*2*2*sizeof(*curr.pkdPts); + size_t curr_H_sz = HSIZE; + size_t best_H_sz = HSIZE; + size_t lm_JtJ_sz = 8*8*sizeof(float); + size_t lm_tmp1_sz = 8*8*sizeof(float); + size_t lm_Jte_sz = 1*8*sizeof(float); + + /* We compute offsets */ + size_t total = 0; +#define MK_OFFSET(v) \ + size_t v ## _of = total; \ + total = alignSize(v ## _of + v ## _sz, MEM_ALIGN) + + MK_OFFSET(ctrl_smpl); + MK_OFFSET(curr_pkdPts); + MK_OFFSET(curr_H); + MK_OFFSET(best_H); + MK_OFFSET(lm_JtJ); + MK_OFFSET(lm_tmp1); + MK_OFFSET(lm_Jte); + +#undef MK_OFFSET + + /* Allocate dynamic memory managed by cv::Mat */ + mem.perObj.create(1, total + MEM_ALIGN, CV_8UC1); + + /* Extract aligned pointer */ + unsigned char* ptr = alignPtr(mem.perObj.data, MEM_ALIGN); + + /* Assign pointers */ + ctrl.smpl = (unsigned*) (ptr + ctrl_smpl_of); + curr.pkdPts = (float*) (ptr + curr_pkdPts_of); + curr.H = (float*) (ptr + curr_H_of); + best.H = (float*) (ptr + best_H_of); + lm.JtJ = (float(*)[8])(ptr + lm_JtJ_of); + lm.tmp1 = (float(*)[8])(ptr + lm_tmp1_of); + lm.Jte = (float*) (ptr + lm_Jte_of); +} + + +/** + * Allocate per-run dynamic storage. + * + * This includes storage that is proportional to the number of points, such as + * the inlier mask. + */ + +inline void RHO_HEST_REFC::allocatePerRun(void){ + /* We have known sizes */ + size_t best_inl_sz = arg.N; + size_t curr_inl_sz = arg.N; + + /* We compute offsets */ + size_t total = 0; +#define MK_OFFSET(v) \ + size_t v ## _of = total; \ + total = alignSize(v ## _of + v ## _sz, MEM_ALIGN) + + MK_OFFSET(best_inl); + MK_OFFSET(curr_inl); + +#undef MK_OFFSET + + /* Allocate dynamic memory managed by cv::Mat */ + mem.perRun.create(1, total + MEM_ALIGN, CV_8UC1); + + /* Extract aligned pointer */ + unsigned char* ptr = alignPtr(mem.perRun.data, MEM_ALIGN); + + /* Assign pointers */ + best.inl = (char*)(ptr + best_inl_of); + curr.inl = (char*)(ptr + curr_inl_of); +} + + +/** + * Deallocate per-run dynamic storage. + * + * Undoes the work by allocatePerRun(). + */ + +inline void RHO_HEST_REFC::deallocatePerRun(void){ + best.inl = NULL; + curr.inl = NULL; + + mem.perRun.release(); +} + + +/** + * Deallocate per-object dynamic storage. + * + * Undoes the work by allocatePerObj(). + */ + +inline void RHO_HEST_REFC::deallocatePerObj(void){ + ctrl.smpl = NULL; + curr.pkdPts = NULL; + curr.H = NULL; + best.H = NULL; + lm.JtJ = NULL; + lm.tmp1 = NULL; + lm.Jte = NULL; + + mem.perObj.release(); +} + + /** * Initialize SAC for a run given its arguments. * @@ -716,46 +802,18 @@ inline int RHO_HEST_REFC::initRun(void){ * 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. + * allocate the two masks. */ - best.inl = arg.inl ? arg.inl : (char*)almalloc(arg.N); - curr.inl = (char*)almalloc(arg.N); - - if(!curr.inl || !best.inl){ - return 0; - } + allocatePerRun(); memset(best.inl, 0, arg.N); memset(curr.inl, 0, arg.N); - /** - * LevMarq workspace alloc. - * - * Runs third, consists only in a few conditional mallocs. If malloc fails - * we wish to quit before doing any serious work. - */ - - if(isRefineEnabled() || isFinalRefineEnabled()){ - lm.ws = (float*)almalloc(2*8*8*sizeof(float) + 1*8*sizeof(float)); - if(!lm.ws){ - return 0; - } - - lm.JtJ = (float(*)[8])(lm.ws + 0*8*8); - lm.tmp1 = (float(*)[8])(lm.ws + 1*8*8); - lm.Jte = (float*) (lm.ws + 2*8*8); - }else{ - lm.ws = NULL; - } - /** * Reset scalar per-run state. * - * Runs fourth because there's no point in resetting/calculating a large + * Runs third because there's no point in resetting/calculating a large * number of fields if something in the above junk failed. */ @@ -801,32 +859,7 @@ inline int RHO_HEST_REFC::initRun(void){ */ inline void RHO_HEST_REFC::finiRun(void){ - /** - * 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(arg.inl){ - if(arg.inl == best.inl){ - alfree(curr.inl); - }else{ - alfree(best.inl); - } - }else{ - alfree(best.inl); - alfree(curr.inl); - } - - best.inl = NULL; - curr.inl = NULL; - - /** - * ₣ree the Levenberg-Marquardt workspace. - */ - - alfree(lm.ws); - lm.ws = NULL; + deallocatePerRun(); } /** @@ -1464,9 +1497,9 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ ctrl.phMax = best_n; ctrl.phNumInl = bestNumInl; arg.maxI = sacCalcIterBound(arg.cfd, - (double)ctrl.phNumInl/ctrl.phMax, - SMPL_SIZE, - arg.maxI); + (double)ctrl.phNumInl/ctrl.phMax, + SMPL_SIZE, + arg.maxI); } } @@ -1488,7 +1521,7 @@ inline void RHO_HEST_REFC::updateBounds(void){ inline void RHO_HEST_REFC::outputModel(void){ if(isBestModelGoodEnough()){ memcpy(arg.finalH, best.H, HSIZE); - if(arg.inl && arg.inl != best.inl){ + if(arg.inl){ memcpy(arg.inl, best.inl, arg.N); } }else{ @@ -1501,8 +1534,12 @@ inline void RHO_HEST_REFC::outputModel(void){ */ inline void RHO_HEST_REFC::outputZeroH(void){ - memset(arg.finalH, 0, HSIZE); - memset(arg.inl, 0, arg.N); + if(arg.finalH){ + memset(arg.finalH, 0, HSIZE); + } + if(arg.inl){ + memset(arg.inl, 0, arg.N); + } } /** From 408f93340a837764768d208bdd1de2c9f1574b77 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Tue, 3 Mar 2015 23:12:33 -0500 Subject: [PATCH 053/171] External interface converted to use OpenCV Ptr<> smart pointer. --- modules/calib3d/src/fundam.cpp | 8 +---- modules/calib3d/src/rhorefc.cpp | 54 ++++++++++++++------------------- modules/calib3d/src/rhorefc.h | 46 +++++++++++----------------- 3 files changed, 41 insertions(+), 67 deletions(-) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 5ec12161d..f5e88d7e9 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -303,7 +303,7 @@ static bool createAndRunRHORegistrator(double confidence, * initialized, used, then finalized. */ - RHO_HEST_REFC* p = rhoRefCInit(); + Ptr p = rhoRefCInit(); /** * Optional. Ideally, the context would survive across calls to @@ -339,12 +339,6 @@ static bool createAndRunRHORegistrator(double confidence, NULL, (float*)tmpH.data); - /** - * Cleanup. - */ - - rhoRefCFini(p); - /* Convert float homography to double precision. */ tmpH.convertTo(_H, CV_64FC1); diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rhorefc.cpp index 0aab9a8b7..90aa16ee9 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rhorefc.cpp @@ -298,12 +298,13 @@ static inline void sacSub8x1 (float* Hout, * @return A pointer to the context if successful; NULL if an error occured. */ -RHO_HEST_REFC* rhoRefCInit(void){ - RHO_HEST_REFC* p = new RHO_HEST_REFC; +Ptr rhoRefCInit(void){ + Ptr p = Ptr(new RHO_HEST_REFC); - if(!p->initialize()){ - delete p; - p = NULL; + if(p){ + if(!p->initialize()){ + p = Ptr((RHO_HEST_REFC*)NULL); + } } return p; @@ -314,7 +315,7 @@ RHO_HEST_REFC* rhoRefCInit(void){ * External access to non-randomness table resize. */ -int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta){ +int rhoRefCEnsureCapacity(Ptr p, unsigned N, double beta){ return p->sacEnsureCapacity(N, beta); } @@ -323,22 +324,11 @@ int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta){ * Seeds the internal PRNG with the given seed. */ -void rhoRefCSeed(RHO_HEST_REFC* p, unsigned long long seed){ +void rhoRefCSeed(Ptr p, unsigned long long seed){ p->fastSeed((uint64_t)seed); } -/** - * External access to context destructor. - * - * @param [in] p The initialized estimator context to finalize. - */ - -void rhoRefCFini(RHO_HEST_REFC* p){ - delete p; -} - - /** * Estimates the homography using the given context, matches and parameters to * PROSAC. @@ -368,20 +358,20 @@ void rhoRefCFini(RHO_HEST_REFC* p){ * inliers for acceptance was reached; 0 otherwise. */ -unsigned rhoRefC(RHO_HEST_REFC* p, /* Homography estimation context. */ - const float* src, /* Source points */ - const float* dst, /* Destination points */ - char* 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. */ +unsigned rhoRefC(Ptr p, /* Homography estimation context. */ + const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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. */ return p->rhoRefC(src, dst, inl, N, maxD, maxI, rConvg, cfd, minInl, beta, flags, guessH, finalH); } diff --git a/modules/calib3d/src/rhorefc.h b/modules/calib3d/src/rhorefc.h index 8ebc277ad..bcd9fba1e 100644 --- a/modules/calib3d/src/rhorefc.h +++ b/modules/calib3d/src/rhorefc.h @@ -50,6 +50,7 @@ /* Includes */ +#include @@ -96,7 +97,7 @@ typedef struct RHO_HEST_REFC RHO_HEST_REFC; * @return A pointer to the context if successful; NULL if an error occured. */ -RHO_HEST_REFC* rhoRefCInit(void); +Ptr rhoRefCInit(void); /** @@ -114,7 +115,7 @@ RHO_HEST_REFC* rhoRefCInit(void); * @return 0 if successful; non-zero if an error occured. */ -int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta); +int rhoRefCEnsureCapacity(Ptr p, unsigned N, double beta); @@ -129,18 +130,7 @@ int rhoRefCEnsureCapacity(RHO_HEST_REFC* p, unsigned N, double beta); * @param [in] seed The 64-bit integer seed. */ -void rhoRefCSeed(RHO_HEST_REFC* p, unsigned long long seed); - - - -/** - * 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); +void rhoRefCSeed(Ptr p, unsigned long long seed); /** @@ -250,20 +240,20 @@ void rhoRefCFini(RHO_HEST_REFC* p); * inliers for acceptance was reached; 0 otherwise. */ -unsigned rhoRefC(RHO_HEST_REFC* p, /* Homography estimation context. */ - const float* src, /* Source points */ - const float* dst, /* Destination points */ - char* 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. */ +unsigned rhoRefC(Ptr p, /* Homography estimation context. */ + const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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. */ From 736b42b3722a064ffdf48d2b6a954e5b4a40e94a Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Wed, 4 Mar 2015 01:43:13 -0500 Subject: [PATCH 054/171] Refactorings and renamings. - Deleted "RefC" from names of external-interface functions. - Renamed rhorefc.[cpp|hpp] to rho.[cpp|hpp] - Introduced RHO_HEST base class, from which RHO_HEST_REFC inherits. - rhoInit() currently only returns a Ptr, but in the future it will be allowed to return pointers to other derived classes, depending on the values returned by cv::checkHardwareSupport(). --- modules/calib3d/src/fundam.cpp | 8 +- modules/calib3d/src/{rhorefc.cpp => rho.cpp} | 295 +++++++++++++------ modules/calib3d/src/{rhorefc.h => rho.h} | 45 +-- 3 files changed, 236 insertions(+), 112 deletions(-) rename modules/calib3d/src/{rhorefc.cpp => rho.cpp} (90%) rename modules/calib3d/src/{rhorefc.h => rho.h} (88%) diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index f5e88d7e9..153f2a1e9 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -41,7 +41,7 @@ //M*/ #include "precomp.hpp" -#include "rhorefc.h" +#include "rho.h" #include namespace cv @@ -303,7 +303,7 @@ static bool createAndRunRHORegistrator(double confidence, * initialized, used, then finalized. */ - Ptr p = rhoRefCInit(); + Ptr p = rhoInit(); /** * Optional. Ideally, the context would survive across calls to @@ -311,7 +311,7 @@ static bool createAndRunRHORegistrator(double confidence, * to pay is marginally more computational work than strictly needed. */ - rhoRefCEnsureCapacity(p, npoints, beta); + rhoEnsureCapacity(p, npoints, beta); /** * The critical call. All parameters are heavily documented in rhorefc.h. @@ -324,7 +324,7 @@ static bool createAndRunRHORegistrator(double confidence, * this behaviour is too problematic. */ - result = !!rhoRefC(p, + result = !!rhoHest(p, (const float*)src.data, (const float*)dst.data, (char*) tempMask.data, diff --git a/modules/calib3d/src/rhorefc.cpp b/modules/calib3d/src/rho.cpp similarity index 90% rename from modules/calib3d/src/rhorefc.cpp rename to modules/calib3d/src/rho.cpp index 90aa16ee9..2289366bc 100644 --- a/modules/calib3d/src/rhorefc.cpp +++ b/modules/calib3d/src/rho.cpp @@ -55,7 +55,7 @@ #include #include #include -#include "rhorefc.h" +#include "rho.h" @@ -84,7 +84,162 @@ const double LM_GAIN_HI = 0.75; /* See sacLMGain(). */ /* Data Structures */ -struct RHO_HEST_REFC{ + +/** + * Base Struct for RHO algorithm. + * + * A RHO estimator has initialization, finalization, capacity, seeding and + * homography-estimation APIs that must be implemented. + */ + +struct RHO_HEST{ + /* This is a virtual base class; It should have a virtual destructor. */ + virtual ~RHO_HEST(){} + + /* External Interface Methods */ + + /** + * Initialization work. + * + * @return 0 if initialization is unsuccessful; non-zero otherwise. + */ + + virtual inline int initialize(void){return 1;} + + + /** + * Finalization work. + */ + + virtual inline void finalize(void){} + + /** + * Ensure that the estimator context's internal table for the 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] 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 unsuccessful; non-zero otherwise. + */ + + virtual inline int ensureCapacity(unsigned N, double beta){ + (void)N; + (void)beta; + + return 1; + } + + + /** + * Generates a random double uniformly distributed in the range [0, 1). + * + * The default implementation uses the xorshift128+ algorithm from + * Sebastiano Vigna. Further scramblings of Marsaglia's xorshift generators. + * CoRR, abs/1402.6246, 2014. + * http://vigna.di.unimi.it/ftp/papers/xorshiftplus.pdf + * + * Source roughly as given in + * http://en.wikipedia.org/wiki/Xorshift#Xorshift.2B + */ + + virtual inline double fastRandom(void){ + uint64_t x = prng.s[0]; + uint64_t y = prng.s[1]; + x ^= x << 23; // a + x ^= x >> 17; // b + x ^= y ^ (y >> 26); // c + prng.s[0] = y; + prng.s[1] = x; + uint64_t s = x + y; + + return s * 5.421010862427522e-20;/* 2^-64 */ + } + + + /** + * Seeds the context's PRNG. + * + * @param [in] seed A 64-bit unsigned integer seed. + */ + + virtual inline void fastSeed(uint64_t seed){ + int i; + + prng.s[0] = seed; + prng.s[1] = ~seed;/* Guarantees one of the elements will be non-zero. */ + + /** + * Escape from zero-land (see xorshift128+ paper). Approximately 20 + * iterations required according to the graph. + */ + + for(i=0;i<20;i++){ + fastRandom(); + } + } + + + /** + * Estimates the homography using the given context, matches and parameters to + * PROSAC. + * + * @param [in] src The pointer to the source points of the matches. + * Cannot be NULL. + * @param [in] dst The pointer to the destination points of the matches. + * Cannot be NULL. + * @param [out] inl The pointer to the output mask of inlier matches. + * 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. + */ + + virtual unsigned rhoHest(const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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) = 0; /* Final result. */ + + + + /* PRNG XORshift128+ */ + struct{ + uint64_t s[2]; /* PRNG state */ + } prng; +}; + + + +/** + * Generic C implementation of RHO algorithm. + */ + +struct RHO_HEST_REFC : RHO_HEST{ /** * Virtual Arguments. * @@ -163,11 +318,6 @@ struct RHO_HEST_REFC{ float* Jte; /* Jte vector */ } lm; - /* PRNG XORshift128+ */ - struct{ - uint64_t s[2]; /* PRNG state */ - } prng; - /* Memory Management */ struct{ cv::Mat perObj; @@ -188,9 +338,9 @@ struct RHO_HEST_REFC{ /* Methods to implement external interface */ inline int initialize(void); - inline int sacEnsureCapacity(unsigned N, double beta); inline void finalize(void); - unsigned rhoRefC(const float* src, /* Source points */ + inline int ensureCapacity(unsigned N, double beta); + unsigned rhoHest(const float* src, /* Source points */ const float* dst, /* Destination points */ char* inl, /* Inlier mask */ unsigned N, /* = src.length = dst.length = inl.length */ @@ -225,8 +375,6 @@ struct RHO_HEST_REFC{ inline void rndSmpl(unsigned sampleSize, unsigned* currentSample, unsigned dataSetSize); - inline double fastRandom(void); - inline void fastSeed(uint64_t seed); inline int isSampleDegenerate(void); inline void generateModel(void); inline int isModelDegenerate(void); @@ -298,15 +446,27 @@ static inline void sacSub8x1 (float* Hout, * @return A pointer to the context if successful; NULL if an error occured. */ -Ptr rhoRefCInit(void){ - Ptr p = Ptr(new RHO_HEST_REFC); +Ptr rhoInit(void){ + /* Select an optimized implementation of RHO here. */ +#if 1 + /** + * For now, only the generic C implementation is available. In the future, + * SSE2/AVX/AVX2/FMA/NEON versions may be added, and they will be selected + * depending on cv::checkHardwareSupport()'s return values. + */ + + Ptr p = Ptr(new RHO_HEST_REFC); +#endif + + /* Initialize it. */ if(p){ if(!p->initialize()){ - p = Ptr((RHO_HEST_REFC*)NULL); + p = Ptr((RHO_HEST*)NULL); } } + /* Return it. */ return p; } @@ -315,8 +475,8 @@ Ptr rhoRefCInit(void){ * External access to non-randomness table resize. */ -int rhoRefCEnsureCapacity(Ptr p, unsigned N, double beta){ - return p->sacEnsureCapacity(N, beta); +int rhoEnsureCapacity(Ptr p, unsigned N, double beta){ + return p->ensureCapacity(N, beta); } @@ -324,8 +484,8 @@ int rhoRefCEnsureCapacity(Ptr p, unsigned N, double beta){ * Seeds the internal PRNG with the given seed. */ -void rhoRefCSeed(Ptr p, unsigned long long seed){ - p->fastSeed((uint64_t)seed); +void rhoSeed(Ptr p, uint64_t seed){ + p->fastSeed(seed); } @@ -358,27 +518,37 @@ void rhoRefCSeed(Ptr p, unsigned long long seed){ * inliers for acceptance was reached; 0 otherwise. */ -unsigned rhoRefC(Ptr p, /* Homography estimation context. */ - const float* src, /* Source points */ - const float* dst, /* Destination points */ - char* 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. */ - return p->rhoRefC(src, dst, inl, N, maxD, maxI, rConvg, cfd, minInl, beta, +unsigned rhoHest(Ptr p, /* Homography estimation context. */ + const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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. */ + return p->rhoHest(src, dst, inl, N, maxD, maxI, rConvg, cfd, minInl, beta, flags, guessH, finalH); } + + + + + + + + +/*********************** RHO_HEST_REFC implementation **********************/ + /** * Constructor for RHO_HEST_REFC. * @@ -479,13 +649,13 @@ inline void RHO_HEST_REFC::finalize(void){ * 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. + * @return 0 if unsuccessful; non-zero otherwise. * * Reads: nr.* * Writes: nr.* */ -inline int RHO_HEST_REFC::sacEnsureCapacity(unsigned N, double beta){ +inline int RHO_HEST_REFC::ensureCapacity(unsigned N, double beta){ if(N == 0){ /* Clear. */ nr.tbl.clear(); @@ -516,9 +686,9 @@ inline int RHO_HEST_REFC::sacEnsureCapacity(unsigned N, double beta){ * @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. + * 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 16 bytes. May be NULL. + * Must be aligned to 4 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. @@ -536,7 +706,7 @@ inline int RHO_HEST_REFC::sacEnsureCapacity(unsigned N, double beta){ * inliers for acceptance was reached; 0 otherwise. */ -unsigned RHO_HEST_REFC::rhoRefC(const float* src, /* Source points */ +unsigned RHO_HEST_REFC::rhoHest(const float* src, /* Source points */ const float* dst, /* Destination points */ char* inl, /* Inlier mask */ unsigned N, /* = src.length = dst.length = inl.length */ @@ -784,7 +954,7 @@ inline int RHO_HEST_REFC::initRun(void){ * substruct and the sanity-checked N and beta arguments from above. */ - if(isNREnabled() && !sacEnsureCapacity(arg.N, arg.beta)){ + if(isNREnabled() && !ensureCapacity(arg.N, arg.beta)){ return 0; } @@ -1061,53 +1231,6 @@ inline void RHO_HEST_REFC::rndSmpl(unsigned sampleSize, } } -/** - * Generates a random double uniformly distributed in the range [0, 1). - * - * Uses xorshift128+ algorithm from - * Sebastiano Vigna. Further scramblings of Marsaglia's xorshift generators. - * CoRR, abs/1402.6246, 2014. - * http://vigna.di.unimi.it/ftp/papers/xorshiftplus.pdf - * - * Source roughly as given in - * http://en.wikipedia.org/wiki/Xorshift#Xorshift.2B - */ - -inline double RHO_HEST_REFC::fastRandom(void){ - uint64_t x = prng.s[0]; - uint64_t y = prng.s[1]; - x ^= x << 23; // a - x ^= x >> 17; // b - x ^= y ^ (y >> 26); // c - prng.s[0] = y; - prng.s[1] = x; - uint64_t s = x + y; - - return s * 5.421010862427522e-20;/* 2^-64 */ -} - -/** - * Seeds the PRNG. - * - * The seed should not be zero, since the state must be initialized to non-zero. - */ - -inline void RHO_HEST_REFC::fastSeed(uint64_t seed){ - int i; - - prng.s[0] = seed; - prng.s[1] = ~seed;/* Guarantees one of the elements will be non-zero. */ - - /** - * Escape from zero-land (see xorshift128+ paper). Approximately 20 - * iterations required according to the graph. - */ - - for(i=0;i<20;i++){ - fastRandom(); - } -} - /** * Checks whether the *sample* is degenerate prior to model generation. * - First, the extremely cheap numerical degeneracy test is run, which weeds diff --git a/modules/calib3d/src/rhorefc.h b/modules/calib3d/src/rho.h similarity index 88% rename from modules/calib3d/src/rhorefc.h rename to modules/calib3d/src/rho.h index bcd9fba1e..082a41603 100644 --- a/modules/calib3d/src/rhorefc.h +++ b/modules/calib3d/src/rho.h @@ -44,13 +44,14 @@ */ /* Include Guards */ -#ifndef __OPENCV_RHOREFC_H__ -#define __OPENCV_RHOREFC_H__ +#ifndef __OPENCV_RHO_H__ +#define __OPENCV_RHO_H__ /* Includes */ #include +#include @@ -84,8 +85,8 @@ namespace cv{ * Homography Estimation context. */ -struct RHO_HEST_REFC; -typedef struct RHO_HEST_REFC RHO_HEST_REFC; +struct RHO_HEST; +typedef struct RHO_HEST RHO_HEST; /* Functions */ @@ -97,7 +98,7 @@ typedef struct RHO_HEST_REFC RHO_HEST_REFC; * @return A pointer to the context if successful; NULL if an error occured. */ -Ptr rhoRefCInit(void); +Ptr rhoInit(void); /** @@ -112,10 +113,10 @@ Ptr rhoRefCInit(void); * 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. + * @return 0 if unsuccessful; non-zero otherwise. */ -int rhoRefCEnsureCapacity(Ptr p, unsigned N, double beta); +int rhoEnsureCapacity(Ptr p, unsigned N, double beta); @@ -130,7 +131,7 @@ int rhoRefCEnsureCapacity(Ptr p, unsigned N, double beta); * @param [in] seed The 64-bit integer seed. */ -void rhoRefCSeed(Ptr p, unsigned long long seed); +void rhoSeed(Ptr p, uint64_t seed); /** @@ -240,20 +241,20 @@ void rhoRefCSeed(Ptr p, unsigned long long seed); * inliers for acceptance was reached; 0 otherwise. */ -unsigned rhoRefC(Ptr p, /* Homography estimation context. */ - const float* src, /* Source points */ - const float* dst, /* Destination points */ - char* 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. */ +unsigned rhoHest(Ptr p, /* Homography estimation context. */ + const float* src, /* Source points */ + const float* dst, /* Destination points */ + char* 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. */ From 27fd810b6fdf7b86d2445acf72fac182944ba9cf Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Wed, 4 Mar 2015 02:09:59 -0500 Subject: [PATCH 055/171] Silenced build warnings on Windows. --- modules/calib3d/src/rho.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index 2289366bc..fdd81c37b 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -813,7 +813,7 @@ inline void RHO_HEST_REFC::allocatePerObj(void){ #undef MK_OFFSET /* Allocate dynamic memory managed by cv::Mat */ - mem.perObj.create(1, total + MEM_ALIGN, CV_8UC1); + mem.perObj.create(1, (int)(total + MEM_ALIGN), CV_8UC1); /* Extract aligned pointer */ unsigned char* ptr = alignPtr(mem.perObj.data, MEM_ALIGN); @@ -853,7 +853,7 @@ inline void RHO_HEST_REFC::allocatePerRun(void){ #undef MK_OFFSET /* Allocate dynamic memory managed by cv::Mat */ - mem.perRun.create(1, total + MEM_ALIGN, CV_8UC1); + mem.perRun.create(1, (int)(total + MEM_ALIGN), CV_8UC1); /* Extract aligned pointer */ unsigned char* ptr = alignPtr(mem.perRun.data, MEM_ALIGN); From 52e67c1a292832d919be1adbb37e3497b5b65f1d Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Wed, 4 Mar 2015 05:04:52 -0500 Subject: [PATCH 056/171] Whitespace & Doc fixes on lower half of rho.cpp. Spaced methods & functions more consistently, and started documenting which members does each method access directly or through its callers within RHO_HEST_REFC. --- modules/calib3d/src/rho.cpp | 75 +++++++++++++++++++++++++++++++++++-- 1 file changed, 71 insertions(+), 4 deletions(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index fdd81c37b..b690f541c 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -1514,10 +1514,12 @@ static inline double sacDesignSPRTTest(double delta, double epsilon, double t_M, * Design the SPRT test. Shorthand for * A = sprt(delta, epsilon, t_M, m_S); * - * Idempotent, reentrant, thread-safe. + * Idempotent. * - * Reads: eval.delta, eval.epsilon, eval.t_M, eval.m_S - * Writes: eval.A, eval.lambdaAccept, eval.lambdaReject + * Reads (direct): eval.delta, eval.epsilon, eval.t_M, eval.m_S + * Reads (callees): None. + * Writes (direct): eval.A, eval.lambdaReject, eval.lambdaAccept. + * Writes (callees): None. */ inline void RHO_HEST_REFC::designSPRTTest(void){ @@ -1528,6 +1530,14 @@ inline void RHO_HEST_REFC::designSPRTTest(void){ /** * Return whether the current model is the best model so far. + * + * @return Non-zero if this is the model with the most inliers seen so far; + * 0 otherwise. + * + * Reads (direct): curr.numInl, best.numInl + * Reads (callees): None. + * Writes (direct): None. + * Writes (callees): None. */ inline int RHO_HEST_REFC::isBestModel(void){ @@ -1538,6 +1548,14 @@ inline int RHO_HEST_REFC::isBestModel(void){ * 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. + * + * @return Non-zero if the current model is "good enough" to save; + * 0 otherwise. + * + * Reads (direct): best.numInl, arg.minInl + * Reads (callees): None. + * Writes (direct): None. + * Writes (callees): None. */ inline int RHO_HEST_REFC::isBestModelGoodEnough(void){ @@ -1547,6 +1565,13 @@ inline int RHO_HEST_REFC::isBestModelGoodEnough(void){ /** * Make current model new best model by swapping the homography, inlier mask * and count of inliers between the current and best models. + * + * Reads (direct): curr.H, curr.inl, curr.numInl, + * best.H, best.inl, best.numInl + * Reads (callees): None. + * Writes (direct): curr.H, curr.inl, curr.numInl, + * best.H, best.inl, best.numInl + * Writes (callees): None. */ inline void RHO_HEST_REFC::saveBestModel(void){ @@ -1586,6 +1611,12 @@ static inline void sacInitNonRand(double beta, /** * Optimize the stopping criterion to account for the non-randomness criterion * of PROSAC. + * + * Reads (direct): arg.N, best.numInl, nr.tbl, arg.inl, ctrl.phMax, + * ctrl.phNumInl, arg.cfd, arg.maxI + * Reads (callees): None. + * Writes (direct): arg.maxI, ctrl.phMax, ctrl.phNumInl + * Writes (callees): None. */ inline void RHO_HEST_REFC::nStarOptimize(void){ @@ -1603,7 +1634,7 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ best_n = test_n; bestNumInl = testNumInl; } - testNumInl -= !!arg.inl[test_n-1]; + testNumInl -= !!arg.inl[test_n-1];/* FIXME: Probable bug! */ } if(bestNumInl*ctrl.phMax > ctrl.phNumInl*best_n){ @@ -1618,6 +1649,11 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ /** * Classic RANSAC iteration bound based on largest # of inliers. + * + * Reads (direct): arg.maxI, arg.cfd, best.numInl, arg.N + * Reads (callees): None. + * Writes (direct): arg.maxI + * Writes (callees): None. */ inline void RHO_HEST_REFC::updateBounds(void){ @@ -1629,6 +1665,11 @@ inline void RHO_HEST_REFC::updateBounds(void){ /** * Ouput the best model so far to the output argument. + * + * Reads (direct): arg.finalH, best.H, arg.inl, best.inl, arg.N + * Reads (callees): arg.finalH, arg.inl, arg.N + * Writes (direct): arg.finalH, arg.inl + * Writes (callees): arg.finalH, arg.inl */ inline void RHO_HEST_REFC::outputModel(void){ @@ -1644,6 +1685,11 @@ inline void RHO_HEST_REFC::outputModel(void){ /** * Ouput a zeroed H to the output argument. + * + * Reads (direct): arg.finalH, arg.inl, arg.N + * Reads (callees): None. + * Writes (direct): arg.finalH, arg.inl + * Writes (callees): None. */ inline void RHO_HEST_REFC::outputZeroH(void){ @@ -1974,10 +2020,18 @@ static void hFuncRefC(float* packedPoints,/* Source (four x,y float coordinates) H[8]=1.0; } + /** * Returns whether refinement is possible. * * NB This is separate from whether it is *enabled*. + * + * @return 0 if refinement isn't possible, non-zero otherwise. + * + * Reads (direct): best.numInl + * Reads (callees): None. + * Writes (direct): None. + * Writes (callees): None. */ inline int RHO_HEST_REFC::canRefine(void){ @@ -1989,8 +2043,15 @@ inline int RHO_HEST_REFC::canRefine(void){ return best.numInl > (unsigned)SMPL_SIZE; } + /** * Refines the best-so-far homography (p->best.H). + * + * Reads (direct): best.H, arg.src, arg.dst, best.inl, arg.N, lm.JtJ, + * lm.Jte, lm.tmp1 + * Reads (callees): None. + * Writes (direct): best.H, lm.JtJ, lm.Jte, lm.tmp1 + * Writes (callees): None. */ inline void RHO_HEST_REFC::refine(void){ @@ -2070,6 +2131,7 @@ inline void RHO_HEST_REFC::refine(void){ } } + /** * Compute directly the JtJ, Jte and sum-of-squared-error for a given * homography and set of inliers. @@ -2224,6 +2286,7 @@ static inline void sacCalcJacobianErrors(const float* H, if(Sp){*Sp = S;} } + /** * Compute the Levenberg-Marquardt "gain" obtained by the given step dH. * @@ -2257,6 +2320,7 @@ static inline float sacLMGain(const float* dH, return fabs(dL) < FLT_EPSILON ? dS : dS / dL; } + /** * Cholesky decomposition on 8x8 real positive-definite matrix defined by its * lower-triangular half. Outputs L, the lower triangular part of the @@ -2307,6 +2371,7 @@ static inline int sacChol8x8Damped(const float (*A)[8], return 1; } + /** * Invert lower-triangular 8x8 matrix L into lower-triangular matrix M. * @@ -2513,6 +2578,7 @@ static inline void sacTRInv8x8(const float (*L)[8], */ } + /** * 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 @@ -2556,6 +2622,7 @@ static inline void sacTRISolve8x8(const float (*L)[8], dH[7] = L[7][7]*t[7]; } + /** * Subtract dH from H. */ From 305cff36e2c5a334821bd6e40eddd16ba304e6fe Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Thu, 5 Mar 2015 13:36:42 +0100 Subject: [PATCH 057/171] Changed from IT to int for distance calculation --- .../src/fast_nlmeans_denoising_invoker.hpp | 50 +++---- ...fast_nlmeans_denoising_invoker_commons.hpp | 124 +++++++++--------- .../fast_nlmeans_multi_denoising_invoker.hpp | 54 ++++---- 3 files changed, 114 insertions(+), 114 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 01588b03d..2ebf76af4 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -80,15 +80,15 @@ private: std::vector almost_dist2weight_; void calcDistSumsForFirstElementInRow( - int i, Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const; + int i, Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const; void calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, - Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const; + Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const; }; inline int getNearestPowerOf2(int value) @@ -128,8 +128,8 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( almost_template_window_size_sq_bin_shift_ = getNearestPowerOf2(template_window_size_sq); double almost_dist2actual_dist_multiplier = ((double)(1 << almost_template_window_size_sq_bin_shift_)) / template_window_size_sq; - IT max_dist = D::template maxDist(); - size_t almost_max_dist = (size_t)(max_dist / almost_dist2actual_dist_multiplier + 1); + int max_dist = D::template maxDist(); + int almost_max_dist = (int)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight_.resize(almost_max_dist); const double WEIGHT_THRESHOLD = 0.001; @@ -156,14 +156,14 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) int row_to = range.end - 1; // sums of cols anf rows for current pixel p - Array2d dist_sums(search_window_size_, search_window_size_); + Array2d dist_sums(search_window_size_, search_window_size_); // for lazy calc optimization (sum of cols for current pixel) - Array3d col_dist_sums(template_window_size_, search_window_size_, search_window_size_); + Array3d col_dist_sums(template_window_size_, search_window_size_, search_window_size_); int first_col_num = -1; // last elements of column sum (for each element in row) - Array3d up_col_dist_sums(src_.cols, search_window_size_, search_window_size_); + Array3d up_col_dist_sums(src_.cols, search_window_size_, search_window_size_); for (int i = row_from; i <= row_to; i++) { @@ -202,9 +202,9 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) for (int y = 0; y < search_window_size; y++) { - IT * dist_sums_row = dist_sums.row_ptr(y); - IT * col_dist_sums_row = col_dist_sums.row_ptr(first_col_num, y); - IT * up_col_dist_sums_row = up_col_dist_sums.row_ptr(j, y); + int * dist_sums_row = dist_sums.row_ptr(y); + int * col_dist_sums_row = col_dist_sums.row_ptr(first_col_num, y); + int * up_col_dist_sums_row = up_col_dist_sums.row_ptr(j, y); const T * b_up_ptr = extended_src_.ptr(start_by - template_window_half_size_ - 1 + y); const T * b_down_ptr = extended_src_.ptr(start_by + template_window_half_size_ + y); @@ -215,7 +215,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) dist_sums_row[x] -= col_dist_sums_row[x]; int bx = start_bx + x; - col_dist_sums_row[x] = up_col_dist_sums_row[x] + D::template calcUpDownDist(a_up, a_down, b_up_ptr[bx], b_down_ptr[bx]); + col_dist_sums_row[x] = up_col_dist_sums_row[x] + D::template calcUpDownDist(a_up, a_down, b_up_ptr[bx], b_down_ptr[bx]); dist_sums_row[x] += col_dist_sums_row[x]; up_col_dist_sums_row[x] = col_dist_sums_row[x]; @@ -234,10 +234,10 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) for (int y = 0; y < search_window_size_; y++) { const T* cur_row_ptr = extended_src_.ptr(border_size_ + search_window_y + y); - IT* dist_sums_row = dist_sums.row_ptr(y); + int* dist_sums_row = dist_sums.row_ptr(y); for (int x = 0; x < search_window_size_; x++) { - size_t almostAvgDist = (size_t)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_); + int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_; IT weight = almost_dist2weight_[almostAvgDist]; weights_sum += weight; @@ -257,9 +257,9 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) template inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( int i, - Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const + Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const { int j = 0; @@ -276,7 +276,7 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElem for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) for (int tx = -template_window_half_size_; tx <= template_window_half_size_; tx++) { - int dist = D::template calcDist(extended_src_, + int dist = D::template calcDist(extended_src_, border_size_ + i + ty, border_size_ + j + tx, border_size_ + start_y + ty, border_size_ + start_x + tx); @@ -291,9 +291,9 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElem template inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, - Array2d& dist_sums, - Array3d& col_dist_sums, - Array3d& up_col_dist_sums) const + Array2d& dist_sums, + Array3d& col_dist_sums, + Array3d& up_col_dist_sums) const { int ay = border_size_ + i; int ax = border_size_ + j + template_window_half_size_; @@ -312,7 +312,7 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementIn int by = start_by + y; int bx = start_bx + x; for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) - col_dist_sums[new_last_col_num][y][x] += D::template calcDist(extended_src_, ay + ty, ax, by + ty, bx); + col_dist_sums[new_last_col_num][y][x] += D::template calcDist(extended_src_, ay + ty, ax, by + ty, bx); dist_sums[y][x] += col_dist_sums[new_last_col_num][y][x]; up_col_dist_sums[j][y][x] = col_dist_sums[new_last_col_num][y][x]; diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index d77ca3e1f..dbb4c5eb3 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -83,63 +83,63 @@ template struct pixelInfo: public pixelInfo_ class DistAbs { - template struct calcDist_ + template struct calcDist_ { - static inline IT f(const T a, const T b) + static inline int f(const T a, const T b) { - return std::abs((IT)(a-b)); + return std::abs((int)(a-b)); } }; - template struct calcDist_, IT> + template struct calcDist_ > { - static inline IT f(const Vec a, const Vec b) + static inline int f(const Vec a, const Vec b) { - return std::abs((IT)(a[0]-b[0])) + std::abs((IT)(a[1]-b[1])); + return std::abs((int)(a[0]-b[0])) + std::abs((int)(a[1]-b[1])); } }; - template struct calcDist_, IT> + template struct calcDist_ > { - static inline IT f(const Vec a, const Vec b) + static inline int f(const Vec a, const Vec b) { return - std::abs((IT)(a[0]-b[0])) + - std::abs((IT)(a[1]-b[1])) + - std::abs((IT)(a[2]-b[2])); + std::abs((int)(a[0]-b[0])) + + std::abs((int)(a[1]-b[1])) + + std::abs((int)(a[2]-b[2])); } }; - template struct calcDist_, IT> + template struct calcDist_ > { - static inline IT f(const Vec a, const Vec b) + static inline int f(const Vec a, const Vec b) { return - std::abs((IT)(a[0]-b[0])) + - std::abs((IT)(a[1]-b[1])) + - std::abs((IT)(a[2]-b[2])) + - std::abs((IT)(a[3]-b[3])); + std::abs((int)(a[0]-b[0])) + + std::abs((int)(a[1]-b[1])) + + std::abs((int)(a[2]-b[2])) + + std::abs((int)(a[3]-b[3])); } }; public: - template static inline IT calcDist(const T a, const T b) + template static inline int calcDist(const T a, const T b) { - return calcDist_::f(a, b); + return calcDist_::f(a, b); } - template - static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) + template + static inline int calcDist(const Mat& m, int i1, int j1, int i2, int j2) { const T a = m.at(i1, j1); const T b = m.at(i2, j2); - return calcDist(a,b); + return calcDist(a,b); } - template - static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) + template + static inline int calcUpDownDist(T a_up, T a_down, T b_up, T b_down) { - return calcDist(a_down, b_down) - calcDist(a_up, b_up); + return calcDist(a_down, b_down) - calcDist(a_up, b_up); }; template @@ -148,93 +148,93 @@ public: return std::exp(-dist*dist / (h * h * pixelInfo::channels)); } - template + template static double maxDist() { - return (IT)pixelInfo::sampleMax() * (IT)pixelInfo::channels; + return (int)pixelInfo::sampleMax() * pixelInfo::channels; } }; class DistSquared { - template struct calcDist_ + template struct calcDist_ { - static inline IT f(const T a, const T b) + static inline int f(const T a, const T b) { - return (IT)(a-b) * (IT)(a-b); + return (int)(a-b) * (int)(a-b); } }; - template struct calcDist_, IT> + template struct calcDist_ > { - static inline IT f(const Vec a, const Vec b) + static inline int f(const Vec a, const Vec b) { - return (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + (IT)(a[1]-b[1])*(IT)(a[1]-b[1]); + return (int)(a[0]-b[0])*(int)(a[0]-b[0]) + (int)(a[1]-b[1])*(int)(a[1]-b[1]); } }; - template struct calcDist_, IT> + template struct calcDist_ > { - static inline IT f(const Vec a, const Vec b) + static inline int f(const Vec a, const Vec b) { return - (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + - (IT)(a[1]-b[1])*(IT)(a[1]-b[1]) + - (IT)(a[2]-b[2])*(IT)(a[2]-b[2]); + (int)(a[0]-b[0])*(int)(a[0]-b[0]) + + (int)(a[1]-b[1])*(int)(a[1]-b[1]) + + (int)(a[2]-b[2])*(int)(a[2]-b[2]); } }; - template struct calcDist_, IT> + template struct calcDist_ > { - static inline IT f(const Vec a, const Vec b) + static inline int f(const Vec a, const Vec b) { return - (IT)(a[0]-b[0])*(IT)(a[0]-b[0]) + - (IT)(a[1]-b[1])*(IT)(a[1]-b[1]) + - (IT)(a[2]-b[2])*(IT)(a[2]-b[2]) + - (IT)(a[3]-b[3])*(IT)(a[3]-b[3]); + (int)(a[0]-b[0])*(int)(a[0]-b[0]) + + (int)(a[1]-b[1])*(int)(a[1]-b[1]) + + (int)(a[2]-b[2])*(int)(a[2]-b[2]) + + (int)(a[3]-b[3])*(int)(a[3]-b[3]); } }; - template struct calcUpDownDist_ + template struct calcUpDownDist_ { - static inline IT f(T a_up, T a_down, T b_up, T b_down) + static inline int f(T a_up, T a_down, T b_up, T b_down) { - IT A = a_down - b_down; - IT B = a_up - b_up; + int A = a_down - b_down; + int B = a_up - b_up; return (A-B)*(A+B); } }; - template struct calcUpDownDist_, IT> + template struct calcUpDownDist_ > { private: typedef Vec T; public: - static inline IT f(T a_up, T a_down, T b_up, T b_down) + static inline int f(T a_up, T a_down, T b_up, T b_down) { - return calcDist(a_down, b_down) - calcDist(a_up, b_up); + return calcDist(a_down, b_down) - calcDist(a_up, b_up); } }; public: - template static inline IT calcDist(const T a, const T b) + template static inline int calcDist(const T a, const T b) { - return calcDist_::f(a, b); + return calcDist_::f(a, b); } - template - static inline IT calcDist(const Mat& m, int i1, int j1, int i2, int j2) + template + static inline int calcDist(const Mat& m, int i1, int j1, int i2, int j2) { const T a = m.at(i1, j1); const T b = m.at(i2, j2); - return calcDist(a,b); + return calcDist(a,b); } - template - static inline IT calcUpDownDist(T a_up, T a_down, T b_up, T b_down) + template + static inline int calcUpDownDist(T a_up, T a_down, T b_up, T b_down) { - return calcUpDownDist_::f(a_up, a_down, b_up, b_down); + return calcUpDownDist_::f(a_up, a_down, b_up, b_down); }; template @@ -243,11 +243,11 @@ public: return std::exp(-dist / (h * h * pixelInfo::channels)); } - template + template static double maxDist() { - return (IT)pixelInfo::sampleMax() * (IT)pixelInfo::sampleMax() * - (IT)pixelInfo::channels; + return (int)pixelInfo::sampleMax() * (int)pixelInfo::sampleMax() * + pixelInfo::channels; } }; diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index eb2078643..f1a334040 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -85,13 +85,13 @@ private: int almost_template_window_size_sq_bin_shift; std::vector almost_dist2weight; - void calcDistSumsForFirstElementInRow(int i, Array3d& dist_sums, - Array4d& col_dist_sums, - Array4d& up_col_dist_sums) const; + void calcDistSumsForFirstElementInRow(int i, Array3d& dist_sums, + Array4d& col_dist_sums, + Array4d& up_col_dist_sums) const; void calcDistSumsForElementInFirstRow(int i, int j, int first_col_num, - Array3d& dist_sums, Array4d& col_dist_sums, - Array4d& up_col_dist_sums) const; + Array3d& dist_sums, Array4d& col_dist_sums, + Array4d& up_col_dist_sums) const; }; template @@ -139,8 +139,8 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoke int almost_template_window_size_sq = 1 << almost_template_window_size_sq_bin_shift; double almost_dist2actual_dist_multiplier = (double) almost_template_window_size_sq / template_window_size_sq; - IT max_dist = D::template maxDist(); - int almost_max_dist = (int) (max_dist / almost_dist2actual_dist_multiplier + 1); + int max_dist = D::template maxDist(); + int almost_max_dist = (int)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight.resize(almost_max_dist); const double WEIGHT_THRESHOLD = 0.001; @@ -166,13 +166,13 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r int row_from = range.start; int row_to = range.end - 1; - Array3d dist_sums(temporal_window_size_, search_window_size_, search_window_size_); + Array3d dist_sums(temporal_window_size_, search_window_size_, search_window_size_); // for lazy calc optimization - Array4d col_dist_sums(template_window_size_, temporal_window_size_, search_window_size_, search_window_size_); + Array4d col_dist_sums(template_window_size_, temporal_window_size_, search_window_size_, search_window_size_); int first_col_num = -1; - Array4d up_col_dist_sums(cols_, temporal_window_size_, search_window_size_, search_window_size_); + Array4d up_col_dist_sums(cols_, temporal_window_size_, search_window_size_, search_window_size_); for (int i = row_from; i <= row_to; i++) { @@ -216,15 +216,15 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r for (int d = 0; d < temporal_window_size_; d++) { Mat cur_extended_src = extended_srcs_[d]; - Array2d cur_dist_sums = dist_sums[d]; - Array2d cur_col_dist_sums = col_dist_sums[first_col_num][d]; - Array2d cur_up_col_dist_sums = up_col_dist_sums[j][d]; + Array2d cur_dist_sums = dist_sums[d]; + Array2d cur_col_dist_sums = col_dist_sums[first_col_num][d]; + Array2d cur_up_col_dist_sums = up_col_dist_sums[j][d]; for (int y = 0; y < search_window_size; y++) { - IT* dist_sums_row = cur_dist_sums.row_ptr(y); + int* dist_sums_row = cur_dist_sums.row_ptr(y); - IT* col_dist_sums_row = cur_col_dist_sums.row_ptr(y); - IT* up_col_dist_sums_row = cur_up_col_dist_sums.row_ptr(y); + int* col_dist_sums_row = cur_col_dist_sums.row_ptr(y); + int* up_col_dist_sums_row = cur_up_col_dist_sums.row_ptr(y); const T* b_up_ptr = cur_extended_src.ptr(start_by - template_window_half_size_ - 1 + y); const T* b_down_ptr = cur_extended_src.ptr(start_by + template_window_half_size_ + y); @@ -234,7 +234,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r dist_sums_row[x] -= col_dist_sums_row[x]; col_dist_sums_row[x] = up_col_dist_sums_row[x] + - D::template calcUpDownDist(a_up, a_down, b_up_ptr[start_bx + x], b_down_ptr[start_bx + x]); + D::template calcUpDownDist(a_up, a_down, b_up_ptr[start_bx + x], b_down_ptr[start_bx + x]); dist_sums_row[x] += col_dist_sums_row[x]; up_col_dist_sums_row[x] = col_dist_sums_row[x]; @@ -260,11 +260,11 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r { const T* cur_row_ptr = esrc_d.ptr(border_size_ + search_window_y + y); - IT* dist_sums_row = dist_sums.row_ptr(d, y); + int* dist_sums_row = dist_sums.row_ptr(d, y); for (int x = 0; x < search_window_size_; x++) { - size_t almostAvgDist = (size_t)(dist_sums_row[x] >> almost_template_window_size_sq_bin_shift); + int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift; IT weight = almost_dist2weight[almostAvgDist]; weights_sum += weight; @@ -286,7 +286,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r template inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRow( - int i, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const + int i, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { int j = 0; @@ -303,14 +303,14 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirs int start_y = i + y - search_window_half_size_; int start_x = j + x - search_window_half_size_; - IT* dist_sums_ptr = &dist_sums[d][y][x]; - IT* col_dist_sums_ptr = &col_dist_sums[0][d][y][x]; + int* dist_sums_ptr = &dist_sums[d][y][x]; + int* col_dist_sums_ptr = &col_dist_sums[0][d][y][x]; int col_dist_sums_step = col_dist_sums.step_size(0); for (int tx = -template_window_half_size_; tx <= template_window_half_size_; tx++) { for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) { - IT dist = D::template calcDist( + int dist = D::template calcDist( main_extended_src_.at(border_size_ + i + ty, border_size_ + j + tx), cur_extended_src.at(border_size_ + start_y + ty, border_size_ + start_x + tx)); @@ -327,8 +327,8 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirs template inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRow( - int i, int j, int first_col_num, Array3d& dist_sums, - Array4d& col_dist_sums, Array4d& up_col_dist_sums) const + int i, int j, int first_col_num, Array3d& dist_sums, + Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { int ay = border_size_ + i; int ax = border_size_ + j + template_window_half_size_; @@ -350,10 +350,10 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElem int by = start_by + y; int bx = start_bx + x; - IT* col_dist_sums_ptr = &col_dist_sums[new_last_col_num][d][y][x]; + int* col_dist_sums_ptr = &col_dist_sums[new_last_col_num][d][y][x]; for (int ty = -template_window_half_size_; ty <= template_window_half_size_; ty++) { - *col_dist_sums_ptr += D::template calcDist( + *col_dist_sums_ptr += D::template calcDist( main_extended_src_.at(ay + ty, ax), cur_extended_src.at(by + ty, bx)); } From 18be52c05b5d3167c937976c146a392675c828fc Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Thu, 5 Mar 2015 13:55:06 +0100 Subject: [PATCH 058/171] Changed LUTs from IT to int --- .../src/fast_nlmeans_denoising_invoker.hpp | 13 +++++---- ...fast_nlmeans_denoising_invoker_commons.hpp | 28 +++++++++---------- .../fast_nlmeans_multi_denoising_invoker.hpp | 13 +++++---- 3 files changed, 28 insertions(+), 26 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 2ebf76af4..ec154fbe6 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -75,9 +75,9 @@ private: int template_window_half_size_; int search_window_half_size_; - IT fixed_point_mult_; + int fixed_point_mult_; int almost_template_window_size_sq_bin_shift_; - std::vector almost_dist2weight_; + std::vector almost_dist2weight_; void calcDistSumsForFirstElementInRow( int i, Array2d& dist_sums, @@ -119,7 +119,8 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const IT max_estimate_sum_value = (IT)search_window_size_ * (IT)search_window_size_ * (IT)pixelInfo::sampleMax(); - fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; + fixed_point_mult_ = (int)std::min(std::numeric_limits::max() / max_estimate_sum_value, + std::numeric_limits::max()); // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift @@ -136,7 +137,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * D::template calcWeight(dist, h)); + int weight = (int)round(fixed_point_mult_ * D::template calcWeight(dist, h)); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -238,8 +239,8 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) for (int x = 0; x < search_window_size_; x++) { int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_; - IT weight = almost_dist2weight_[almostAvgDist]; - weights_sum += weight; + int weight = almost_dist2weight_[almostAvgDist]; + weights_sum += (IT)weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; incWithWeight(estimation, weight, p); diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index dbb4c5eb3..4d66efe46 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -253,39 +253,39 @@ public: template struct incWithWeight_ { - static inline void f(IT* estimation, IT weight, T p) + static inline void f(IT* estimation, int weight, T p) { - estimation[0] += weight * p; + estimation[0] += (IT)weight * p; } }; template struct incWithWeight_, IT> { - static inline void f(IT* estimation, IT weight, Vec p) + static inline void f(IT* estimation, int weight, Vec p) { - estimation[0] += weight * p[0]; - estimation[1] += weight * p[1]; + estimation[0] += (IT)weight * p[0]; + estimation[1] += (IT)weight * p[1]; } }; template struct incWithWeight_, IT> { - static inline void f(IT* estimation, IT weight, Vec p) + static inline void f(IT* estimation, int weight, Vec p) { - estimation[0] += weight * p[0]; - estimation[1] += weight * p[1]; - estimation[2] += weight * p[2]; + estimation[0] += (IT)weight * p[0]; + estimation[1] += (IT)weight * p[1]; + estimation[2] += (IT)weight * p[2]; } }; template struct incWithWeight_, IT> { - static inline void f(IT* estimation, IT weight, Vec p) + static inline void f(IT* estimation, int weight, Vec p) { - estimation[0] += weight * p[0]; - estimation[1] += weight * p[1]; - estimation[2] += weight * p[2]; - estimation[3] += weight * p[3]; + estimation[0] += (IT)weight * p[0]; + estimation[1] += (IT)weight * p[1]; + estimation[2] += (IT)weight * p[2]; + estimation[3] += (IT)weight * p[3]; } }; diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index f1a334040..f9c1264b2 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -81,9 +81,9 @@ private: int search_window_half_size_; int temporal_window_half_size_; - IT fixed_point_mult_; + int fixed_point_mult_; int almost_template_window_size_sq_bin_shift; - std::vector almost_dist2weight; + std::vector almost_dist2weight; void calcDistSumsForFirstElementInRow(int i, Array3d& dist_sums, Array4d& col_dist_sums, @@ -127,7 +127,8 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoke main_extended_src_ = extended_srcs_[temporal_window_half_size_]; const IT max_estimate_sum_value = (IT)temporal_window_size_ * (IT)search_window_size_ * (IT)search_window_size_ * (IT)pixelInfo::sampleMax(); - fixed_point_mult_ = std::numeric_limits::max() / max_estimate_sum_value; + fixed_point_mult_ = (int)std::min(std::numeric_limits::max() / max_estimate_sum_value, + std::numeric_limits::max()); // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift @@ -147,7 +148,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoke for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - IT weight = (IT)round(fixed_point_mult_ * D::template calcWeight(dist, h)); + int weight = (int)round(fixed_point_mult_ * D::template calcWeight(dist, h)); if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) weight = 0; @@ -266,8 +267,8 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r { int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift; - IT weight = almost_dist2weight[almostAvgDist]; - weights_sum += weight; + int weight = almost_dist2weight[almostAvgDist]; + weights_sum += (IT)weight; T p = cur_row_ptr[border_size_ + search_window_x + x]; incWithWeight(estimation, weight, p); From c41efe4e303d51bf207bb54f60d2f4508acfe53d Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Thu, 5 Mar 2015 17:50:52 +0100 Subject: [PATCH 059/171] Refactoring in preparation for per-channel h-values --- modules/photo/src/denoising.cpp | 96 ++++++------ .../src/fast_nlmeans_denoising_invoker.hpp | 45 +++--- ...fast_nlmeans_denoising_invoker_commons.hpp | 147 +++++++++++++++--- .../fast_nlmeans_multi_denoising_invoker.hpp | 48 +++--- 4 files changed, 216 insertions(+), 120 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index b41f83ec9..29899f791 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -65,23 +65,23 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, switch (src.type()) { case CV_8U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC2: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC3: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC4: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; default: CV_Error(Error::StsBadArg, @@ -104,43 +104,43 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, switch (src.type()) { case CV_8U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC2: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC3: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC4: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16UC2: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs>( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16UC3: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs>( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + src, dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16UC4: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs>( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + src, dst, templateWindowSize, searchWindowSize, &h)); break; default: CV_Error(Error::StsBadArg, @@ -239,27 +239,27 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds { case CV_8U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC2: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC3: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC4: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; default: CV_Error(Error::StsBadArg, @@ -285,51 +285,51 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray { case CV_8U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC2: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC3: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_8UC4: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16UC2: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16UC3: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; case CV_16UC4: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h)); break; default: CV_Error(Error::StsBadArg, diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index ec154fbe6..9dea2a02f 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -50,13 +50,13 @@ using namespace cv; -template +template struct FastNlMeansDenoisingInvoker : public ParallelLoopBody { public: FastNlMeansDenoisingInvoker(const Mat& src, Mat& dst, - int template_window_size, int search_window_size, const float h); + int template_window_size, int search_window_size, const float *h); void operator() (const Range& range) const; @@ -77,7 +77,7 @@ private: int fixed_point_mult_; int almost_template_window_size_sq_bin_shift_; - std::vector almost_dist2weight_; + std::vector almost_dist2weight_; void calcDistSumsForFirstElementInRow( int i, Array2d& dist_sums, @@ -99,12 +99,12 @@ inline int getNearestPowerOf2(int value) return p; } -template -FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( +template +FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const Mat& src, Mat& dst, int template_window_size, int search_window_size, - const float h) : + const float *h) : src_(src), dst_(dst) { CV_Assert(src.channels() == pixelInfo::channels); @@ -133,25 +133,20 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( int almost_max_dist = (int)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight_.resize(almost_max_dist); - const double WEIGHT_THRESHOLD = 0.001; for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - int weight = (int)round(fixed_point_mult_ * D::template calcWeight(dist, h)); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) - weight = 0; - - almost_dist2weight_[almost_dist] = weight; + almost_dist2weight_[almost_dist] = + D::template calcWeight(dist, h, fixed_point_mult_); } - CV_Assert(almost_dist2weight_[0] == fixed_point_mult_); // additional optimization init end if (dst_.empty()) dst_ = Mat::zeros(src_.size(), src_.type()); } -template -void FastNlMeansDenoisingInvoker::operator() (const Range& range) const +template +void FastNlMeansDenoisingInvoker::operator() (const Range& range) const { int row_from = range.start; int row_to = range.end - 1; @@ -228,9 +223,9 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) } // calc weights - IT estimation[pixelInfo::channels], weights_sum = 0; + IT estimation[pixelInfo::channels], weights_sum[pixelInfo::channels]; for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = 0; + estimation[channel_num] = weights_sum[channel_num] = 0; for (int y = 0; y < search_window_size_; y++) { @@ -240,23 +235,23 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& range) { int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_; int weight = almost_dist2weight_[almostAvgDist]; - weights_sum += (IT)weight; - T p = cur_row_ptr[border_size_ + search_window_x + x]; - incWithWeight(estimation, weight, p); + incWithWeight(estimation, weights_sum, weight, p); } } for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = (static_cast(estimation[channel_num]) + weights_sum/2) / weights_sum; + estimation[channel_num] = + (static_cast(estimation[channel_num]) + weights_sum[channel_num]/2) / + weights_sum[channel_num]; dst_.at(i,j) = saturateCastFromArray(estimation); } } } -template -inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( +template +inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElementInRow( int i, Array2d& dist_sums, Array3d& col_dist_sums, @@ -289,8 +284,8 @@ inline void FastNlMeansDenoisingInvoker::calcDistSumsForFirstElem } } -template -inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( +template +inline void FastNlMeansDenoisingInvoker::calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, Array2d& dist_sums, Array3d& col_dist_sums, diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index 4d66efe46..53a6f5ed6 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -122,6 +122,36 @@ class DistAbs } }; + static const double WEIGHT_THRESHOLD = 0.001; + template struct calcWeight_ + { + static inline WT f(double dist, const float *h, int fixed_point_mult) + { + WT weight = (WT)round(fixed_point_mult * + std::exp(-dist*dist / (h[0]*h[0] * pixelInfo::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult) + weight = 0; + return weight; + } + }; + + template struct calcWeight_ > + { + static inline Vec f(double dist, const float *h, int fixed_point_mult) + { + Vec res; + for (int i=0; i::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult) + weight = 0; + res[i] = weight; + } + return res; + } + }; + public: template static inline int calcDist(const T a, const T b) { @@ -142,14 +172,14 @@ public: return calcDist(a_down, b_down) - calcDist(a_up, b_up); }; - template - static double calcWeight(double dist, double h) + template + static inline WT calcWeight(double dist, const float *h, int fixed_point_mult) { - return std::exp(-dist*dist / (h * h * pixelInfo::channels)); + return calcWeight_::f(dist, h, fixed_point_mult); } template - static double maxDist() + static inline double maxDist() { return (int)pixelInfo::sampleMax() * pixelInfo::channels; } @@ -217,6 +247,36 @@ class DistSquared } }; + static const double WEIGHT_THRESHOLD = 0.001; + template struct calcWeight_ + { + static inline WT f(double dist, const float *h, int fixed_point_mult) + { + WT weight = (WT)round(fixed_point_mult * + std::exp(-dist / (h[0]*h[0] * pixelInfo::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult) + weight = 0; + return weight; + } + }; + + template struct calcWeight_ > + { + static inline Vec f(double dist, const float *h, int fixed_point_mult) + { + Vec res; + for (int i=0; i::channels))); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult) + weight = 0; + res[i] = weight; + } + return res; + } + }; + public: template static inline int calcDist(const T a, const T b) { @@ -237,62 +297,111 @@ public: return calcUpDownDist_::f(a_up, a_down, b_up, b_down); }; - template - static double calcWeight(double dist, double h) + template + static inline WT calcWeight(double dist, const float *h, int fixed_point_mult) { - return std::exp(-dist / (h * h * pixelInfo::channels)); + return calcWeight_::f(dist, h, fixed_point_mult); } template - static double maxDist() + static inline double maxDist() { return (int)pixelInfo::sampleMax() * (int)pixelInfo::sampleMax() * pixelInfo::channels; } }; -template struct incWithWeight_ +template struct incWithWeight_ { - static inline void f(IT* estimation, int weight, T p) + static inline void f(IT* estimation, IT* weights_sum, WT weight, T p) { estimation[0] += (IT)weight * p; + weights_sum[0] += (IT)weight; } }; -template struct incWithWeight_, IT> +template struct incWithWeight_, IT, int> { - static inline void f(IT* estimation, int weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, int weight, Vec p) { estimation[0] += (IT)weight * p[0]; estimation[1] += (IT)weight * p[1]; + weights_sum[0] += (IT)weight; + weights_sum[1] += (IT)weight; } }; -template struct incWithWeight_, IT> +template struct incWithWeight_, IT, int> { - static inline void f(IT* estimation, int weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, int weight, Vec p) { estimation[0] += (IT)weight * p[0]; estimation[1] += (IT)weight * p[1]; estimation[2] += (IT)weight * p[2]; + weights_sum[0] += (IT)weight; + weights_sum[1] += (IT)weight; + weights_sum[2] += (IT)weight; } }; -template struct incWithWeight_, IT> +template struct incWithWeight_, IT, int> { - static inline void f(IT* estimation, int weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, int weight, Vec p) { estimation[0] += (IT)weight * p[0]; estimation[1] += (IT)weight * p[1]; estimation[2] += (IT)weight * p[2]; estimation[3] += (IT)weight * p[3]; + weights_sum[0] += (IT)weight; + weights_sum[1] += (IT)weight; + weights_sum[2] += (IT)weight; + weights_sum[3] += (IT)weight; } }; -template -static inline void incWithWeight(IT* estimation, IT weight, T p) +template struct incWithWeight_, IT, Vec > { - return incWithWeight_::f(estimation, weight, p); + static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) + { + estimation[0] += (IT)weight[0] * p[0]; + estimation[1] += (IT)weight[1] * p[1]; + weights_sum[0] += (IT)weight[0]; + weights_sum[1] += (IT)weight[1]; + } +}; + +template struct incWithWeight_, IT, Vec > +{ + static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) + { + estimation[0] += (IT)weight[0] * p[0]; + estimation[1] += (IT)weight[1] * p[1]; + estimation[2] += (IT)weight[2] * p[2]; + weights_sum[0] += (IT)weight[0]; + weights_sum[1] += (IT)weight[1]; + weights_sum[2] += (IT)weight[2]; + } +}; + +template struct incWithWeight_, IT, Vec > +{ + static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) + { + estimation[0] += (IT)weight[0] * p[0]; + estimation[1] += (IT)weight[1] * p[1]; + estimation[2] += (IT)weight[2] * p[2]; + estimation[3] += (IT)weight[3] * p[3]; + weights_sum[0] += (IT)weight[0]; + weights_sum[1] += (IT)weight[1]; + weights_sum[2] += (IT)weight[2]; + weights_sum[3] += (IT)weight[3]; + } +}; + +template +static inline void incWithWeight(IT* estimation, IT* weights_sum, IT weight, T p) +{ + return incWithWeight_::f(estimation, weights_sum, weight, p); } template struct saturateCastFromArray_ diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index f9c1264b2..489ee673f 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -50,14 +50,14 @@ using namespace cv; -template +template struct FastNlMeansMultiDenoisingInvoker : ParallelLoopBody { public: FastNlMeansMultiDenoisingInvoker(const std::vector& srcImgs, int imgToDenoiseIndex, int temporalWindowSize, Mat& dst, int template_window_size, - int search_window_size, const float h); + int search_window_size, const float *h); void operator() (const Range& range) const; @@ -83,7 +83,7 @@ private: int fixed_point_mult_; int almost_template_window_size_sq_bin_shift; - std::vector almost_dist2weight; + std::vector almost_dist2weight; void calcDistSumsForFirstElementInRow(int i, Array3d& dist_sums, Array4d& col_dist_sums, @@ -94,15 +94,15 @@ private: Array4d& up_col_dist_sums) const; }; -template -FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( +template +FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoker( const std::vector& srcImgs, int imgToDenoiseIndex, int temporalWindowSize, cv::Mat& dst, int template_window_size, int search_window_size, - const float h) : + const float *h) : dst_(dst), extended_srcs_(srcImgs.size()) { CV_Assert(srcImgs.size() > 0); @@ -144,25 +144,20 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingInvoke int almost_max_dist = (int)(max_dist / almost_dist2actual_dist_multiplier + 1); almost_dist2weight.resize(almost_max_dist); - const double WEIGHT_THRESHOLD = 0.001; for (int almost_dist = 0; almost_dist < almost_max_dist; almost_dist++) { double dist = almost_dist * almost_dist2actual_dist_multiplier; - int weight = (int)round(fixed_point_mult_ * D::template calcWeight(dist, h)); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult_) - weight = 0; - - almost_dist2weight[almost_dist] = weight; + almost_dist2weight[almost_dist] = + D::template calcWeight(dist, h, fixed_point_mult_); } - CV_Assert(almost_dist2weight[0] == fixed_point_mult_); // additional optimization init end if (dst_.empty()) dst_ = Mat::zeros(srcImgs[0].size(), srcImgs[0].type()); } -template -void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const +template +void FastNlMeansMultiDenoisingInvoker::operator() (const Range& range) const { int row_from = range.start; int row_to = range.end - 1; @@ -248,11 +243,9 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r } // calc weights - IT weights_sum = 0; - - IT estimation[pixelInfo::channels]; + IT estimation[pixelInfo::channels], weights_sum[pixelInfo::channels]; for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = 0; + estimation[channel_num] = weights_sum[channel_num] = 0; for (int d = 0; d < temporal_window_size_; d++) { @@ -268,25 +261,24 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Range& r int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift; int weight = almost_dist2weight[almostAvgDist]; - weights_sum += (IT)weight; - T p = cur_row_ptr[border_size_ + search_window_x + x]; - incWithWeight(estimation, weight, p); + incWithWeight(estimation, weights_sum, weight, p); } } } for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = (static_cast(estimation[channel_num]) + weights_sum / 2) / weights_sum; + estimation[channel_num] = + (static_cast(estimation[channel_num]) + weights_sum[channel_num] / 2) / + weights_sum[channel_num]; dst_.at(i,j) = saturateCastFromArray(estimation); - } } } -template -inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRow( +template +inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirstElementInRow( int i, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { int j = 0; @@ -326,8 +318,8 @@ inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForFirs } } -template -inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRow( +template +inline void FastNlMeansMultiDenoisingInvoker::calcDistSumsForElementInFirstRow( int i, int j, int first_col_num, Array3d& dist_sums, Array4d& col_dist_sums, Array4d& up_col_dist_sums) const { From 1e82a67cc4d082abe9437dd163314a543bd90232 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 6 Mar 2015 14:28:43 +0100 Subject: [PATCH 060/171] Additional refactoring --- .../src/fast_nlmeans_denoising_invoker.hpp | 17 ++-- ...fast_nlmeans_denoising_invoker_commons.hpp | 91 +++++++++++-------- .../fast_nlmeans_multi_denoising_invoker.hpp | 17 ++-- 3 files changed, 71 insertions(+), 54 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 9dea2a02f..ff35550df 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -75,7 +75,7 @@ private: int template_window_half_size_; int search_window_half_size_; - int fixed_point_mult_; + typename pixelInfo::sampleType fixed_point_mult_; int almost_template_window_size_sq_bin_shift_; std::vector almost_dist2weight_; @@ -120,7 +120,7 @@ FastNlMeansDenoisingInvoker::FastNlMeansDenoisingInvoker( const IT max_estimate_sum_value = (IT)search_window_size_ * (IT)search_window_size_ * (IT)pixelInfo::sampleMax(); fixed_point_mult_ = (int)std::min(std::numeric_limits::max() / max_estimate_sum_value, - std::numeric_limits::max()); + pixelInfo::sampleMax()); // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift @@ -223,9 +223,11 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& ra } // calc weights - IT estimation[pixelInfo::channels], weights_sum[pixelInfo::channels]; + IT estimation[pixelInfo::channels], weights_sum[pixelInfo::channels]; for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = weights_sum[channel_num] = 0; + estimation[channel_num] = 0; + for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) + weights_sum[channel_num] = 0; for (int y = 0; y < search_window_size_; y++) { @@ -240,11 +242,8 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& ra } } - for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = - (static_cast(estimation[channel_num]) + weights_sum[channel_num]/2) / - weights_sum[channel_num]; - + divByWeightsSum::channels, pixelInfo::channels>(estimation, + weights_sum); dst_.at(i,j) = saturateCastFromArray(estimation); } } diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index 53a6f5ed6..df8e4703e 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -122,11 +122,11 @@ class DistAbs } }; - static const double WEIGHT_THRESHOLD = 0.001; template struct calcWeight_ { - static inline WT f(double dist, const float *h, int fixed_point_mult) + static inline WT f(double dist, const float *h, WT fixed_point_mult) { + static const double WEIGHT_THRESHOLD = 0.001; WT weight = (WT)round(fixed_point_mult * std::exp(-dist*dist / (h[0]*h[0] * pixelInfo::channels))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult) @@ -137,17 +137,11 @@ class DistAbs template struct calcWeight_ > { - static inline Vec f(double dist, const float *h, int fixed_point_mult) + static inline Vec f(double dist, const float *h, ET fixed_point_mult) { Vec res; for (int i=0; i::channels))); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult) - weight = 0; - res[i] = weight; - } + res[i] = calcWeight(dist, &h[i], fixed_point_mult); return res; } }; @@ -247,11 +241,11 @@ class DistSquared } }; - static const double WEIGHT_THRESHOLD = 0.001; template struct calcWeight_ { static inline WT f(double dist, const float *h, int fixed_point_mult) { + static const double WEIGHT_THRESHOLD = 0.001; WT weight = (WT)round(fixed_point_mult * std::exp(-dist / (h[0]*h[0] * pixelInfo::channels))); if (weight < WEIGHT_THRESHOLD * fixed_point_mult) @@ -266,13 +260,7 @@ class DistSquared { Vec res; for (int i=0; i::channels))); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult) - weight = 0; - res[i] = weight; - } + res[i] = calcWeight(dist, &h[i], fixed_point_mult); return res; } }; @@ -320,48 +308,42 @@ template struct incWithWeight_ } }; -template struct incWithWeight_, IT, int> +template struct incWithWeight_, IT, WT> { - static inline void f(IT* estimation, IT* weights_sum, int weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, WT weight, Vec p) { estimation[0] += (IT)weight * p[0]; estimation[1] += (IT)weight * p[1]; weights_sum[0] += (IT)weight; - weights_sum[1] += (IT)weight; } }; -template struct incWithWeight_, IT, int> +template struct incWithWeight_, IT, WT> { - static inline void f(IT* estimation, IT* weights_sum, int weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, WT weight, Vec p) { estimation[0] += (IT)weight * p[0]; estimation[1] += (IT)weight * p[1]; estimation[2] += (IT)weight * p[2]; weights_sum[0] += (IT)weight; - weights_sum[1] += (IT)weight; - weights_sum[2] += (IT)weight; } }; -template struct incWithWeight_, IT, int> +template struct incWithWeight_, IT, WT> { - static inline void f(IT* estimation, IT* weights_sum, int weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, WT weight, Vec p) { estimation[0] += (IT)weight * p[0]; estimation[1] += (IT)weight * p[1]; estimation[2] += (IT)weight * p[2]; estimation[3] += (IT)weight * p[3]; weights_sum[0] += (IT)weight; - weights_sum[1] += (IT)weight; - weights_sum[2] += (IT)weight; - weights_sum[3] += (IT)weight; } }; -template struct incWithWeight_, IT, Vec > +template struct incWithWeight_, IT, Vec > { - static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) { estimation[0] += (IT)weight[0] * p[0]; estimation[1] += (IT)weight[1] * p[1]; @@ -370,9 +352,9 @@ template struct incWithWeight_, IT, Vec struct incWithWeight_, IT, Vec > +template struct incWithWeight_, IT, Vec > { - static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) { estimation[0] += (IT)weight[0] * p[0]; estimation[1] += (IT)weight[1] * p[1]; @@ -383,9 +365,9 @@ template struct incWithWeight_, IT, Vec struct incWithWeight_, IT, Vec > +template struct incWithWeight_, IT, Vec > { - static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) + static inline void f(IT* estimation, IT* weights_sum, Vec weight, Vec p) { estimation[0] += (IT)weight[0] * p[0]; estimation[1] += (IT)weight[1] * p[1]; @@ -404,6 +386,43 @@ static inline void incWithWeight(IT* estimation, IT* weights_sum, IT weight, T p return incWithWeight_::f(estimation, weights_sum, weight, p); } +template struct divByWeightsSum_ +{ + static inline void f(IT* estimation, IT* weights_sum); +}; + +template struct divByWeightsSum_ +{ + static inline void f(IT* estimation, IT* weights_sum) + { + estimation[0] = (static_cast(estimation[0]) + weights_sum[0]/2) / weights_sum[0]; + } +}; + +template struct divByWeightsSum_ +{ + static inline void f(IT* estimation, IT* weights_sum) + { + for (size_t i = 0; i < n; i++) + estimation[i] = (static_cast(estimation[i]) + weights_sum[0]/2) / weights_sum[0]; + } +}; + +template struct divByWeightsSum_ +{ + static inline void f(IT* estimation, IT* weights_sum) + { + for (size_t i = 0; i < n; i++) + estimation[i] = (static_cast(estimation[i]) + weights_sum[i]/2) / weights_sum[i]; + } +}; + +template +static inline void divByWeightsSum(IT* estimation, IT* weights_sum) +{ + return divByWeightsSum_::f(estimation, weights_sum); +} + template struct saturateCastFromArray_ { static inline T f(IT* estimation) diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index 489ee673f..cd3833a56 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -81,7 +81,7 @@ private: int search_window_half_size_; int temporal_window_half_size_; - int fixed_point_mult_; + typename pixelInfo::sampleType fixed_point_mult_; int almost_template_window_size_sq_bin_shift; std::vector almost_dist2weight; @@ -128,7 +128,7 @@ FastNlMeansMultiDenoisingInvoker::FastNlMeansMultiDenoisingIn const IT max_estimate_sum_value = (IT)temporal_window_size_ * (IT)search_window_size_ * (IT)search_window_size_ * (IT)pixelInfo::sampleMax(); fixed_point_mult_ = (int)std::min(std::numeric_limits::max() / max_estimate_sum_value, - std::numeric_limits::max()); + pixelInfo::sampleMax()); // precalc weight for every possible l2 dist between blocks // additional optimization of precalced weights to replace division(averaging) by binary shift @@ -243,9 +243,11 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Rang } // calc weights - IT estimation[pixelInfo::channels], weights_sum[pixelInfo::channels]; + IT estimation[pixelInfo::channels], weights_sum[pixelInfo::channels]; for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = weights_sum[channel_num] = 0; + estimation[channel_num] = 0; + for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) + weights_sum[channel_num] = 0; for (int d = 0; d < temporal_window_size_; d++) { @@ -267,11 +269,8 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Rang } } - for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) - estimation[channel_num] = - (static_cast(estimation[channel_num]) + weights_sum[channel_num] / 2) / - weights_sum[channel_num]; - + divByWeightsSum::channels, pixelInfo::channels>(estimation, + weights_sum); dst_.at(i,j) = saturateCastFromArray(estimation); } } From 441cd2234353767ccfbcf2cca6a71190fb1698c3 Mon Sep 17 00:00:00 2001 From: sanuj Date: Fri, 6 Mar 2015 19:14:07 +0530 Subject: [PATCH 061/171] Add documentation for solvePnP in calib3d --- modules/calib3d/include/opencv2/calib3d.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index d059eed94..e34b1852b 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -512,6 +512,16 @@ projections, as well as the camera matrix and the distortion coefficients. @note - An example of how to use solvePnP for planar augmented reality can be found at opencv_source_code/samples/python2/plane_ar.py + - If you are using Python: + - Numpy array slices won't work as input because solvePnP requires contiguous + arrays (enforced by the assertion using cv::Mat::checkVector() around line 55 of + modules/calib3d/src/solvepnp.cpp version 2.4.9) + - The P3P algorithm requires image points to be in an array of shape (N,1,2) due + to its calling of cv::undistortPoints (around line 75 of modules/calib3d/src/solvepnp.cpp version 2.4.9) + which requires 2-channel information. + - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of + it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints = + np.ascontiguousarray(D[:,:2]).reshape((N,1,2)) */ CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, From 41ffcc27dd0887ee5942a9d48761f6958df0f318 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 6 Mar 2015 15:06:11 +0100 Subject: [PATCH 062/171] Added support for h = 0.0 --- ...fast_nlmeans_denoising_invoker_commons.hpp | 30 +++++++++++-------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index df8e4703e..efd482f6b 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -126,11 +126,13 @@ class DistAbs { static inline WT f(double dist, const float *h, WT fixed_point_mult) { + double w = std::exp(-dist*dist / (h[0]*h[0] * pixelInfo::channels)); + if (std::isnan(w)) w = 1.0; // Handle h = 0.0 + static const double WEIGHT_THRESHOLD = 0.001; - WT weight = (WT)round(fixed_point_mult * - std::exp(-dist*dist / (h[0]*h[0] * pixelInfo::channels))); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult) - weight = 0; + WT weight = (WT)round(fixed_point_mult * w); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult) weight = 0; + return weight; } }; @@ -167,7 +169,8 @@ public: }; template - static inline WT calcWeight(double dist, const float *h, int fixed_point_mult) + static inline WT calcWeight(double dist, const float *h, + typename pixelInfo::sampleType fixed_point_mult) { return calcWeight_::f(dist, h, fixed_point_mult); } @@ -243,20 +246,22 @@ class DistSquared template struct calcWeight_ { - static inline WT f(double dist, const float *h, int fixed_point_mult) + static inline WT f(double dist, const float *h, WT fixed_point_mult) { + double w = std::exp(-dist / (h[0]*h[0] * pixelInfo::channels)); + if (std::isnan(w)) w = 1.0; // Handle h = 0.0 + static const double WEIGHT_THRESHOLD = 0.001; - WT weight = (WT)round(fixed_point_mult * - std::exp(-dist / (h[0]*h[0] * pixelInfo::channels))); - if (weight < WEIGHT_THRESHOLD * fixed_point_mult) - weight = 0; + WT weight = (WT)round(fixed_point_mult * w); + if (weight < WEIGHT_THRESHOLD * fixed_point_mult) weight = 0; + return weight; } }; template struct calcWeight_ > { - static inline Vec f(double dist, const float *h, int fixed_point_mult) + static inline Vec f(double dist, const float *h, ET fixed_point_mult) { Vec res; for (int i=0; i - static inline WT calcWeight(double dist, const float *h, int fixed_point_mult) + static inline WT calcWeight(double dist, const float *h, + typename pixelInfo::sampleType fixed_point_mult) { return calcWeight_::f(dist, h, fixed_point_mult); } From fcdbacdbb0625380ab58873eb4dc085bde9a1489 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Fri, 6 Mar 2015 09:15:00 -0500 Subject: [PATCH 063/171] Corrected initialization of smart pointer. --- modules/calib3d/src/rho.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index b690f541c..832b921c0 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -462,7 +462,7 @@ Ptr rhoInit(void){ /* Initialize it. */ if(p){ if(!p->initialize()){ - p = Ptr((RHO_HEST*)NULL); + p.release(); } } From 2113636d29ab0b87676832ed634370d0b558ffd5 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Fri, 6 Mar 2015 09:24:45 -0500 Subject: [PATCH 064/171] Made seed a constant. --- modules/calib3d/src/rho.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index 832b921c0..bb41e0630 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -609,7 +609,7 @@ inline int RHO_HEST_REFC::initialize(void){ nr.beta = 0.0; - fastSeed((unsigned)cv::theRNG()); + fastSeed(~0); int areAllAllocsSuccessful = !mem.perObj.empty(); From 9c432f4f75eefe7c5fd39dc0689a2398ce43e9b5 Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Fri, 6 Mar 2015 12:16:38 -0500 Subject: [PATCH 065/171] Silence Windows warnings. --- modules/calib3d/src/rho.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index bb41e0630..1de801720 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -609,7 +609,7 @@ inline int RHO_HEST_REFC::initialize(void){ nr.beta = 0.0; - fastSeed(~0); + fastSeed((uint64_t)~0); int areAllAllocsSuccessful = !mem.perObj.empty(); From 324fa26848f1020d125bd45e1fa5459c07fb092a Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 6 Mar 2015 19:07:13 +0100 Subject: [PATCH 066/171] Refactoring of OpenCL implementation --- modules/photo/src/denoising.cpp | 6 ++- .../src/fast_nlmeans_denoising_opencl.hpp | 38 ++++++++++++------- modules/photo/src/opencl/nlmeans.cl | 31 ++++++++------- 3 files changed, 44 insertions(+), 31 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 29899f791..30f638d4c 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -51,7 +51,8 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, Size src_size = _src.size(); CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes - ocl_fastNlMeansDenoising(_src, _dst, h, templateWindowSize, searchWindowSize, false)) + ocl_fastNlMeansDenoising(_src, _dst, &h, 1, + templateWindowSize, searchWindowSize, false)) Mat src = _src.getMat(); _dst.create(src_size, src.type()); @@ -95,7 +96,8 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, Size src_size = _src.size(); CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes - ocl_fastNlMeansDenoising(_src, _dst, h, templateWindowSize, searchWindowSize, true)) + ocl_fastNlMeansDenoising(_src, _dst, &h, 1, + templateWindowSize, searchWindowSize, true)) Mat src = _src.getMat(); _dst.create(src_size, src.type()); diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index 2fa11a351..a06dc6192 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -29,7 +29,7 @@ static int divUp(int a, int b) } template -static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindowSize, int templateWindowSize, FT h, int cn, +static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindowSize, int templateWindowSize, FT *h, int hn, int cn, int & almostTemplateWindowSizeSqBinShift, bool abs) { const WT maxEstimateSumValue = searchWindowSize * searchWindowSize * @@ -53,24 +53,32 @@ static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindow int maxDist = abs ? std::numeric_limits::max() * cn : std::numeric_limits::max() * std::numeric_limits::max() * cn; int almostMaxDist = (int)(maxDist / almostDist2ActualDistMultiplier + 1); - FT den = 1.0f / (h * h * cn); + FT den[4]; + CV_Assert(hn > 0 && hn <= 4); + for (int i=0; i 1 ? format("%d", hn).c_str() : "").c_str(), + depth == CV_8U ? ocl::convertTypeStr(CV_32S, CV_32S, hn, buf[0]) : + format("convert_long%s", hn > 1 ? format("%d", hn).c_str() : "").c_str(), depth == CV_8U ? ocl::typeToStr(CV_32SC(cn)) : - (sprintf(buf[0], "long%d", cn), buf[0]), + format("long%s", cn > 1 ? format("%d", cn).c_str() : "").c_str(), depth == CV_8U ? ocl::convertTypeStr(depth, CV_32S, cn, buf[1]) : - (sprintf(buf[1], "convert_long%d", cn), buf[1]), + format("convert_long%s", cn > 1 ? format("%d", cn).c_str() : "").c_str(), BLOCK_COLS, BLOCK_ROWS, ctaSize, templateWindowHalfWize, searchWindowHalfSize, ocl::convertTypeStr(depth, CV_32S, cn, buf[2]), cn, @@ -115,13 +127,13 @@ static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float h, if ((depth == CV_8U && !ocl_calcAlmostDist2Weight(almostDist2Weight, searchWindowSize, templateWindowSize, - h, cn, + h, hn, cn, almostTemplateWindowSizeSqBinShift, abs)) || (depth == CV_16U && !ocl_calcAlmostDist2Weight(almostDist2Weight, searchWindowSize, templateWindowSize, - h, cn, + h, hn, cn, almostTemplateWindowSizeSqBinShift, abs))) return false; diff --git a/modules/photo/src/opencl/nlmeans.cl b/modules/photo/src/opencl/nlmeans.cl index 11837a5fc..936aed6fa 100644 --- a/modules/photo/src/opencl/nlmeans.cl +++ b/modules/photo/src/opencl/nlmeans.cl @@ -20,9 +20,9 @@ #ifdef OP_CALC_WEIGHTS -__kernel void calcAlmostDist2Weight(__global int * almostDist2Weight, int almostMaxDist, +__kernel void calcAlmostDist2Weight(__global wlut_t * almostDist2Weight, int almostMaxDist, FT almostDist2ActualDistMultiplier, int fixedPointMult, - FT den, FT WEIGHT_THRESHOLD) + w_t den, FT WEIGHT_THRESHOLD) { int almostDist = get_global_id(0); @@ -30,14 +30,13 @@ __kernel void calcAlmostDist2Weight(__global int * almostDist2Weight, int almost { FT dist = almostDist * almostDist2ActualDistMultiplier; #ifdef ABS - int weight = convert_int_sat_rte(fixedPointMult * exp(-dist*dist * den)); + w_t w = exp((w_t)(-dist*dist) * den); #else - int weight = convert_int_sat_rte(fixedPointMult * exp(-dist * den)); + w_t w = exp((w_t)(-dist) * den); #endif - if (weight < WEIGHT_THRESHOLD * fixedPointMult) - weight = 0; - - almostDist2Weight[almostDist] = weight; + wlut_t weight = convert_wlut_t(fixedPointMult * (isnan(w) ? (w_t)1.0 : w)); + almostDist2Weight[almostDist] = + weight < WEIGHT_THRESHOLD * fixedPointMult ? (wlut_t)0 : weight; } } @@ -208,14 +207,14 @@ inline void calcElement(__global const uchar * src, int src_step, int src_offset } inline void convolveWindow(__global const uchar * src, int src_step, int src_offset, - __local int * dists, __global const int * almostDist2Weight, + __local int * dists, __global const wlut_t * almostDist2Weight, __global uchar * dst, int dst_step, int dst_offset, int y, int x, int id, __local weight_t * weights_local, __local sum_t * weighted_sum_local, int almostTemplateWindowSizeSqBinShift) { int sx = x - SEARCH_SIZE2, sy = y - SEARCH_SIZE2; - weight_t weights = 0; - sum_t weighted_sum = (sum_t)(0); + weight_t weights = (weight_t)0; + sum_t weighted_sum = (sum_t)0; for (int i = id; i < SEARCH_SIZE_SQ; i += CTA_SIZE) { @@ -223,10 +222,10 @@ inline void convolveWindow(__global const uchar * src, int src_step, int src_off sum_t src_value = convert_sum_t(*(__global const pixel_t *)(src + src_index)); int almostAvgDist = dists[i] >> almostTemplateWindowSizeSqBinShift; - int weight = almostDist2Weight[almostAvgDist]; + weight_t weight = convert_weight_t(almostDist2Weight[almostAvgDist]); - weights += (weight_t)weight; - weighted_sum += (sum_t)(weight) * src_value; + weights += weight; + weighted_sum += (sum_t)weight * src_value; } weights_local[id] = weights; @@ -251,13 +250,13 @@ inline void convolveWindow(__global const uchar * src, int src_step, int src_off weighted_sum_local[2] + weighted_sum_local[3]; weight_t weights_local_0 = weights_local[0] + weights_local[1] + weights_local[2] + weights_local[3]; - *(__global pixel_t *)(dst + dst_index) = convert_pixel_t(weighted_sum_local_0 / (sum_t)(weights_local_0)); + *(__global pixel_t *)(dst + dst_index) = convert_pixel_t(weighted_sum_local_0 / (sum_t)weights_local_0); } } __kernel void fastNlMeansDenoising(__global const uchar * src, int src_step, int src_offset, __global uchar * dst, int dst_step, int dst_offset, int dst_rows, int dst_cols, - __global const int * almostDist2Weight, __global uchar * buffer, + __global const wlut_t * almostDist2Weight, __global uchar * buffer, int almostTemplateWindowSizeSqBinShift) { int block_x = get_group_id(0), nblocks_x = get_num_groups(0); From cc8d94c6fc977d116beb81c6a50f123790d01bef Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 6 Mar 2015 20:43:55 +0100 Subject: [PATCH 067/171] Addition of per-channel h-values for fastNlMeansDenoising[Multi][Abs] --- modules/photo/include/opencv2/photo.hpp | 126 +++++++++- modules/photo/src/denoising.cpp | 221 ++++++++++++++++++ .../src/fast_nlmeans_denoising_invoker.hpp | 2 +- ...fast_nlmeans_denoising_invoker_commons.hpp | 2 +- .../fast_nlmeans_multi_denoising_invoker.hpp | 2 +- modules/photo/src/opencl/nlmeans.cl | 2 +- 6 files changed, 344 insertions(+), 11 deletions(-) diff --git a/modules/photo/include/opencv2/photo.hpp b/modules/photo/include/opencv2/photo.hpp index 446e81750..5e11333ee 100644 --- a/modules/photo/include/opencv2/photo.hpp +++ b/modules/photo/include/opencv2/photo.hpp @@ -138,6 +138,31 @@ parameter. CV_EXPORTS_W void fastNlMeansDenoising( InputArray src, OutputArray dst, float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); +/** @brief Perform image denoising using Non-local Means Denoising algorithm + with several computational +optimizations. Noise expected to be a gaussian white noise + +@param src Input 8-bit 1-channel, 2-channel, 3-channel or 4-channel image. +@param dst Output image with the same size and type as src . +@param templateWindowSize Size in pixels of the template patch that is used to compute weights. +Should be odd. Recommended value 7 pixels +@param searchWindowSize Size in pixels of the window that is used to compute weighted average for +given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater +denoising time. Recommended value 21 pixels +@param h Array of parameters regulating filter strength, one per +channel. Big h value perfectly removes noise but also removes image +details, smaller h value preserves details but also preserves some +noise + +This function expected to be applied to grayscale images. For colored images look at +fastNlMeansDenoisingColored. Advanced usage of this functions can be manual denoising of colored +image in different colorspaces. Such approach is used in fastNlMeansDenoisingColored by converting +image to CIELAB colorspace and then separately denoise L and AB components with different h +parameter. + */ +CV_EXPORTS_W void fastNlMeansDenoising( InputArray src, OutputArray dst, float *h, + int templateWindowSize = 7, int searchWindowSize = 21); + /** @brief Perform image denoising using Non-local Means Denoising algorithm with several computational optimizations. Noise expected to be a @@ -163,6 +188,33 @@ parameter. CV_EXPORTS_W void fastNlMeansDenoisingAbs( InputArray src, OutputArray dst, float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); +/** @brief Perform image denoising using Non-local Means Denoising +algorithm +with several computational optimizations. Noise expected to be a +gaussian white noise. Uses squared sum of absolute value distances +instead of sum of squared distances for weight calculation + +@param src Input 8-bit or 16-bit 1-channel, 2-channel, 3-channel or 4-channel image. +@param dst Output image with the same size and type as src . +@param templateWindowSize Size in pixels of the template patch that is used to compute weights. +Should be odd. Recommended value 7 pixels +@param searchWindowSize Size in pixels of the window that is used to compute weighted average for +given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater +denoising time. Recommended value 21 pixels +@param h Array of parameters regulating filter strength, one per +channel. Big h value perfectly removes noise but also removes image +details, smaller h value preserves details but also preserves some +noise + +This function expected to be applied to grayscale images. For colored images look at +fastNlMeansDenoisingColored. Advanced usage of this functions can be manual denoising of colored +image in different colorspaces. Such approach is used in fastNlMeansDenoisingColored by converting +image to CIELAB colorspace and then separately denoise L and AB components with different h +parameter. + */ +CV_EXPORTS_W void fastNlMeansDenoisingAbs( InputArray src, OutputArray dst, float *h, + int templateWindowSize = 7, int searchWindowSize = 21); + /** @brief Modification of fastNlMeansDenoising function for colored images @param src Input 8-bit 3-channel image. @@ -204,14 +256,73 @@ Should be odd. Recommended value 7 pixels @param searchWindowSize Size in pixels of the window that is used to compute weighted average for given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels -@param h Parameter regulating filter strength for luminance component. Bigger h value perfectly -removes noise but also removes image details, smaller h value preserves details but also preserves -some noise +@param h Parameter regulating filter strength. Bigger h value +perfectly removes noise but also removes image details, smaller h +value preserves details but also preserves some noise */ CV_EXPORTS_W void fastNlMeansDenoisingMulti( InputArrayOfArrays srcImgs, OutputArray dst, int imgToDenoiseIndex, int temporalWindowSize, float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); +/** @brief Modification of fastNlMeansDenoising function for images sequence where consequtive images have been +captured in small period of time. For example video. This version of the function is for grayscale +images or for manual manipulation with colorspaces. For more details see + + +@param srcImgs Input 8-bit 1-channel, 2-channel, 3-channel or +4-channel images sequence. All images should have the same type and +size. +@param imgToDenoiseIndex Target image to denoise index in srcImgs sequence +@param temporalWindowSize Number of surrounding images to use for target image denoising. Should +be odd. Images from imgToDenoiseIndex - temporalWindowSize / 2 to +imgToDenoiseIndex - temporalWindowSize / 2 from srcImgs will be used to denoise +srcImgs[imgToDenoiseIndex] image. +@param dst Output image with the same size and type as srcImgs images. +@param templateWindowSize Size in pixels of the template patch that is used to compute weights. +Should be odd. Recommended value 7 pixels +@param searchWindowSize Size in pixels of the window that is used to compute weighted average for +given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater +denoising time. Recommended value 21 pixels +@param h Array of parameters regulating filter strength, one for each +channel. Bigger h value perfectly removes noise but also removes image +details, smaller h value preserves details but also preserves some +noise + */ +CV_EXPORTS_W void fastNlMeansDenoisingMulti( InputArrayOfArrays srcImgs, OutputArray dst, + int imgToDenoiseIndex, int temporalWindowSize, + float *h , int templateWindowSize = 7, int searchWindowSize = 21); + +/** @brief Modification of fastNlMeansDenoising function for images +sequence where consequtive images have been captured in small period +of time. For example video. This version of the function is for +grayscale images or for manual manipulation with colorspaces. For more +details see +. Uses +squared sum of absolute value distances instead of sum of squared +distances for weight calculation + +@param srcImgs Input 8-bit or 16-bit 1-channel, 2-channel, 3-channel +or 4-channel images sequence. All images should have the same type and +size. +@param imgToDenoiseIndex Target image to denoise index in srcImgs sequence +@param temporalWindowSize Number of surrounding images to use for target image denoising. Should +be odd. Images from imgToDenoiseIndex - temporalWindowSize / 2 to +imgToDenoiseIndex - temporalWindowSize / 2 from srcImgs will be used to denoise +srcImgs[imgToDenoiseIndex] image. +@param dst Output image with the same size and type as srcImgs images. +@param templateWindowSize Size in pixels of the template patch that is used to compute weights. +Should be odd. Recommended value 7 pixels +@param searchWindowSize Size in pixels of the window that is used to compute weighted average for +given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater +denoising time. Recommended value 21 pixels +@param h Parameter regulating filter strength. Bigger h value +perfectly removes noise but also removes image details, smaller h +value preserves details but also preserves some noise + */ +CV_EXPORTS_W void fastNlMeansDenoisingMultiAbs( InputArrayOfArrays srcImgs, OutputArray dst, + int imgToDenoiseIndex, int temporalWindowSize, + float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); + /** @brief Modification of fastNlMeansDenoising function for images sequence where consequtive images have been captured in small period of time. For example video. This version of the function is for @@ -235,13 +346,14 @@ Should be odd. Recommended value 7 pixels @param searchWindowSize Size in pixels of the window that is used to compute weighted average for given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels -@param h Parameter regulating filter strength for luminance component. Bigger h value perfectly -removes noise but also removes image details, smaller h value preserves details but also preserves -some noise +@param h Array of parameters regulating filter strength, one for each +channel. Bigger h value perfectly removes noise but also removes image +details, smaller h value preserves details but also preserves some +noise */ CV_EXPORTS_W void fastNlMeansDenoisingMultiAbs( InputArrayOfArrays srcImgs, OutputArray dst, int imgToDenoiseIndex, int temporalWindowSize, - float h = 3, int templateWindowSize = 7, int searchWindowSize = 21); + float *h, int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Modification of fastNlMeansDenoisingMulti function for colored images sequences diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 30f638d4c..9f63254b0 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -90,6 +90,51 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, } } +void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float *h, + int templateWindowSize, int searchWindowSize) +{ + Size src_size = _src.size(); + CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && + src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes + ocl_fastNlMeansDenoising(_src, _dst, h, CV_MAT_CN(_src.type()), + templateWindowSize, searchWindowSize, false)) + + Mat src = _src.getMat(); + _dst.create(src_size, src.type()); + Mat dst = _dst.getMat(); + +#ifdef HAVE_TEGRA_OPTIMIZATION + if(tegra::fastNlMeansDenoising(src, dst, h, templateWindowSize, searchWindowSize)) + return; +#endif + + switch (src.type()) { + case CV_8U: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC2: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC3: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC4: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + default: + CV_Error(Error::StsBadArg, + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3 and CV_8UC4 are supported"); + } +} + void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, int templateWindowSize, int searchWindowSize) { @@ -150,6 +195,66 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, } } +void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float *h, + int templateWindowSize, int searchWindowSize) +{ + Size src_size = _src.size(); + CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && + src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes + ocl_fastNlMeansDenoising(_src, _dst, h, CV_MAT_CN(_src.type()), + templateWindowSize, searchWindowSize, true)) + + Mat src = _src.getMat(); + _dst.create(src_size, src.type()); + Mat dst = _dst.getMat(); + + switch (src.type()) { + case CV_8U: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC2: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC3: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC4: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16U: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC2: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC3: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC4: + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( + src, dst, templateWindowSize, searchWindowSize, h)); + break; + default: + CV_Error(Error::StsBadArg, + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_8UC4, CV_16U, CV_16UC2, CV_16UC3 and CV_16UC4 are supported"); + } +} + void cv::fastNlMeansDenoisingColored( InputArray _src, OutputArray _dst, float h, float hForColorComponents, int templateWindowSize, int searchWindowSize) @@ -269,6 +374,52 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds } } +void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _dst, + int imgToDenoiseIndex, int temporalWindowSize, + float *h, int templateWindowSize, int searchWindowSize) +{ + std::vector srcImgs; + _srcImgs.getMatVector(srcImgs); + + fastNlMeansDenoisingMultiCheckPreconditions( + srcImgs, imgToDenoiseIndex, + temporalWindowSize, templateWindowSize, searchWindowSize); + + _dst.create(srcImgs[0].size(), srcImgs[0].type()); + Mat dst = _dst.getMat(); + + switch (srcImgs[0].type()) + { + case CV_8U: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC2: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC3: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC4: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + default: + CV_Error(Error::StsBadArg, + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3 and CV_8UC4 are supported"); + } +} + void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray _dst, int imgToDenoiseIndex, int temporalWindowSize, float h, int templateWindowSize, int searchWindowSize) @@ -339,6 +490,76 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray } } +void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray _dst, + int imgToDenoiseIndex, int temporalWindowSize, + float *h, int templateWindowSize, int searchWindowSize) +{ + std::vector srcImgs; + _srcImgs.getMatVector(srcImgs); + + fastNlMeansDenoisingMultiCheckPreconditions( + srcImgs, imgToDenoiseIndex, + temporalWindowSize, templateWindowSize, searchWindowSize); + + _dst.create(srcImgs[0].size(), srcImgs[0].type()); + Mat dst = _dst.getMat(); + + switch (srcImgs[0].type()) + { + case CV_8U: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC2: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC3: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_8UC4: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16U: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC2: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC3: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + case CV_16UC4: + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, h)); + break; + default: + CV_Error(Error::StsBadArg, + "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_8UC4, CV_16U, CV_16UC2, CV_16UC3 and CV_16UC4 are supported"); + } +} + void cv::fastNlMeansDenoisingColoredMulti( InputArrayOfArrays _srcImgs, OutputArray _dst, int imgToDenoiseIndex, int temporalWindowSize, float h, float hForColorComponents, diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index ff35550df..6e74acf03 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -236,7 +236,7 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& ra for (int x = 0; x < search_window_size_; x++) { int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift_; - int weight = almost_dist2weight_[almostAvgDist]; + WT weight = almost_dist2weight_[almostAvgDist]; T p = cur_row_ptr[border_size_ + search_window_x + x]; incWithWeight(estimation, weights_sum, weight, p); } diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index efd482f6b..9833ea7d3 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -387,7 +387,7 @@ template struct incWithWeight_ -static inline void incWithWeight(IT* estimation, IT* weights_sum, IT weight, T p) +static inline void incWithWeight(IT* estimation, IT* weights_sum, WT weight, T p) { return incWithWeight_::f(estimation, weights_sum, weight, p); } diff --git a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp index cd3833a56..3f13f400d 100644 --- a/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_multi_denoising_invoker.hpp @@ -262,7 +262,7 @@ void FastNlMeansMultiDenoisingInvoker::operator() (const Rang { int almostAvgDist = dist_sums_row[x] >> almost_template_window_size_sq_bin_shift; - int weight = almost_dist2weight[almostAvgDist]; + WT weight = almost_dist2weight[almostAvgDist]; T p = cur_row_ptr[border_size_ + search_window_x + x]; incWithWeight(estimation, weights_sum, weight, p); } diff --git a/modules/photo/src/opencl/nlmeans.cl b/modules/photo/src/opencl/nlmeans.cl index 936aed6fa..879665f48 100644 --- a/modules/photo/src/opencl/nlmeans.cl +++ b/modules/photo/src/opencl/nlmeans.cl @@ -36,7 +36,7 @@ __kernel void calcAlmostDist2Weight(__global wlut_t * almostDist2Weight, int alm #endif wlut_t weight = convert_wlut_t(fixedPointMult * (isnan(w) ? (w_t)1.0 : w)); almostDist2Weight[almostDist] = - weight < WEIGHT_THRESHOLD * fixedPointMult ? (wlut_t)0 : weight; + weight < (wlut_t)(WEIGHT_THRESHOLD * fixedPointMult) ? (wlut_t)0 : weight; } } From 21160137d4a8eaae0be2c2545ab7e18cd3bfc7a3 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Fri, 6 Mar 2015 20:44:31 +0100 Subject: [PATCH 068/171] Addition of test cases --- modules/photo/test/ocl/test_denoising.cpp | 50 +++++++++++++++++++---- 1 file changed, 43 insertions(+), 7 deletions(-) diff --git a/modules/photo/test/ocl/test_denoising.cpp b/modules/photo/test/ocl/test_denoising.cpp index 4aba4b51e..3b6998f06 100644 --- a/modules/photo/test/ocl/test_denoising.cpp +++ b/modules/photo/test/ocl/test_denoising.cpp @@ -16,7 +16,7 @@ namespace ocl { PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool, bool) { int cn, templateWindowSize, searchWindowSize; - float h; + float h[4]; bool use_roi, use_image; TEST_DECLARE_INPUT_PARAMETER(src); @@ -30,7 +30,10 @@ PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool, bool) templateWindowSize = 7; searchWindowSize = 21; - h = 3.0f; + + ASSERT_TRUE(cn > 0 && cn <= 4); + for (int i=0; i Date: Sat, 7 Mar 2015 12:53:32 +0100 Subject: [PATCH 069/171] Changes to be committed: (use "git reset HEAD ..." to unstage) modified: modules/java/generator/gen_java.py modified: modules/videoio/include/opencv2/videoio.hpp modified: modules/videoio/include/opencv2/videoio/videoio_c.h modified: modules/videoio/src/cap_dshow.cpp modified: modules/videoio/src/cap_pvapi.cpp Following changes have been made: 1. Some minor bugs have been removed. 2. In the PvAPI module the option CAP_PROP_MONOCROME has been removed because this option does not make sense and causes an error if a color camera is used. 3. Instead the new option CAP_PROP_PVAPI_PIXELFORMAT has been added which allows to activate the different pixel formats (color modes) of an AVT camera. 4. Since there were two identical defines CAP_PROP_MONOCROME = 19 CAP_PROP_MONOCHROME = 19 which were also used in the other module DSHOW, the first one with an orthographic error has been removed in favor of the second one. --- modules/java/generator/gen_java.py | 2 +- modules/videoio/include/opencv2/videoio.hpp | 17 +- .../include/opencv2/videoio/videoio_c.h | 2 +- modules/videoio/src/cap_dshow.cpp | 6 +- modules/videoio/src/cap_pvapi.cpp | 146 ++++++++++-------- 5 files changed, 104 insertions(+), 69 deletions(-) diff --git a/modules/java/generator/gen_java.py b/modules/java/generator/gen_java.py index ccdb03fda..b9fce4294 100755 --- a/modules/java/generator/gen_java.py +++ b/modules/java/generator/gen_java.py @@ -75,7 +75,7 @@ const_ignore_list = ( "CV_CAP_PROP_CONVERT_RGB", "CV_CAP_PROP_WHITE_BALANCE_BLUE_U", "CV_CAP_PROP_RECTIFICATION", - "CV_CAP_PROP_MONOCROME", + "CV_CAP_PROP_MONOCHROME", "CV_CAP_PROP_SHARPNESS", "CV_CAP_PROP_AUTO_EXPOSURE", "CV_CAP_PROP_GAMMA", diff --git a/modules/videoio/include/opencv2/videoio.hpp b/modules/videoio/include/opencv2/videoio.hpp index 88164a326..ab6033112 100644 --- a/modules/videoio/include/opencv2/videoio.hpp +++ b/modules/videoio/include/opencv2/videoio.hpp @@ -110,8 +110,7 @@ enum { CAP_PROP_POS_MSEC =0, CAP_PROP_CONVERT_RGB =16, CAP_PROP_WHITE_BALANCE_BLUE_U =17, CAP_PROP_RECTIFICATION =18, - CAP_PROP_MONOCROME =19, - CAP_PROP_MONOCHROME =CAP_PROP_MONOCROME, + CAP_PROP_MONOCHROME =19, CAP_PROP_SHARPNESS =20, CAP_PROP_AUTO_EXPOSURE =21, // DC1394: exposure control done by camera, user can adjust refernce level using this feature CAP_PROP_GAMMA =22, @@ -216,7 +215,8 @@ enum { CAP_PROP_PVAPI_MULTICASTIP = 300, // ip for anable multicast ma CAP_PROP_PVAPI_DECIMATIONHORIZONTAL = 302, // Horizontal sub-sampling of the image CAP_PROP_PVAPI_DECIMATIONVERTICAL = 303, // Vertical sub-sampling of the image CAP_PROP_PVAPI_BINNINGX = 304, // Horizontal binning factor - CAP_PROP_PVAPI_BINNINGY = 305 // Vertical binning factor + CAP_PROP_PVAPI_BINNINGY = 305, // Vertical binning factor + CAP_PROP_PVAPI_PIXELFORMAT = 306 // Pixel format }; // PVAPI: FrameStartTriggerMode @@ -234,6 +234,17 @@ enum { CAP_PVAPI_DECIMATION_OFF = 1, // Off CAP_PVAPI_DECIMATION_2OUTOF16 = 8 // 2 out of 16 decimation }; +// PVAPI: PixelFormat +enum { CAP_PVAPI_PIXELFORMAT_MONO8 = 1, // Mono8 + CAP_PVAPI_PIXELFORMAT_MONO16 = 2, // Mono16 + CAP_PVAPI_PIXELFORMAT_BAYER8 = 3, // Bayer8 + CAP_PVAPI_PIXELFORMAT_BAYER16 = 4, // Bayer16 + CAP_PVAPI_PIXELFORMAT_RGB24 = 5, // Rgb24 + CAP_PVAPI_PIXELFORMAT_BGR24 = 6, // Bgr24 + CAP_PVAPI_PIXELFORMAT_RGBA32 = 7, // Rgba32 + CAP_PVAPI_PIXELFORMAT_BGRA32 = 8, // Bgra32 + }; + // Properties of cameras available through XIMEA SDK interface enum { CAP_PROP_XI_DOWNSAMPLING = 400, // Change image resolution by binning or skipping. CAP_PROP_XI_DATA_FORMAT = 401, // Output data format. diff --git a/modules/videoio/include/opencv2/videoio/videoio_c.h b/modules/videoio/include/opencv2/videoio/videoio_c.h index 5130fa6f5..461307bd2 100644 --- a/modules/videoio/include/opencv2/videoio/videoio_c.h +++ b/modules/videoio/include/opencv2/videoio/videoio_c.h @@ -160,7 +160,6 @@ enum CV_CAP_PROP_CONVERT_RGB =16, CV_CAP_PROP_WHITE_BALANCE_BLUE_U =17, CV_CAP_PROP_RECTIFICATION =18, - CV_CAP_PROP_MONOCROME =19, CV_CAP_PROP_MONOCHROME =19, CV_CAP_PROP_SHARPNESS =20, CV_CAP_PROP_AUTO_EXPOSURE =21, // exposure control done by camera, @@ -227,6 +226,7 @@ enum CV_CAP_PROP_PVAPI_DECIMATIONVERTICAL = 303, // Vertical sub-sampling of the image CV_CAP_PROP_PVAPI_BINNINGX = 304, // Horizontal binning factor CV_CAP_PROP_PVAPI_BINNINGY = 305, // Vertical binning factor + CV_CAP_PROP_PVAPI_PIXELFORMAT = 306, // Pixel format // Properties of cameras available through XIMEA SDK interface CV_CAP_PROP_XI_DOWNSAMPLING = 400, // Change image resolution by binning or skipping. diff --git a/modules/videoio/src/cap_dshow.cpp b/modules/videoio/src/cap_dshow.cpp index 013d08e54..c8c63fcb7 100644 --- a/modules/videoio/src/cap_dshow.cpp +++ b/modules/videoio/src/cap_dshow.cpp @@ -2257,7 +2257,7 @@ int videoInput::getVideoPropertyFromCV(int cv_property){ case CV_CAP_PROP_GAMMA: return VideoProcAmp_Gamma; - case CV_CAP_PROP_MONOCROME: + case CV_CAP_PROP_MONOCHROME: return VideoProcAmp_ColorEnable; case CV_CAP_PROP_WHITE_BALANCE_BLUE_U: @@ -3170,7 +3170,7 @@ double VideoCapture_DShow::getProperty(int propIdx) const case CV_CAP_PROP_SATURATION: case CV_CAP_PROP_SHARPNESS: case CV_CAP_PROP_GAMMA: - case CV_CAP_PROP_MONOCROME: + case CV_CAP_PROP_MONOCHROME: case CV_CAP_PROP_WHITE_BALANCE_BLUE_U: case CV_CAP_PROP_BACKLIGHT: case CV_CAP_PROP_GAIN: @@ -3273,7 +3273,7 @@ bool VideoCapture_DShow::setProperty(int propIdx, double propVal) case CV_CAP_PROP_SATURATION: case CV_CAP_PROP_SHARPNESS: case CV_CAP_PROP_GAMMA: - case CV_CAP_PROP_MONOCROME: + case CV_CAP_PROP_MONOCHROME: case CV_CAP_PROP_WHITE_BALANCE_BLUE_U: case CV_CAP_PROP_BACKLIGHT: case CV_CAP_PROP_GAIN: diff --git a/modules/videoio/src/cap_pvapi.cpp b/modules/videoio/src/cap_pvapi.cpp index 5c7e05e34..a6577fc78 100644 --- a/modules/videoio/src/cap_pvapi.cpp +++ b/modules/videoio/src/cap_pvapi.cpp @@ -60,6 +60,7 @@ #ifdef WIN32 # include #else +# include # include #endif @@ -106,18 +107,14 @@ protected: } tCamera; IplImage *frame; - IplImage *grayframe; tCamera Camera; tPvErr Errcode; - bool monocrome; }; CvCaptureCAM_PvAPI::CvCaptureCAM_PvAPI() { - monocrome=false; frame = NULL; - grayframe = NULL; memset(&this->Camera, 0, sizeof(this->Camera)); } @@ -190,13 +187,6 @@ bool CvCaptureCAM_PvAPI::open( int index ) tPvUint32 frameWidth, frameHeight; unsigned long maxSize; - // By Default, try to set the pixel format to Mono8. This can be changed later - // via calls to setProperty. Some colour cameras (i.e. the Manta line) have a default - // image mode of Bayer8, which is currently unsupported, so Mono8 is a safe bet for - // startup. - - monocrome = (PvAttrEnumSet(Camera.Handle, "PixelFormat", "Mono8") == ePvErrSuccess); - PvAttrUint32Get(Camera.Handle, "Width", &frameWidth); PvAttrUint32Get(Camera.Handle, "Height", &frameHeight); @@ -229,15 +219,9 @@ bool CvCaptureCAM_PvAPI::grabFrame() IplImage* CvCaptureCAM_PvAPI::retrieveFrame(int) { - if (PvCaptureWaitForFrameDone(Camera.Handle, &(Camera.Frame), 1000) == ePvErrSuccess) { - if (!monocrome) - { - cvMerge(grayframe,grayframe,grayframe,NULL,frame); - return frame; - } - return grayframe; + return frame; } else return NULL; } @@ -254,11 +238,6 @@ double CvCaptureCAM_PvAPI::getProperty( int property_id ) const case CV_CAP_PROP_FRAME_HEIGHT: PvAttrUint32Get(Camera.Handle, "Height", &nTemp); return (double)nTemp; - case CV_CAP_PROP_MONOCROME: - if (monocrome) - return 1; - else - return 0; case CV_CAP_PROP_EXPOSURE: PvAttrUint32Get(Camera.Handle,"ExposureValue",&nTemp); return (double)nTemp; @@ -312,6 +291,25 @@ double CvCaptureCAM_PvAPI::getProperty( int property_id ) const case CV_CAP_PROP_PVAPI_BINNINGY: PvAttrUint32Get(Camera.Handle,"BinningY",&nTemp); return (double)nTemp; + case CV_CAP_PROP_PVAPI_PIXELFORMAT: + char pixelFormat[256]; + PvAttrEnumGet(Camera.Handle, "PixelFormat", pixelFormat,256,NULL); + if (strcmp(pixelFormat, "Mono8")==0) + return 1.0; + else if (strcmp(pixelFormat, "Mono16")==0) + return 2.0; + else if (strcmp(pixelFormat, "Bayer8")==0) + return 3.0; + else if (strcmp(pixelFormat, "Bayer16")==0) + return 4.0; + else if (strcmp(pixelFormat, "Rgb24")==0) + return 5.0; + else if (strcmp(pixelFormat, "Bgr24")==0) + return 6.0; + else if (strcmp(pixelFormat, "Rgba32")==0) + return 7.0; + else if (strcmp(pixelFormat, "Bgra32")==0) + return 8.0; } return -1.0; } @@ -359,21 +357,6 @@ bool CvCaptureCAM_PvAPI::setProperty( int property_id, double value ) break; } - case CV_CAP_PROP_MONOCROME: - if (value==1) - { - char pixelFormat[256]; - PvAttrEnumGet(Camera.Handle, "PixelFormat", pixelFormat,256,NULL); - if ((strcmp(pixelFormat, "Mono8")==0) || strcmp(pixelFormat, "Mono16")==0) - { - monocrome=true; - } - else - return false; - } - else - monocrome=false; - break; case CV_CAP_PROP_EXPOSURE: if ((PvAttrUint32Set(Camera.Handle,"ExposureValue",(tPvUint32)value)==ePvErrSuccess)) break; @@ -449,6 +432,51 @@ bool CvCaptureCAM_PvAPI::setProperty( int property_id, double value ) break; else return false; + case CV_CAP_PROP_PVAPI_PIXELFORMAT: + { + cv::String pixelFormat; + + if (value==1) + pixelFormat = "Mono8"; + else if (value==2) + pixelFormat = "Mono16"; + else if (value==3) + pixelFormat = "Bayer8"; + else if (value==4) + pixelFormat = "Bayer16"; + else if (value==5) + pixelFormat = "Rgb24"; + else if (value==6) + pixelFormat = "Bgr24"; + else if (value==7) + pixelFormat = "Rgba32"; + else if (value==8) + pixelFormat = "Bgra32"; + else + return false; + + if ((PvAttrEnumSet(Camera.Handle,"PixelFormat", pixelFormat.c_str())==ePvErrSuccess)) + { + tPvUint32 currWidth; + tPvUint32 currHeight; + + PvAttrUint32Get(Camera.Handle, "Width", &currWidth); + PvAttrUint32Get(Camera.Handle, "Height", &currHeight); + + stopCapture(); + // Reallocate Frames + if (!resizeCaptureFrame(currWidth, currHeight)) + { + startCapture(); + return false; + } + + startCapture(); + return true; + } + else + return false; + } default: return false; } @@ -495,13 +523,6 @@ bool CvCaptureCAM_PvAPI::resizeCaptureFrame (int frameWidth, int frameHeight) tPvUint32 sensorHeight; tPvUint32 sensorWidth; - - if (grayframe) - { - cvReleaseImage(&grayframe); - grayframe = NULL; - } - if (frame) { cvReleaseImage(&frame); @@ -544,28 +565,31 @@ bool CvCaptureCAM_PvAPI::resizeCaptureFrame (int frameWidth, int frameHeight) PvAttrUint32Get(Camera.Handle, "TotalBytesPerFrame", &frameSize); - if (strcmp(pixelFormat, "Mono8")==0) + if ( (strcmp(pixelFormat, "Mono8")==0) || (strcmp(pixelFormat, "Bayer8")==0) ) + { + frame = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_8U, 1); + frame->widthStep = (int)frameWidth; + Camera.Frame.ImageBufferSize = frameSize; + Camera.Frame.ImageBuffer = frame->imageData; + } + else if ( (strcmp(pixelFormat, "Mono16")==0) || (strcmp(pixelFormat, "Bayer16")==0) ) + { + frame = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_16U, 1); + frame->widthStep = (int)frameWidth*2; + Camera.Frame.ImageBufferSize = frameSize; + Camera.Frame.ImageBuffer = frame->imageData; + } + else if ( (strcmp(pixelFormat, "Rgb24")==0) || (strcmp(pixelFormat, "Bgr24")==0) ) { - grayframe = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_8U, 1); - grayframe->widthStep = (int)frameWidth; frame = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_8U, 3); frame->widthStep = (int)frameWidth*3; Camera.Frame.ImageBufferSize = frameSize; - Camera.Frame.ImageBuffer = grayframe->imageData; + Camera.Frame.ImageBuffer = frame->imageData; } - else if (strcmp(pixelFormat, "Mono16")==0) + else if ( (strcmp(pixelFormat, "Rgba32")==0) || (strcmp(pixelFormat, "Bgra32")==0) ) { - grayframe = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_16U, 1); - grayframe->widthStep = (int)frameWidth; - frame = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_16U, 3); - frame->widthStep = (int)frameWidth*3; - Camera.Frame.ImageBufferSize = frameSize; - Camera.Frame.ImageBuffer = grayframe->imageData; - } - else if (strcmp(pixelFormat, "Bgr24")==0) - { - frame = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_8U, 3); - frame->widthStep = (int)frameWidth*3; + frame = cvCreateImage(cvSize((int)frameWidth, (int)frameHeight), IPL_DEPTH_8U, 4); + frame->widthStep = (int)frameWidth*4; Camera.Frame.ImageBufferSize = frameSize; Camera.Frame.ImageBuffer = frame->imageData; } From bf16f74618ebbcd0dfc4a9675b61bd08f3156105 Mon Sep 17 00:00:00 2001 From: StevenPuttemans Date: Mon, 9 Mar 2015 12:53:20 +0100 Subject: [PATCH 070/171] fixing universal location execution --- apps/traincascade/imagestorage.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/apps/traincascade/imagestorage.cpp b/apps/traincascade/imagestorage.cpp index 7a004c610..ef33c89f1 100644 --- a/apps/traincascade/imagestorage.cpp +++ b/apps/traincascade/imagestorage.cpp @@ -33,20 +33,12 @@ bool CvCascadeImageReader::NegReader::create( const string _filename, Size _winS if ( !file.is_open() ) return false; - size_t pos = _filename.rfind('\\'); - char dlmrt = '\\'; - if (pos == string::npos) - { - pos = _filename.rfind('/'); - dlmrt = '/'; - } - dirname = pos == string::npos ? "" : _filename.substr(0, pos) + dlmrt; while( !file.eof() ) { std::getline(file, str); if (str.empty()) break; if (str.at(0) == '#' ) continue; /* comment */ - imgFilenames.push_back(dirname + str); + imgFilenames.push_back(str); } file.close(); From c44488629a41d71c14e81b0705f9dbebad541bfe Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Mon, 9 Mar 2015 15:52:16 +0100 Subject: [PATCH 071/171] Changed parameters of fastNlMeansDenoising[Multi][Abs] from float * to std::vector --- modules/photo/include/opencv2/photo.hpp | 40 +- modules/photo/src/denoising.cpp | 482 +++++++++------------- modules/photo/test/ocl/test_denoising.cpp | 5 +- 3 files changed, 225 insertions(+), 302 deletions(-) diff --git a/modules/photo/include/opencv2/photo.hpp b/modules/photo/include/opencv2/photo.hpp index 5e11333ee..d613c2420 100644 --- a/modules/photo/include/opencv2/photo.hpp +++ b/modules/photo/include/opencv2/photo.hpp @@ -149,10 +149,10 @@ Should be odd. Recommended value 7 pixels @param searchWindowSize Size in pixels of the window that is used to compute weighted average for given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels -@param h Array of parameters regulating filter strength, one per -channel. Big h value perfectly removes noise but also removes image -details, smaller h value preserves details but also preserves some -noise +@param h Array of parameters regulating filter strength, either one +parameter applied to all channels or one per channel in src. Big h value +perfectly removes noise but also removes image details, smaller h +value preserves details but also preserves some noise This function expected to be applied to grayscale images. For colored images look at fastNlMeansDenoisingColored. Advanced usage of this functions can be manual denoising of colored @@ -160,7 +160,7 @@ image in different colorspaces. Such approach is used in fastNlMeansDenoisingCol image to CIELAB colorspace and then separately denoise L and AB components with different h parameter. */ -CV_EXPORTS_W void fastNlMeansDenoising( InputArray src, OutputArray dst, float *h, +CV_EXPORTS_W void fastNlMeansDenoising( InputArray src, OutputArray dst, std::vector h, int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Perform image denoising using Non-local Means Denoising @@ -201,10 +201,10 @@ Should be odd. Recommended value 7 pixels @param searchWindowSize Size in pixels of the window that is used to compute weighted average for given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels -@param h Array of parameters regulating filter strength, one per -channel. Big h value perfectly removes noise but also removes image -details, smaller h value preserves details but also preserves some -noise +@param h Array of parameters regulating filter strength, either one +parameter applied to all channels or one per channel in src. Big h value +perfectly removes noise but also removes image details, smaller h +value preserves details but also preserves some noise This function expected to be applied to grayscale images. For colored images look at fastNlMeansDenoisingColored. Advanced usage of this functions can be manual denoising of colored @@ -212,7 +212,7 @@ image in different colorspaces. Such approach is used in fastNlMeansDenoisingCol image to CIELAB colorspace and then separately denoise L and AB components with different h parameter. */ -CV_EXPORTS_W void fastNlMeansDenoisingAbs( InputArray src, OutputArray dst, float *h, +CV_EXPORTS_W void fastNlMeansDenoisingAbs( InputArray src, OutputArray dst, std::vector h, int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Modification of fastNlMeansDenoising function for colored images @@ -283,14 +283,14 @@ Should be odd. Recommended value 7 pixels @param searchWindowSize Size in pixels of the window that is used to compute weighted average for given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels -@param h Array of parameters regulating filter strength, one for each -channel. Bigger h value perfectly removes noise but also removes image -details, smaller h value preserves details but also preserves some -noise +@param h Array of parameters regulating filter strength, either one +parameter applied to all channels or one per channel in src. Big h value +perfectly removes noise but also removes image details, smaller h +value preserves details but also preserves some noise */ CV_EXPORTS_W void fastNlMeansDenoisingMulti( InputArrayOfArrays srcImgs, OutputArray dst, int imgToDenoiseIndex, int temporalWindowSize, - float *h , int templateWindowSize = 7, int searchWindowSize = 21); + std::vector h , int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Modification of fastNlMeansDenoising function for images sequence where consequtive images have been captured in small period @@ -346,14 +346,14 @@ Should be odd. Recommended value 7 pixels @param searchWindowSize Size in pixels of the window that is used to compute weighted average for given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels -@param h Array of parameters regulating filter strength, one for each -channel. Bigger h value perfectly removes noise but also removes image -details, smaller h value preserves details but also preserves some -noise +@param h Array of parameters regulating filter strength, either one +parameter applied to all channels or one per channel in src. Big h value +perfectly removes noise but also removes image details, smaller h +value preserves details but also preserves some noise */ CV_EXPORTS_W void fastNlMeansDenoisingMultiAbs( InputArrayOfArrays srcImgs, OutputArray dst, int imgToDenoiseIndex, int temporalWindowSize, - float *h, int templateWindowSize = 7, int searchWindowSize = 21); + std::vector h, int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Modification of fastNlMeansDenoisingMulti function for colored images sequences diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 9f63254b0..7251b6446 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -48,55 +48,20 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, int templateWindowSize, int searchWindowSize) { - Size src_size = _src.size(); - CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && - src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes - ocl_fastNlMeansDenoising(_src, _dst, &h, 1, - templateWindowSize, searchWindowSize, false)) - - Mat src = _src.getMat(); - _dst.create(src_size, src.type()); - Mat dst = _dst.getMat(); - -#ifdef HAVE_TEGRA_OPTIMIZATION - if(tegra::fastNlMeansDenoising(src, dst, h, templateWindowSize, searchWindowSize)) - return; -#endif - - switch (src.type()) { - case CV_8U: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC2: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC3: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC4: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - default: - CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3 and CV_8UC4 are supported"); - } + fastNlMeansDenoising(_src, _dst, std::vector(1, h), + templateWindowSize, searchWindowSize); } -void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float *h, +void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, std::vector h, int templateWindowSize, int searchWindowSize) { + int hn = h.size(); + CV_Assert(hn == 1 || hn == CV_MAT_CN(_src.type())); + Size src_size = _src.size(); CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes - ocl_fastNlMeansDenoising(_src, _dst, h, CV_MAT_CN(_src.type()), + ocl_fastNlMeansDenoising(_src, _dst, &h[0], hn, templateWindowSize, searchWindowSize, false)) Mat src = _src.getMat(); @@ -111,23 +76,38 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float *h, switch (src.type()) { case CV_8U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC2: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; default: CV_Error(Error::StsBadArg, @@ -138,70 +118,20 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float *h, void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, int templateWindowSize, int searchWindowSize) { - Size src_size = _src.size(); - CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && - src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes - ocl_fastNlMeansDenoising(_src, _dst, &h, 1, - templateWindowSize, searchWindowSize, true)) - - Mat src = _src.getMat(); - _dst.create(src_size, src.type()); - Mat dst = _dst.getMat(); - - switch (src.type()) { - case CV_8U: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC2: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC3: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC4: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16U: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16UC2: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16UC3: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16UC4: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( - src, dst, templateWindowSize, searchWindowSize, &h)); - break; - default: - CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_8UC4, CV_16U, CV_16UC2, CV_16UC3 and CV_16UC4 are supported"); - } + fastNlMeansDenoisingAbs(_src, _dst, std::vector(1, h), + templateWindowSize, searchWindowSize); } -void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float *h, +void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, std::vector h, int templateWindowSize, int searchWindowSize) { + int hn = h.size(); + CV_Assert(hn == 1 || hn == CV_MAT_CN(_src.type())); + Size src_size = _src.size(); CV_OCL_RUN(_src.dims() <= 2 && (_src.isUMat() || _dst.isUMat()) && src_size.width > 5 && src_size.height > 5, // low accuracy on small sizes - ocl_fastNlMeansDenoising(_src, _dst, h, CV_MAT_CN(_src.type()), + ocl_fastNlMeansDenoising(_src, _dst, &h[0], hn, templateWindowSize, searchWindowSize, true)) Mat src = _src.getMat(); @@ -211,43 +141,73 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float *h, switch (src.type()) { case CV_8U: parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC2: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16U: parallel_for_(cv::Range(0, src.rows), FastNlMeansDenoisingInvoker( - src, dst, templateWindowSize, searchWindowSize, h)); + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC2: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC3: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC4: - parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( - src, dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + src, dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, src.rows), + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( + src, dst, templateWindowSize, searchWindowSize, &h[0])); break; default: CV_Error(Error::StsBadArg, @@ -332,51 +292,14 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds int imgToDenoiseIndex, int temporalWindowSize, float h, int templateWindowSize, int searchWindowSize) { - std::vector srcImgs; - _srcImgs.getMatVector(srcImgs); - - fastNlMeansDenoisingMultiCheckPreconditions( - srcImgs, imgToDenoiseIndex, - temporalWindowSize, templateWindowSize, searchWindowSize); - - _dst.create(srcImgs[0].size(), srcImgs[0].type()); - Mat dst = _dst.getMat(); - - switch (srcImgs[0].type()) - { - case CV_8U: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC2: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC3: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC4: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - default: - CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3 and CV_8UC4 are supported"); - } + fastNlMeansDenoisingMulti(_srcImgs, _dst, imgToDenoiseIndex, temporalWindowSize, + std::vector(1, h), templateWindowSize, searchWindowSize); } void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _dst, int imgToDenoiseIndex, int temporalWindowSize, - float *h, int templateWindowSize, int searchWindowSize) + std::vector h, + int templateWindowSize, int searchWindowSize) { std::vector srcImgs; _srcImgs.getMatVector(srcImgs); @@ -385,6 +308,9 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds srcImgs, imgToDenoiseIndex, temporalWindowSize, templateWindowSize, searchWindowSize); + int hn = h.size(); + CV_Assert(hn == 1 || hn == CV_MAT_CN(srcImgs[0].type())); + _dst.create(srcImgs[0].size(), srcImgs[0].type()); Mat dst = _dst.getMat(); @@ -392,27 +318,45 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds { case CV_8U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC2: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; default: CV_Error(Error::StsBadArg, @@ -424,75 +368,14 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray int imgToDenoiseIndex, int temporalWindowSize, float h, int templateWindowSize, int searchWindowSize) { - std::vector srcImgs; - _srcImgs.getMatVector(srcImgs); - - fastNlMeansDenoisingMultiCheckPreconditions( - srcImgs, imgToDenoiseIndex, - temporalWindowSize, templateWindowSize, searchWindowSize); - - _dst.create(srcImgs[0].size(), srcImgs[0].type()); - Mat dst = _dst.getMat(); - - switch (srcImgs[0].type()) - { - case CV_8U: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC2: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC3: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_8UC4: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16U: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16UC2: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16UC3: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - case CV_16UC4: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, &h)); - break; - default: - CV_Error(Error::StsBadArg, - "Unsupported image format! Only CV_8U, CV_8UC2, CV_8UC3, CV_8UC4, CV_16U, CV_16UC2, CV_16UC3 and CV_16UC4 are supported"); - } + fastNlMeansDenoisingMulti(_srcImgs, _dst, imgToDenoiseIndex, temporalWindowSize, + std::vector(1, h), templateWindowSize, searchWindowSize); } void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray _dst, int imgToDenoiseIndex, int temporalWindowSize, - float *h, int templateWindowSize, int searchWindowSize) + std::vector h, + int templateWindowSize, int searchWindowSize) { std::vector srcImgs; _srcImgs.getMatVector(srcImgs); @@ -501,6 +384,9 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray srcImgs, imgToDenoiseIndex, temporalWindowSize, templateWindowSize, searchWindowSize); + int hn = h.size(); + CV_Assert(hn == 1 || hn == CV_MAT_CN(srcImgs[0].type())); + _dst.create(srcImgs[0].size(), srcImgs[0].type()); Mat dst = _dst.getMat(); @@ -508,51 +394,87 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray { case CV_8U: parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC2: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16U: parallel_for_(cv::Range(0, srcImgs[0].rows), FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC2: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC3: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC4: - parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( - srcImgs, imgToDenoiseIndex, temporalWindowSize, - dst, templateWindowSize, searchWindowSize, h)); + if (hn == 1) + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); + else + parallel_for_(cv::Range(0, srcImgs[0].rows), + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( + srcImgs, imgToDenoiseIndex, temporalWindowSize, + dst, templateWindowSize, searchWindowSize, &h[0])); break; default: CV_Error(Error::StsBadArg, diff --git a/modules/photo/test/ocl/test_denoising.cpp b/modules/photo/test/ocl/test_denoising.cpp index 3b6998f06..360c16296 100644 --- a/modules/photo/test/ocl/test_denoising.cpp +++ b/modules/photo/test/ocl/test_denoising.cpp @@ -16,7 +16,7 @@ namespace ocl { PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool, bool) { int cn, templateWindowSize, searchWindowSize; - float h[4]; + std::vector h; bool use_roi, use_image; TEST_DECLARE_INPUT_PARAMETER(src); @@ -31,7 +31,7 @@ PARAM_TEST_CASE(FastNlMeansDenoisingTestBase, Channels, bool, bool) templateWindowSize = 7; searchWindowSize = 21; - ASSERT_TRUE(cn > 0 && cn <= 4); + h.resize(cn); for (int i=0; i 0 && cn <= 4); if (cn == 2) { int from_to[] = { 0,0, 1,1 }; src_roi.create(roiSize, type); From a594a0677afb7106791cfaf2e2d129fa0690d426 Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Mon, 9 Mar 2015 16:00:24 +0100 Subject: [PATCH 072/171] Cleanup --- modules/photo/src/denoising.cpp | 72 ++++++++++++++++----------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 7251b6446..c42ac4567 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -82,31 +82,31 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, std::vector( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; default: @@ -147,31 +147,31 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, std::vector case CV_8UC2: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker( + FastNlMeansDenoisingInvoker( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16U: @@ -182,31 +182,31 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, std::vector case CV_16UC2: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC3: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC4: if (hn == 1) parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, int>( src, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, src.rows), - FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( + FastNlMeansDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( src, dst, templateWindowSize, searchWindowSize, &h[0])); break; default: @@ -325,36 +325,36 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds case CV_8UC2: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; @@ -401,36 +401,36 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray case CV_8UC2: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC3: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_8UC4: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker( + FastNlMeansMultiDenoisingInvoker( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; @@ -443,36 +443,36 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray case CV_16UC2: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec2i>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC3: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec3i>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; case CV_16UC4: if (hn == 1) parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, int>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); else parallel_for_(cv::Range(0, srcImgs[0].rows), - FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( + FastNlMeansMultiDenoisingInvoker, int64, uint64, DistAbs, Vec4i>( srcImgs, imgToDenoiseIndex, temporalWindowSize, dst, templateWindowSize, searchWindowSize, &h[0])); break; From 4b5753daea25f0ba439d7f82f7d320d9ff743d8a Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Mon, 9 Mar 2015 16:11:18 +0100 Subject: [PATCH 073/171] Corrected documentation --- modules/photo/include/opencv2/photo.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/photo/include/opencv2/photo.hpp b/modules/photo/include/opencv2/photo.hpp index d613c2420..1867d3ef0 100644 --- a/modules/photo/include/opencv2/photo.hpp +++ b/modules/photo/include/opencv2/photo.hpp @@ -150,7 +150,7 @@ Should be odd. Recommended value 7 pixels given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels @param h Array of parameters regulating filter strength, either one -parameter applied to all channels or one per channel in src. Big h value +parameter applied to all channels or one per channel in dst. Big h value perfectly removes noise but also removes image details, smaller h value preserves details but also preserves some noise @@ -202,7 +202,7 @@ Should be odd. Recommended value 7 pixels given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels @param h Array of parameters regulating filter strength, either one -parameter applied to all channels or one per channel in src. Big h value +parameter applied to all channels or one per channel in dst. Big h value perfectly removes noise but also removes image details, smaller h value preserves details but also preserves some noise @@ -284,7 +284,7 @@ Should be odd. Recommended value 7 pixels given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels @param h Array of parameters regulating filter strength, either one -parameter applied to all channels or one per channel in src. Big h value +parameter applied to all channels or one per channel in dst. Big h value perfectly removes noise but also removes image details, smaller h value preserves details but also preserves some noise */ @@ -347,7 +347,7 @@ Should be odd. Recommended value 7 pixels given pixel. Should be odd. Affect performance linearly: greater searchWindowsSize - greater denoising time. Recommended value 21 pixels @param h Array of parameters regulating filter strength, either one -parameter applied to all channels or one per channel in src. Big h value +parameter applied to all channels or one per channel in dst. Big h value perfectly removes noise but also removes image details, smaller h value preserves details but also preserves some noise */ From 1af452123992f6dbd49ee45368455778359ce4bf Mon Sep 17 00:00:00 2001 From: hahne Date: Mon, 9 Mar 2015 16:19:47 +0100 Subject: [PATCH 074/171] fix for issue 1915 --- modules/videoio/src/cap_ffmpeg_impl.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/modules/videoio/src/cap_ffmpeg_impl.hpp b/modules/videoio/src/cap_ffmpeg_impl.hpp index 5870f4bc7..b5ff31a28 100644 --- a/modules/videoio/src/cap_ffmpeg_impl.hpp +++ b/modules/videoio/src/cap_ffmpeg_impl.hpp @@ -649,13 +649,13 @@ bool CvCapture_FFMPEG::grabFrame() frame_number > ic->streams[video_stream]->nb_frames ) return false; - av_free_packet (&packet); - picture_pts = AV_NOPTS_VALUE_; // get the next frame while (!valid) { + + av_free_packet (&packet); int ret = av_read_frame(ic, &packet); if (ret == AVERROR(EAGAIN)) continue; @@ -698,8 +698,6 @@ bool CvCapture_FFMPEG::grabFrame() if (count_errs > max_number_of_attempts) break; } - - av_free_packet (&packet); } if( valid && first_frame_number < 0 ) From b471f9ee2622641426e7e71978ab9e1ba181841b Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Mon, 9 Mar 2015 18:52:25 +0100 Subject: [PATCH 075/171] Fixed call to tegra::fastNlMeansDenoising --- modules/photo/src/denoising.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index c42ac4567..5445d26cd 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -69,7 +69,7 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, std::vector Date: Mon, 9 Mar 2015 23:47:49 +0100 Subject: [PATCH 076/171] Changed parameter type of fastNlMeansDenoising[Multi][Abs] from std::vector to const std::vector& --- modules/photo/include/opencv2/photo.hpp | 14 ++++++++------ modules/photo/src/denoising.cpp | 8 ++++---- .../photo/src/fast_nlmeans_denoising_opencl.hpp | 4 ++-- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/modules/photo/include/opencv2/photo.hpp b/modules/photo/include/opencv2/photo.hpp index 1867d3ef0..ff98ba74f 100644 --- a/modules/photo/include/opencv2/photo.hpp +++ b/modules/photo/include/opencv2/photo.hpp @@ -160,8 +160,9 @@ image in different colorspaces. Such approach is used in fastNlMeansDenoisingCol image to CIELAB colorspace and then separately denoise L and AB components with different h parameter. */ -CV_EXPORTS_W void fastNlMeansDenoising( InputArray src, OutputArray dst, std::vector h, - int templateWindowSize = 7, int searchWindowSize = 21); + CV_EXPORTS_W void fastNlMeansDenoising( InputArray src, OutputArray dst, + const std::vector& h, + int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Perform image denoising using Non-local Means Denoising algorithm @@ -212,8 +213,9 @@ image in different colorspaces. Such approach is used in fastNlMeansDenoisingCol image to CIELAB colorspace and then separately denoise L and AB components with different h parameter. */ -CV_EXPORTS_W void fastNlMeansDenoisingAbs( InputArray src, OutputArray dst, std::vector h, - int templateWindowSize = 7, int searchWindowSize = 21); +CV_EXPORTS_W void fastNlMeansDenoisingAbs( InputArray src, OutputArray dst, + const std::vector& h, + int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Modification of fastNlMeansDenoising function for colored images @@ -290,7 +292,7 @@ value preserves details but also preserves some noise */ CV_EXPORTS_W void fastNlMeansDenoisingMulti( InputArrayOfArrays srcImgs, OutputArray dst, int imgToDenoiseIndex, int temporalWindowSize, - std::vector h , int templateWindowSize = 7, int searchWindowSize = 21); + const std::vector& h , int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Modification of fastNlMeansDenoising function for images sequence where consequtive images have been captured in small period @@ -353,7 +355,7 @@ value preserves details but also preserves some noise */ CV_EXPORTS_W void fastNlMeansDenoisingMultiAbs( InputArrayOfArrays srcImgs, OutputArray dst, int imgToDenoiseIndex, int temporalWindowSize, - std::vector h, int templateWindowSize = 7, int searchWindowSize = 21); + const std::vector& h, int templateWindowSize = 7, int searchWindowSize = 21); /** @brief Modification of fastNlMeansDenoisingMulti function for colored images sequences diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 7dde96081..5243b4330 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -52,7 +52,7 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, templateWindowSize, searchWindowSize); } -void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, std::vector h, +void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, const std::vector& h, int templateWindowSize, int searchWindowSize) { int hn = h.size(); @@ -123,7 +123,7 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, templateWindowSize, searchWindowSize); } -void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, std::vector h, +void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, const std::vector& h, int templateWindowSize, int searchWindowSize) { int hn = h.size(); @@ -299,7 +299,7 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _dst, int imgToDenoiseIndex, int temporalWindowSize, - std::vector h, + const std::vector& h, int templateWindowSize, int searchWindowSize) { std::vector srcImgs; @@ -375,7 +375,7 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray _dst, int imgToDenoiseIndex, int temporalWindowSize, - std::vector h, + const std::vector& h, int templateWindowSize, int searchWindowSize) { std::vector srcImgs; diff --git a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp index a06dc6192..b7fdc7cf9 100644 --- a/modules/photo/src/fast_nlmeans_denoising_opencl.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_opencl.hpp @@ -29,7 +29,7 @@ static int divUp(int a, int b) } template -static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindowSize, int templateWindowSize, FT *h, int hn, int cn, +static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindowSize, int templateWindowSize, const FT *h, int hn, int cn, int & almostTemplateWindowSizeSqBinShift, bool abs) { const WT maxEstimateSumValue = searchWindowSize * searchWindowSize * @@ -78,7 +78,7 @@ static bool ocl_calcAlmostDist2Weight(UMat & almostDist2Weight, int searchWindow return k.run(1, globalsize, NULL, false); } -static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, float *h, int hn, +static bool ocl_fastNlMeansDenoising(InputArray _src, OutputArray _dst, const float *h, int hn, int templateWindowSize, int searchWindowSize, bool abs) { int type = _src.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type); From 812edb5fdc8eaa72151994280fb673a0d48fb62b Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 10 Mar 2015 01:34:02 +0100 Subject: [PATCH 077/171] Fixed bug, maxDist() should reurn int, not double --- modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index 9833ea7d3..8f31e8b02 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -176,7 +176,7 @@ public: } template - static inline double maxDist() + static inline int maxDist() { return (int)pixelInfo::sampleMax() * pixelInfo::channels; } @@ -298,7 +298,7 @@ public: } template - static inline double maxDist() + static inline int maxDist() { return (int)pixelInfo::sampleMax() * (int)pixelInfo::sampleMax() * pixelInfo::channels; From 82c54104d6901e03027240cd9c6866f6b2509d0a Mon Sep 17 00:00:00 2001 From: Erik Karlsson Date: Tue, 10 Mar 2015 01:39:43 +0100 Subject: [PATCH 078/171] Fix warnings on Win x64 --- modules/photo/src/denoising.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/photo/src/denoising.cpp b/modules/photo/src/denoising.cpp index 5243b4330..fb3889339 100644 --- a/modules/photo/src/denoising.cpp +++ b/modules/photo/src/denoising.cpp @@ -55,7 +55,7 @@ void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, float h, void cv::fastNlMeansDenoising( InputArray _src, OutputArray _dst, const std::vector& h, int templateWindowSize, int searchWindowSize) { - int hn = h.size(); + int hn = (int)h.size(); CV_Assert(hn == 1 || hn == CV_MAT_CN(_src.type())); Size src_size = _src.size(); @@ -126,7 +126,7 @@ void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, float h, void cv::fastNlMeansDenoisingAbs( InputArray _src, OutputArray _dst, const std::vector& h, int templateWindowSize, int searchWindowSize) { - int hn = h.size(); + int hn = (int)h.size(); CV_Assert(hn == 1 || hn == CV_MAT_CN(_src.type())); Size src_size = _src.size(); @@ -309,7 +309,7 @@ void cv::fastNlMeansDenoisingMulti( InputArrayOfArrays _srcImgs, OutputArray _ds srcImgs, imgToDenoiseIndex, temporalWindowSize, templateWindowSize, searchWindowSize); - int hn = h.size(); + int hn = (int)h.size(); CV_Assert(hn == 1 || hn == CV_MAT_CN(srcImgs[0].type())); _dst.create(srcImgs[0].size(), srcImgs[0].type()); @@ -385,7 +385,7 @@ void cv::fastNlMeansDenoisingMultiAbs( InputArrayOfArrays _srcImgs, OutputArray srcImgs, imgToDenoiseIndex, temporalWindowSize, templateWindowSize, searchWindowSize); - int hn = h.size(); + int hn = (int)h.size(); CV_Assert(hn == 1 || hn == CV_MAT_CN(srcImgs[0].type())); _dst.create(srcImgs[0].size(), srcImgs[0].type()); From 6f8b3fc6337a804bc140df7ed6b07a205589717d Mon Sep 17 00:00:00 2001 From: Ilya Lavrenov Date: Fri, 27 Feb 2015 18:59:55 +0300 Subject: [PATCH 079/171] cvRound --- modules/core/include/opencv2/core/cvdef.h | 16 ++++---- modules/core/perf/perf_cvround.cpp | 45 +++++++++++++++++++++++ 2 files changed, 53 insertions(+), 8 deletions(-) create mode 100644 modules/core/perf/perf_cvround.cpp diff --git a/modules/core/include/opencv2/core/cvdef.h b/modules/core/include/opencv2/core/cvdef.h index 892c59d3b..3498b0918 100644 --- a/modules/core/include/opencv2/core/cvdef.h +++ b/modules/core/include/opencv2/core/cvdef.h @@ -480,16 +480,14 @@ CV_INLINE int cvRound( double value ) fistp t; } return t; -#elif defined _MSC_VER && defined _M_ARM && defined HAVE_TEGRA_OPTIMIZATION - TEGRA_ROUND(value); +#elif ((defined _MSC_VER && defined _M_ARM) || defined CV_ICC || defined __GNUC__) && defined HAVE_TEGRA_OPTIMIZATION + TEGRA_ROUND_DBL(value); #elif defined CV_ICC || defined __GNUC__ -# ifdef HAVE_TEGRA_OPTIMIZATION - TEGRA_ROUND(value); -# elif CV_VFP +# if CV_VFP ARM_ROUND_DBL(value) -# else +# else return (int)lrint(value); -# endif +# endif #else double intpart, fractpart; fractpart = modf(value, &intpart); @@ -505,7 +503,9 @@ CV_INLINE int cvRound( double value ) /** @overload */ CV_INLINE int cvRound(float value) { -#if CV_VFP && !defined HAVE_TEGRA_OPTIMIZATION +#if defined ANDROID && (defined CV_ICC || defined __GNUC__) && defined HAVE_TEGRA_OPTIMIZATION + TEGRA_ROUND_FLT(value); +#elif CV_VFP && !defined HAVE_TEGRA_OPTIMIZATION ARM_ROUND_FLT(value) #else return cvRound((double)value); diff --git a/modules/core/perf/perf_cvround.cpp b/modules/core/perf/perf_cvround.cpp new file mode 100644 index 000000000..e9db32354 --- /dev/null +++ b/modules/core/perf/perf_cvround.cpp @@ -0,0 +1,45 @@ +#include "perf_precomp.hpp" + +using namespace std; +using namespace cv; +using namespace perf; +using std::tr1::make_tuple; +using std::tr1::get; + +template +static void CvRoundMat(const cv::Mat & src, cv::Mat & dst) +{ + for (int y = 0; y < dst.rows; ++y) + { + const T * sptr = src.ptr(y); + int * dptr = dst.ptr(y); + + for (int x = 0; x < dst.cols; ++x) + dptr[x] = cvRound(sptr[x]); + } +} + +PERF_TEST_P(Size_MatType, CvRound_Float, + testing::Combine(testing::Values(TYPICAL_MAT_SIZES), + testing::Values(CV_32FC1, CV_64FC1))) +{ + Size size = get<0>(GetParam()); + int type = get<1>(GetParam()), depth = CV_MAT_DEPTH(type); + + cv::Mat src(size, type), dst(size, CV_32SC1); + + declare.in(src, WARMUP_RNG).out(dst); + + if (depth == CV_32F) + { + TEST_CYCLE() + CvRoundMat(src, dst); + } + else if (depth == CV_64F) + { + TEST_CYCLE() + CvRoundMat(src, dst); + } + + SANITY_CHECK_NOTHING(); +} From 085409340e2529a2398e8190909b216b788971a4 Mon Sep 17 00:00:00 2001 From: Pavel Vlasov Date: Thu, 12 Mar 2015 17:58:03 +0300 Subject: [PATCH 080/171] Implementation collector update; Moved out of TLS to properly collect data from different threads; Concurrent access guards were added to collection functions; --- modules/core/include/opencv2/core/utility.hpp | 2 +- modules/core/src/precomp.hpp | 30 ++++++++----- modules/core/src/system.cpp | 45 +++++++++++-------- 3 files changed, 47 insertions(+), 30 deletions(-) diff --git a/modules/core/include/opencv2/core/utility.hpp b/modules/core/include/opencv2/core/utility.hpp index f89560a80..e6dfd7a4f 100644 --- a/modules/core/include/opencv2/core/utility.hpp +++ b/modules/core/include/opencv2/core/utility.hpp @@ -61,7 +61,7 @@ CV_EXPORTS void addImpl(int flag, const char* func = 0); // add implementation a // Each implementation entry correspond to function name entry, so you can find which implementation was executed in which fucntion CV_EXPORTS int getImpl(std::vector &impl, std::vector &funName); -CV_EXPORTS bool useCollection(); // return implementation colelction state +CV_EXPORTS bool useCollection(); // return implementation collection state CV_EXPORTS void setUseCollection(bool flag); // set implementation collection state #define CV_IMPL_PLAIN 0x01 // native CPU OpenCV implementation diff --git a/modules/core/src/precomp.hpp b/modules/core/src/precomp.hpp index a57849012..fa6120371 100644 --- a/modules/core/src/precomp.hpp +++ b/modules/core/src/precomp.hpp @@ -232,15 +232,30 @@ inline bool checkScalar(InputArray sc, int atype, int sckind, int akind) void convertAndUnrollScalar( const Mat& sc, int buftype, uchar* scbuf, size_t blocksize ); +#ifdef CV_COLLECT_IMPL_DATA +struct ImplCollector +{ + ImplCollector() + { + useCollection = false; + implFlags = 0; + } + bool useCollection; // enable/disable impl data collection + + int implFlags; + std::vector implCode; + std::vector implFun; + + cv::Mutex mutex; +}; +#endif + struct CoreTLSData { - CoreTLSData() : device(0), useOpenCL(-1), useIPP(-1), useCollection(false) + CoreTLSData() : device(0), useOpenCL(-1), useIPP(-1) { #ifdef HAVE_TEGRA_OPTIMIZATION useTegra = -1; -#endif -#ifdef CV_COLLECT_IMPL_DATA - implFlags = 0; #endif } @@ -251,13 +266,6 @@ struct CoreTLSData int useIPP; // 1 - use, 0 - do not use, -1 - auto/not initialized #ifdef HAVE_TEGRA_OPTIMIZATION int useTegra; // 1 - use, 0 - do not use, -1 - auto/not initialized -#endif - bool useCollection; // enable/disable impl data collection - -#ifdef CV_COLLECT_IMPL_DATA - int implFlags; - std::vector implCode; - std::vector implFun; #endif }; diff --git a/modules/core/src/system.cpp b/modules/core/src/system.cpp index cefae8cb8..46f41dcca 100644 --- a/modules/core/src/system.cpp +++ b/modules/core/src/system.cpp @@ -1163,47 +1163,56 @@ TLSData& getCoreTlsData() #ifdef CV_COLLECT_IMPL_DATA +ImplCollector& getImplData() +{ + static ImplCollector *value = new ImplCollector(); + return *value; +} + void setImpl(int flags) { - CoreTLSData* data = getCoreTlsData().get(); - data->implFlags = flags; - data->implCode.clear(); - data->implFun.clear(); + cv::AutoLock lock(getImplData().mutex); + + getImplData().implFlags = flags; + getImplData().implCode.clear(); + getImplData().implFun.clear(); } void addImpl(int flag, const char* func) { - CoreTLSData* data = getCoreTlsData().get(); - data->implFlags |= flag; + cv::AutoLock lock(getImplData().mutex); + + getImplData().implFlags |= flag; if(func) // use lazy collection if name was not specified { - size_t index = data->implCode.size(); - if(!index || (data->implCode[index-1] != flag || data->implFun[index-1].compare(func))) // avoid duplicates + size_t index = getImplData().implCode.size(); + if(!index || (getImplData().implCode[index-1] != flag || getImplData().implFun[index-1].compare(func))) // avoid duplicates { - data->implCode.push_back(flag); - data->implFun.push_back(func); + getImplData().implCode.push_back(flag); + getImplData().implFun.push_back(func); } } } int getImpl(std::vector &impl, std::vector &funName) { - CoreTLSData* data = getCoreTlsData().get(); - impl = data->implCode; - funName = data->implFun; - return data->implFlags; // return actual flags for lazy collection + cv::AutoLock lock(getImplData().mutex); + + impl = getImplData().implCode; + funName = getImplData().implFun; + return getImplData().implFlags; // return actual flags for lazy collection } bool useCollection() { - CoreTLSData* data = getCoreTlsData().get(); - return data->useCollection; + return getImplData().useCollection; } void setUseCollection(bool flag) { - CoreTLSData* data = getCoreTlsData().get(); - data->useCollection = flag; + cv::AutoLock lock(getImplData().mutex); + + getImplData().useCollection = flag; } #endif From 632afe6ae3f95fc4228a0c3b92ce08eaed748183 Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Thu, 12 Mar 2015 19:14:14 +0300 Subject: [PATCH 081/171] Fixed mangled install layout on unix machines --- CMakeLists.txt | 20 ++++++++++++++++++-- cmake/OpenCVGenConfig.cmake | 23 +++++------------------ data/CMakeLists.txt | 11 +++-------- doc/CMakeLists.txt | 27 ++++----------------------- modules/java/CMakeLists.txt | 15 +++++---------- 5 files changed, 35 insertions(+), 61 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bc81d51cd..20a1f6fd8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -274,8 +274,6 @@ endif() if(ANDROID OR WIN32) set(OPENCV_DOC_INSTALL_PATH doc) -elseif(INSTALL_TO_MANGLED_PATHS) - set(OPENCV_DOC_INSTALL_PATH share/OpenCV-${OPENCV_VERSION}/doc) else() set(OPENCV_DOC_INSTALL_PATH share/OpenCV/doc) endif() @@ -309,6 +307,10 @@ if(NOT OPENCV_TEST_INSTALL_PATH) set(OPENCV_TEST_INSTALL_PATH "${OPENCV_BIN_INSTALL_PATH}") endif() +if (OPENCV_TEST_DATA_PATH) + get_filename_component(OPENCV_TEST_DATA_PATH ${OPENCV_TEST_DATA_PATH} ABSOLUTE) +endif() + if(OPENCV_TEST_DATA_PATH AND NOT OPENCV_TEST_DATA_INSTALL_PATH) if(ANDROID) set(OPENCV_TEST_DATA_INSTALL_PATH "sdk/etc/testdata") @@ -327,9 +329,11 @@ if(ANDROID) set(OPENCV_CONFIG_INSTALL_PATH sdk/native/jni) set(OPENCV_INCLUDE_INSTALL_PATH sdk/native/jni/include) set(OPENCV_SAMPLES_SRC_INSTALL_PATH samples/native) + set(OPENCV_OTHER_INSTALL_PATH sdk/etc) else() set(LIBRARY_OUTPUT_PATH "${OpenCV_BINARY_DIR}/lib") set(3P_LIBRARY_OUTPUT_PATH "${OpenCV_BINARY_DIR}/3rdparty/lib${LIB_SUFFIX}") + if(WIN32 AND CMAKE_HOST_SYSTEM_NAME MATCHES Windows) if(OpenCV_STATIC) set(OPENCV_LIB_INSTALL_PATH "${OpenCV_INSTALL_BINARIES_PREFIX}staticlib${LIB_SUFFIX}") @@ -338,10 +342,14 @@ else() endif() set(OPENCV_3P_LIB_INSTALL_PATH "${OpenCV_INSTALL_BINARIES_PREFIX}staticlib${LIB_SUFFIX}") set(OPENCV_SAMPLES_SRC_INSTALL_PATH samples/native) + set(OPENCV_JAR_INSTALL_PATH java) + set(OPENCV_OTHER_INSTALL_PATH etc) else() set(OPENCV_LIB_INSTALL_PATH lib${LIB_SUFFIX}) set(OPENCV_3P_LIB_INSTALL_PATH share/OpenCV/3rdparty/${OPENCV_LIB_INSTALL_PATH}) set(OPENCV_SAMPLES_SRC_INSTALL_PATH share/OpenCV/samples) + set(OPENCV_JAR_INSTALL_PATH share/OpenCV/java) + set(OPENCV_OTHER_INSTALL_PATH share/OpenCV) endif() set(OPENCV_INCLUDE_INSTALL_PATH "include") @@ -358,8 +366,16 @@ set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) if(INSTALL_TO_MANGLED_PATHS) set(OPENCV_INCLUDE_INSTALL_PATH ${OPENCV_INCLUDE_INSTALL_PATH}/opencv-${OPENCV_VERSION}) + string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OPENCV_3P_LIB_INSTALL_PATH "${OPENCV_3P_LIB_INSTALL_PATH}") + string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OPENCV_SAMPLES_SRC_INSTALL_PATH "${OPENCV_SAMPLES_SRC_INSTALL_PATH}") + string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OPENCV_CONFIG_INSTALL_PATH "${OPENCV_CONFIG_INSTALL_PATH}") + string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OPENCV_DOC_INSTALL_PATH "${OPENCV_DOC_INSTALL_PATH}") + string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OPENCV_JAR_INSTALL_PATH "${OPENCV_JAR_INSTALL_PATH}") + string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OPENCV_TEST_DATA_INSTALL_PATH "${OPENCV_TEST_DATA_INSTALL_PATH}") + string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OPENCV_OTHER_INSTALL_PATH "${OPENCV_OTHER_INSTALL_PATH}") endif() + if(WIN32) # Postfix of DLLs: set(OPENCV_DLLVERSION "${OPENCV_VERSION_MAJOR}${OPENCV_VERSION_MINOR}${OPENCV_VERSION_PATCH}") diff --git a/cmake/OpenCVGenConfig.cmake b/cmake/OpenCVGenConfig.cmake index 249479d39..ae8fc8939 100644 --- a/cmake/OpenCVGenConfig.cmake +++ b/cmake/OpenCVGenConfig.cmake @@ -101,10 +101,7 @@ configure_file("${OpenCV_SOURCE_DIR}/cmake/templates/OpenCVConfig-version.cmake. set(OpenCV_INCLUDE_DIRS_CONFIGCMAKE "\"\${OpenCV_INSTALL_PATH}/${OPENCV_INCLUDE_INSTALL_PATH}/opencv" "\${OpenCV_INSTALL_PATH}/${OPENCV_INCLUDE_INSTALL_PATH}\"") set(OpenCV2_INCLUDE_DIRS_CONFIGCMAKE "\"\"") -if(INSTALL_TO_MANGLED_PATHS) - string(REPLACE "OpenCV" "OpenCV-${OPENCV_VERSION}" OpenCV_3RDPARTY_LIB_DIRS_CONFIGCMAKE "${OPENCV_3P_LIB_INSTALL_PATH}") - set(OpenCV_3RDPARTY_LIB_DIRS_CONFIGCMAKE "\"\${OpenCV_INSTALL_PATH}/${OpenCV_3RDPARTY_LIB_DIRS_CONFIGCMAKE}\"") -endif() +set(OpenCV_3RDPARTY_LIB_DIRS_CONFIGCMAKE "\"\${OpenCV_INSTALL_PATH}/${OPENCV_3P_LIB_INSTALL_PATH}\"") if(UNIX) # ANDROID configuration is created here also #http://www.vtk.org/Wiki/CMake/Tutorials/Packaging reference @@ -114,23 +111,13 @@ if(UNIX) # ANDROID configuration is created here also # /(share|lib)/*/ (U) # /(share|lib)/*/(cmake|CMake)/ (U) if(USE_IPPICV) - if(INSTALL_TO_MANGLED_PATHS) - file(RELATIVE_PATH INSTALL_PATH_RELATIVE_IPPICV "${CMAKE_INSTALL_PREFIX}/${OPENCV_CONFIG_INSTALL_PATH}-${OPENCV_VERSION}/" ${IPPICV_INSTALL_PATH}) - else() - file(RELATIVE_PATH INSTALL_PATH_RELATIVE_IPPICV "${CMAKE_INSTALL_PREFIX}/${OPENCV_CONFIG_INSTALL_PATH}/" ${IPPICV_INSTALL_PATH}) - endif() + file(RELATIVE_PATH INSTALL_PATH_RELATIVE_IPPICV "${CMAKE_INSTALL_PREFIX}/${OPENCV_CONFIG_INSTALL_PATH}/" ${IPPICV_INSTALL_PATH}) endif() configure_file("${OpenCV_SOURCE_DIR}/cmake/templates/OpenCVConfig.cmake.in" "${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig.cmake" @ONLY) configure_file("${OpenCV_SOURCE_DIR}/cmake/templates/OpenCVConfig-version.cmake.in" "${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig-version.cmake" @ONLY) - if(INSTALL_TO_MANGLED_PATHS) - install(FILES ${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig.cmake DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}-${OPENCV_VERSION}/ COMPONENT dev) - install(FILES ${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig-version.cmake DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}-${OPENCV_VERSION}/ COMPONENT dev) - install(EXPORT OpenCVModules DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}-${OPENCV_VERSION}/ FILE OpenCVModules${modules_file_suffix}.cmake COMPONENT dev) - else() - install(FILES "${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig.cmake" DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}/ COMPONENT dev) - install(FILES ${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig-version.cmake DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}/ COMPONENT dev) - install(EXPORT OpenCVModules DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}/ FILE OpenCVModules${modules_file_suffix}.cmake COMPONENT dev) - endif() + install(FILES "${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig.cmake" DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}/ COMPONENT dev) + install(FILES ${CMAKE_BINARY_DIR}/unix-install/OpenCVConfig-version.cmake DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}/ COMPONENT dev) + install(EXPORT OpenCVModules DESTINATION ${OPENCV_CONFIG_INSTALL_PATH}/ FILE OpenCVModules${modules_file_suffix}.cmake COMPONENT dev) endif() if(ANDROID) diff --git a/data/CMakeLists.txt b/data/CMakeLists.txt index bc5a0361a..1f0d72008 100644 --- a/data/CMakeLists.txt +++ b/data/CMakeLists.txt @@ -1,14 +1,9 @@ file(GLOB HAAR_CASCADES haarcascades/*.xml) file(GLOB LBP_CASCADES lbpcascades/*.xml) -if(ANDROID) - install(FILES ${HAAR_CASCADES} DESTINATION sdk/etc/haarcascades COMPONENT libs) - install(FILES ${LBP_CASCADES} DESTINATION sdk/etc/lbpcascades COMPONENT libs) -else() - install(FILES ${HAAR_CASCADES} DESTINATION share/OpenCV/haarcascades COMPONENT libs) - install(FILES ${LBP_CASCADES} DESTINATION share/OpenCV/lbpcascades COMPONENT libs) -endif() +install(FILES ${HAAR_CASCADES} DESTINATION ${OPENCV_OTHER_INSTALL_PATH}/haarcascades COMPONENT libs) +install(FILES ${LBP_CASCADES} DESTINATION ${OPENCV_OTHER_INSTALL_PATH}/lbpcascades COMPONENT libs) if(INSTALL_TESTS AND OPENCV_TEST_DATA_PATH) install(DIRECTORY "${OPENCV_TEST_DATA_PATH}/" DESTINATION "${OPENCV_TEST_DATA_INSTALL_PATH}" COMPONENT "tests") -endif() \ No newline at end of file +endif() diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 866134fe1..a7f5372bf 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -27,15 +27,6 @@ if(HAVE_DOC_GENERATOR) set(FIXED_ORDER_MODULES core imgproc imgcodecs videoio highgui video calib3d features2d objdetect ml flann photo stitching) list(REMOVE_ITEM BASE_MODULES ${FIXED_ORDER_MODULES}) set(BASE_MODULES ${FIXED_ORDER_MODULES} ${BASE_MODULES}) - - set(DOC_LIST - "${OpenCV_SOURCE_DIR}/doc/opencv-logo.png" - "${OpenCV_SOURCE_DIR}/doc/opencv-logo2.png" - "${OpenCV_SOURCE_DIR}/doc/opencv-logo-white.png" - "${OpenCV_SOURCE_DIR}/doc/opencv.ico" - "${OpenCV_SOURCE_DIR}/doc/pattern.png" - "${OpenCV_SOURCE_DIR}/doc/acircles_pattern.png") - set(OPTIONAL_DOC_LIST "") endif(HAVE_DOC_GENERATOR) # ========= Doxygen docs ========= @@ -160,18 +151,8 @@ if(BUILD_DOCS AND DOXYGEN_FOUND) COMMAND ${DOXYGEN_EXECUTABLE} ${doxyfile} DEPENDS ${doxyfile} ${rootfile} ${bibfile} ${deps} ) + install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doxygen/html + DESTINATION "${OPENCV_DOC_INSTALL_PATH}" + COMPONENT "docs" OPTIONAL + ) endif() - -if(HAVE_DOC_GENERATOR) - # installation - foreach(f ${DOC_LIST}) - install(FILES "${f}" DESTINATION "${OPENCV_DOC_INSTALL_PATH}" COMPONENT docs) - endforeach() - foreach(f ${OPTIONAL_DOC_LIST}) - install(FILES "${f}" DESTINATION "${OPENCV_DOC_INSTALL_PATH}" OPTIONAL COMPONENT docs) - endforeach() - - # dummy targets - add_custom_target(docs) - add_custom_target(html_docs) -endif(HAVE_DOC_GENERATOR) diff --git a/modules/java/CMakeLists.txt b/modules/java/CMakeLists.txt index e38ec1f19..38f410e64 100644 --- a/modules/java/CMakeLists.txt +++ b/modules/java/CMakeLists.txt @@ -318,12 +318,7 @@ else(ANDROID) COMMENT "Generating ${JAR_NAME}" ) - if(WIN32) - set(JAR_INSTALL_DIR java) - else(WIN32) - set(JAR_INSTALL_DIR share/OpenCV/java) - endif(WIN32) - install(FILES ${JAR_FILE} OPTIONAL DESTINATION ${JAR_INSTALL_DIR} COMPONENT java) + install(FILES ${JAR_FILE} OPTIONAL DESTINATION ${OPENCV_JAR_INSTALL_PATH} COMPONENT java) endif(ANDROID) # step 5: build native part @@ -398,12 +393,12 @@ if(ANDROID) else() if(NOT INSTALL_CREATE_DISTRIB) ocv_install_target(${the_module} OPTIONAL EXPORT OpenCVModules - RUNTIME DESTINATION ${JAR_INSTALL_DIR} COMPONENT java - LIBRARY DESTINATION ${JAR_INSTALL_DIR} COMPONENT java) + RUNTIME DESTINATION ${OPENCV_JAR_INSTALL_PATH} COMPONENT java + LIBRARY DESTINATION ${OPENCV_JAR_INSTALL_PATH} COMPONENT java) else() ocv_install_target(${the_module} OPTIONAL EXPORT OpenCVModules - RUNTIME DESTINATION ${JAR_INSTALL_DIR}/${OpenCV_ARCH} COMPONENT java - LIBRARY DESTINATION ${JAR_INSTALL_DIR}/${OpenCV_ARCH} COMPONENT java) + RUNTIME DESTINATION ${OPENCV_JAR_INSTALL_PATH}/${OpenCV_ARCH} COMPONENT java + LIBRARY DESTINATION ${OPENCV_JAR_INSTALL_PATH}/${OpenCV_ARCH} COMPONENT java) endif() endif() From 6cef4eba2938ef93ffcd282d65ee8202384d325b Mon Sep 17 00:00:00 2001 From: Maxim Kostin Date: Fri, 13 Mar 2015 14:33:09 +0300 Subject: [PATCH 082/171] Replacing license headers per PR-3700 feedback. Updates #26 --- platforms/winrt/setup_winrt.ps1 | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/platforms/winrt/setup_winrt.ps1 b/platforms/winrt/setup_winrt.ps1 index b7ef93dcd..c205ea2a4 100644 --- a/platforms/winrt/setup_winrt.ps1 +++ b/platforms/winrt/setup_winrt.ps1 @@ -1,17 +1,29 @@ <# -Copyright Microsoft Open Technologies, Inc. -All Rights Reserved -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. +Copyright (c) Microsoft Open Technologies, Inc. +All rights reserved. -You may obtain a copy of the License at -http://www.apache.org/licenses/LICENSE-2.0 +(3-clause BSD License) -THIS CODE IS PROVIDED ON AN *AS IS* BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, -EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION ANY IMPLIED WARRANTIES OR CONDITIONS OF TITLE, -FITNESS FOR A PARTICULAR PURPOSE, MERCHANTABLITY OR NON-INFRINGEMENT. +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: -See the Apache 2 License for the specific language governing permissions and limitations under the License. +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the +following disclaimer. + +2. Redistributions 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. + +3. Neither the name of the copyright holder nor the names of its contributors may 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 COPYRIGHT HOLDER 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. #> [CmdletBinding()] From 7e07220440264f22f15e48f57934cc093b111432 Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Fri, 13 Mar 2015 18:04:23 +0300 Subject: [PATCH 083/171] made the solvepnp a bit more modest; test 5-point configuration instead of 4-point in some cases; reduce the noise in 4-point configurations in other cases --- modules/calib3d/perf/perf_pnp.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/calib3d/perf/perf_pnp.cpp b/modules/calib3d/perf/perf_pnp.cpp index 55584f819..c1c9ab90f 100644 --- a/modules/calib3d/perf/perf_pnp.cpp +++ b/modules/calib3d/perf/perf_pnp.cpp @@ -19,7 +19,7 @@ typedef perf::TestBaseWithParam PointsNum; PERF_TEST_P(PointsNum_Algo, solvePnP, testing::Combine( - testing::Values(4, 3*9, 7*13), //TODO: find why results on 4 points are too unstable + testing::Values(5, 3*9, 7*13), //TODO: find why results on 4 points are too unstable testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP) ) ) @@ -92,7 +92,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, //add noise Mat noise(1, (int)points2d.size(), CV_32FC2); - randu(noise, 0, 0.01); + randu(noise, 0, 0.005); add(points2d, noise, points2d); declare.in(points3d, points2d); @@ -107,7 +107,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, SANITY_CHECK(tvec, 1e-2); } -PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(4, 3*9, 7*13)) +PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(5, 3*9, 7*13)) { int count = GetParam(); From 79f512337d4c249c25c4427370138f4d95801152 Mon Sep 17 00:00:00 2001 From: Maxim Kostin Date: Fri, 13 Mar 2015 19:28:36 +0300 Subject: [PATCH 084/171] Changed encoding from ANSI to UTF8. --- platforms/winrt/setup_winrt.ps1 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/winrt/setup_winrt.ps1 b/platforms/winrt/setup_winrt.ps1 index c205ea2a4..ddd82864d 100644 --- a/platforms/winrt/setup_winrt.ps1 +++ b/platforms/winrt/setup_winrt.ps1 @@ -1,4 +1,4 @@ -<# +<# Copyright (c) Microsoft Open Technologies, Inc. All rights reserved. From 1760078f6721054abf6dae0556fa764f4495baed Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Fri, 13 Mar 2015 20:11:33 +0300 Subject: [PATCH 085/171] trying to make solvePnPSmallPoints pass --- modules/calib3d/perf/perf_pnp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/calib3d/perf/perf_pnp.cpp b/modules/calib3d/perf/perf_pnp.cpp index c1c9ab90f..63c758798 100644 --- a/modules/calib3d/perf/perf_pnp.cpp +++ b/modules/calib3d/perf/perf_pnp.cpp @@ -92,7 +92,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, //add noise Mat noise(1, (int)points2d.size(), CV_32FC2); - randu(noise, 0, 0.005); + randu(noise, 0, 0.001); add(points2d, noise, points2d); declare.in(points3d, points2d); From 3d2c0ed97fdc59a444fe587fded32a02332cc6cf Mon Sep 17 00:00:00 2001 From: Scott Graybill Date: Fri, 13 Mar 2015 17:04:13 -0700 Subject: [PATCH 086/171] Removed check on limits. A common use of HoughLines would be to restrict theta to be between a small negative number and a small positive number, e.g. -pi/16 to pi/16. This wasn't possible with the previous checks. --- modules/imgproc/src/hough.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/modules/imgproc/src/hough.cpp b/modules/imgproc/src/hough.cpp index 5172024b7..e11df27f0 100644 --- a/modules/imgproc/src/hough.cpp +++ b/modules/imgproc/src/hough.cpp @@ -90,11 +90,8 @@ HoughLinesStandard( const Mat& img, float rho, float theta, int width = img.cols; int height = img.rows; - if (max_theta < 0 || max_theta > CV_PI ) { - CV_Error( CV_StsBadArg, "max_theta must fall between 0 and pi" ); - } - if (min_theta < 0 || min_theta > max_theta ) { - CV_Error( CV_StsBadArg, "min_theta must fall between 0 and max_theta" ); + if (max_theta < min_theta ) { + CV_Error( CV_StsBadArg, "max_theta must be greater than min_theta" ); } int numangle = cvRound((max_theta - min_theta) / theta); int numrho = cvRound(((width + height) * 2 + 1) / rho); @@ -178,7 +175,7 @@ HoughLinesStandard( const Mat& img, float rho, float theta, int n = cvFloor(idx*scale) - 1; int r = idx - (n+1)*(numrho+2) - 1; line.rho = (r - (numrho - 1)*0.5f) * rho; - line.angle = n * theta; + line.angle = static_cast(min_theta) + n * theta; lines.push_back(Vec2f(line.rho, line.angle)); } } From dcc2512f581f56fb887ae955ebe828071dee7b92 Mon Sep 17 00:00:00 2001 From: Naer Chang Date: Sat, 14 Mar 2015 02:56:29 -0400 Subject: [PATCH 087/171] issue 3710 added a check to prevent illegal access to operator[] when vector is empty --- modules/objdetect/src/cascadedetect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/objdetect/src/cascadedetect.cpp b/modules/objdetect/src/cascadedetect.cpp index 1c0bbf1b0..e34d5f6ef 100644 --- a/modules/objdetect/src/cascadedetect.cpp +++ b/modules/objdetect/src/cascadedetect.cpp @@ -1268,7 +1268,7 @@ void CascadeClassifierImpl::detectMultiScaleNoGrouping( InputArray _image, std:: scales.push_back((float)factor); } - if( !featureEvaluator->setImage(gray, scales) ) + if ( scales.size() == 0 || !featureEvaluator->setImage(gray, scales) ) return; // OpenCL code From 8ed4bae4dd5b2f6d1f6e80d0887c44432d5cd538 Mon Sep 17 00:00:00 2001 From: Dmitry-Me Date: Sat, 14 Mar 2015 12:50:42 +0300 Subject: [PATCH 088/171] Reduce variable scope, make formatting consistent with surrounding code --- modules/core/src/lapack.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/core/src/lapack.cpp b/modules/core/src/lapack.cpp index 6cfd5baa8..a766e5f2e 100644 --- a/modules/core/src/lapack.cpp +++ b/modules/core/src/lapack.cpp @@ -725,11 +725,11 @@ template static void MatrAXPY( int m, int n, const T1* x, int dx, const T2* a, int inca, T3* y, int dy ) { - int i, j; + int i; for( i = 0; i < m; i++, x += dx, y += dy ) { T2 s = a[i*inca]; - j=0; + int j = 0; #if CV_ENABLE_UNROLLED for(; j <= n - 4; j += 4 ) { From 7e3cc4473884984131d7b8a37e96c6496bba9b7a Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Sat, 14 Mar 2015 12:41:25 -0400 Subject: [PATCH 089/171] More docs on variable accesses. Listed accesses for more functions. --- modules/calib3d/src/rho.cpp | 58 ++++++++++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 14 deletions(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index 1de801720..8ac22df33 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -1124,8 +1124,10 @@ inline int RHO_HEST_REFC::isFinalRefineEnabled(void){ * 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. + * Reads (direct): ctrl.i, ctrl.phEndI, ctrl.phNum, ctrl.phMax + * Reads (callees): None. + * Writes (direct): None. + * Writes (callees): None. */ inline int RHO_HEST_REFC::PROSACPhaseEndReached(void){ @@ -1138,9 +1140,10 @@ inline int RHO_HEST_REFC::PROSACPhaseEndReached(void){ * * Not idempotent. * - * Accesses: - * Read: phNum, phEndFpI, phEndI - * Write: phNum, phEndFpI, phEndI + * Reads (direct): ctrl.phNum, ctrl.phEndFpI, ctrl.phEndI + * Reads (callees): None. + * Writes (direct): ctrl.phNum, ctrl.phEndFpI, ctrl.phEndI + * Writes (callees): None. */ inline void RHO_HEST_REFC::PROSACGoToNextPhase(void){ @@ -1158,6 +1161,11 @@ inline void RHO_HEST_REFC::PROSACGoToNextPhase(void){ * phNum matches. * - Otherwise, select match phNum-1 and select randomly the 3 others out of * the first phNum-1 matches. + * + * Reads (direct): ctrl.i, ctrl.phEndI, ctrl.phNum + * Reads (callees): prng.s + * Writes (direct): ctrl.smpl + * Writes (callees): prng.s */ inline void RHO_HEST_REFC::getPROSACSample(void){ @@ -1172,6 +1180,11 @@ inline void RHO_HEST_REFC::getPROSACSample(void){ /** * Choose, without repetition, sampleSize integers in the range [0, numDataPoints). + * + * Reads (direct): None. + * Reads (callees): prng.s + * Writes (direct): None. + * Writes (callees): prng.s */ inline void RHO_HEST_REFC::rndSmpl(unsigned sampleSize, @@ -1237,6 +1250,11 @@ inline void RHO_HEST_REFC::rndSmpl(unsigned sampleSize, * out bad samples to the optimized GE implementation. * - Second, the geometrical degeneracy test is run, which weeds out most other * bad samples. + * + * Reads (direct): ctrl.smpl, arg.src, arg.dst + * Reads (callees): None. + * Writes (direct): curr.pkdPts + * Writes (callees): None. */ inline int RHO_HEST_REFC::isSampleDegenerate(void){ @@ -1330,6 +1348,11 @@ inline int RHO_HEST_REFC::isSampleDegenerate(void){ /** * Compute homography of matches in gathered, packed sample and output the * current homography. + * + * Reads (direct): None. + * Reads (callees): curr.pkdPts + * Writes (direct): None. + * Writes (callees): curr.H */ inline void RHO_HEST_REFC::generateModel(void){ @@ -1340,6 +1363,11 @@ inline void RHO_HEST_REFC::generateModel(void){ * 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. + * + * Reads (direct): curr.H + * Reads (callees): None. + * Writes (direct): None. + * Writes (callees): None. */ inline int RHO_HEST_REFC::isModelDegenerate(void){ @@ -1359,8 +1387,13 @@ inline int RHO_HEST_REFC::isModelDegenerate(void){ /** * 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 + * Reads (direct): arg.maxD, arg.src, arg.dst, arg.N, curr.inl, curr.H, + * ctrl.numModels, eval.Ntestedtotal, eval.lambdaAccept, + * eval.lambdaReject, eval.A + * Reads (callees): None. + * Writes (direct): ctrl.numModels, curr.numInl, eval.Ntested, eval.good, + * eval.Ntestedtotal + * Writes (callees): None. */ inline void RHO_HEST_REFC::evaluateModelSPRT(void){ @@ -1423,13 +1456,10 @@ inline void RHO_HEST_REFC::evaluateModelSPRT(void){ * 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 - * - * Reads: eval.good, eval.delta, eval.epsilon, eval.t_M, eval.m_S, - * curr.numInl, best.numInl - * Writes: eval.epsilon, eval.delta, eval.A, eval.lambdaAccept, - * eval.lambdaReject + * Reads (direct): eval.good, curr.numInl, arg.N, eval.Ntested, eval.delta + * Reads (callees): eval.delta, eval.epsilon, eval.t_M, eval.m_S + * Writes (direct): eval.epsilon, eval.delta + * Writes (callees): eval.A, eval.lambdaReject, eval.lambdaAccept */ inline void RHO_HEST_REFC::updateSPRT(void){ From cdc9bc397dbb9667cdb742973b1f89e000b315a6 Mon Sep 17 00:00:00 2001 From: Naer Chang Date: Sat, 14 Mar 2015 18:32:46 -0400 Subject: [PATCH 090/171] Revert "issue 3710" Trying to fix whitespace issue. This reverts commit dcc2512f581f56fb887ae955ebe828071dee7b92. --- modules/objdetect/src/cascadedetect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/objdetect/src/cascadedetect.cpp b/modules/objdetect/src/cascadedetect.cpp index e34d5f6ef..af94fac0e 100644 --- a/modules/objdetect/src/cascadedetect.cpp +++ b/modules/objdetect/src/cascadedetect.cpp @@ -1268,7 +1268,7 @@ void CascadeClassifierImpl::detectMultiScaleNoGrouping( InputArray _image, std:: scales.push_back((float)factor); } - if ( scales.size() == 0 || !featureEvaluator->setImage(gray, scales) ) + if( scales.size() == 0 || !featureEvaluator->setImage(gray, scales) ) return; // OpenCL code From 5c352c9146f3c0efa5cbe593873757e15cda98a0 Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Mon, 16 Mar 2015 12:46:24 +0300 Subject: [PATCH 091/171] temporarily use EPNP in SolvePnP instead of UPNP or DLS algorithms, since the latter two are not quite stable --- modules/calib3d/perf/perf_pnp.cpp | 4 ++-- modules/calib3d/src/solvepnp.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/calib3d/perf/perf_pnp.cpp b/modules/calib3d/perf/perf_pnp.cpp index 63c758798..9048f2be6 100644 --- a/modules/calib3d/perf/perf_pnp.cpp +++ b/modules/calib3d/perf/perf_pnp.cpp @@ -64,7 +64,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnP, PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, testing::Combine( - testing::Values(4), //TODO: find why results on 4 points are too unstable + testing::Values(4), testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP) ) ) @@ -92,7 +92,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, //add noise Mat noise(1, (int)points2d.size(), CV_32FC2); - randu(noise, 0, 0.001); + randu(noise, -0.001, 0.001); add(points2d, noise, points2d); declare.in(points3d, points2d); diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 129c10ee7..eeec156df 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -59,9 +59,9 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); _rvec.create(3, 1, CV_64F); _tvec.create(3, 1, CV_64F); - Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); + Mat cameraMatrix = Mat_(_cameraMatrix.getMat()), distCoeffs = Mat_(_distCoeffs.getMat()); - if (flags == SOLVEPNP_EPNP) + if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) { cv::Mat undistortedPoints; cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); @@ -95,7 +95,7 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, &c_rvec, &c_tvec, useExtrinsicGuess ); return true; } - else if (flags == SOLVEPNP_DLS) + /*else if (flags == SOLVEPNP_DLS) { cv::Mat undistortedPoints; cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); @@ -120,7 +120,7 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, else cameraMatrix.at(0,0) = cameraMatrix.at(1,1) = f; return true; - } + }*/ else CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); return false; From ca19ae8b5ae53afe3ca0084c55178b8d0796b28b Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Mon, 16 Mar 2015 16:56:26 +0300 Subject: [PATCH 092/171] in solvePnPRansac call the solvePnP in the end with all the inliers to get more precise estimate --- modules/calib3d/perf/perf_pnp.cpp | 8 ++- modules/calib3d/src/epnp.cpp | 21 ++++--- modules/calib3d/src/epnp.h | 5 ++ modules/calib3d/src/fundam.cpp | 18 +----- modules/calib3d/src/precomp.hpp | 13 +++++ modules/calib3d/src/solvepnp.cpp | 93 ++++++++++++++++++++----------- 6 files changed, 97 insertions(+), 61 deletions(-) diff --git a/modules/calib3d/perf/perf_pnp.cpp b/modules/calib3d/perf/perf_pnp.cpp index 9048f2be6..e5a92bf1c 100644 --- a/modules/calib3d/perf/perf_pnp.cpp +++ b/modules/calib3d/perf/perf_pnp.cpp @@ -20,7 +20,7 @@ typedef perf::TestBaseWithParam PointsNum; PERF_TEST_P(PointsNum_Algo, solvePnP, testing::Combine( testing::Values(5, 3*9, 7*13), //TODO: find why results on 4 points are too unstable - testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP) + testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS) ) ) { @@ -64,13 +64,15 @@ PERF_TEST_P(PointsNum_Algo, solvePnP, PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, testing::Combine( - testing::Values(4), - testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP) + testing::Values(5), + testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP) ) ) { int pointsNum = get<0>(GetParam()); pnpAlgo algo = get<1>(GetParam()); + if( algo == SOLVEPNP_P3P ) + pointsNum = 4; vector points2d(pointsNum); vector points3d(pointsNum); diff --git a/modules/calib3d/src/epnp.cpp b/modules/calib3d/src/epnp.cpp index edbcaffd3..ec7dfe0ad 100644 --- a/modules/calib3d/src/epnp.cpp +++ b/modules/calib3d/src/epnp.cpp @@ -2,7 +2,10 @@ #include "precomp.hpp" #include "epnp.h" -epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints) +namespace cv +{ + +epnp::epnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints) { if (cameraMatrix.depth() == CV_32F) init_camera_parameters(cameraMatrix); @@ -17,14 +20,14 @@ epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& i if (opoints.depth() == ipoints.depth()) { if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); + init_points(opoints, ipoints); else - init_points(opoints, ipoints); + init_points(opoints, ipoints); } else if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); + init_points(opoints, ipoints); else - init_points(opoints, ipoints); + init_points(opoints, ipoints); alphas.resize(4 * number_of_correspondences); pcs.resize(3 * number_of_correspondences); @@ -144,7 +147,7 @@ void epnp::compute_pcs(void) } } -void epnp::compute_pose(cv::Mat& R, cv::Mat& t) +void epnp::compute_pose(Mat& R, Mat& t) { choose_control_points(); compute_barycentric_coordinates(); @@ -189,8 +192,8 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t) if (rep_errors[2] < rep_errors[1]) N = 2; if (rep_errors[3] < rep_errors[N]) N = 3; - cv::Mat(3, 1, CV_64F, ts[N]).copyTo(t); - cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R); + Mat(3, 1, CV_64F, ts[N]).copyTo(t); + Mat(3, 3, CV_64F, Rs[N]).copyTo(R); } void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3], @@ -621,3 +624,5 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) pX[i] = (pb[i] - sum) / A2[i]; } } + +} diff --git a/modules/calib3d/src/epnp.h b/modules/calib3d/src/epnp.h index 2619f7595..350e9d482 100644 --- a/modules/calib3d/src/epnp.h +++ b/modules/calib3d/src/epnp.h @@ -4,6 +4,9 @@ #include "precomp.hpp" #include "opencv2/core/core_c.h" +namespace cv +{ + class epnp { public: epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints); @@ -78,4 +81,6 @@ class epnp { double * A1, * A2; }; +} + #endif diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index c700ece70..bc5c77b42 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/modules/calib3d/src/fundam.cpp @@ -69,20 +69,6 @@ static bool haveCollinearPoints( const Mat& m, int count ) } -template int compressPoints( T* ptr, const uchar* mask, int mstep, int count ) -{ - int i, j; - for( i = j = 0; i < count; i++ ) - if( mask[i*mstep] ) - { - if( i > j ) - ptr[j] = ptr[i]; - j++; - } - return j; -} - - class HomographyEstimatorCallback : public PointSetRegistrator::Callback { public: @@ -322,8 +308,8 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2, if( result && npoints > 4 ) { - compressPoints( src.ptr(), tempMask.ptr(), 1, npoints ); - npoints = compressPoints( dst.ptr(), tempMask.ptr(), 1, npoints ); + compressElems( src.ptr(), tempMask.ptr(), 1, npoints ); + npoints = compressElems( dst.ptr(), tempMask.ptr(), 1, npoints ); if( npoints > 0 ) { Mat src1 = src.rowRange(0, npoints); diff --git a/modules/calib3d/src/precomp.hpp b/modules/calib3d/src/precomp.hpp index e8a81120f..83a513dca 100644 --- a/modules/calib3d/src/precomp.hpp +++ b/modules/calib3d/src/precomp.hpp @@ -102,6 +102,19 @@ CV_EXPORTS Ptr createRANSACPointSetRegistrator(const Ptr createLMeDSPointSetRegistrator(const Ptr& cb, int modelPoints, double confidence=0.99, int maxIters=1000 ); +template inline int compressElems( T* ptr, const uchar* mask, int mstep, int count ) +{ + int i, j; + for( i = j = 0; i < count; i++ ) + if( mask[i*mstep] ) + { + if( i > j ) + ptr[j] = ptr[i]; + j++; + } + return j; +} + } #endif diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index eeec156df..dd5c5eb8c 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -48,11 +48,13 @@ #include "opencv2/calib3d/calib3d_c.h" #include -using namespace cv; -bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, - InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) +namespace cv +{ + +bool solvePnP( InputArray _opoints, InputArray _ipoints, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) { Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); @@ -63,26 +65,26 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) { - cv::Mat undistortedPoints; - cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); epnp PnP(cameraMatrix, opoints, undistortedPoints); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); PnP.compute_pose(R, tvec); - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); return true; } else if (flags == SOLVEPNP_P3P) { CV_Assert( npoints == 4); - cv::Mat undistortedPoints; - cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); p3p P3Psolver(cameraMatrix); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); if (result) - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); return result; } else if (flags == SOLVEPNP_ITERATIVE) @@ -97,24 +99,24 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, } /*else if (flags == SOLVEPNP_DLS) { - cv::Mat undistortedPoints; - cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + Mat undistortedPoints; + undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); dls PnP(opoints, undistortedPoints); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); bool result = PnP.compute_pose(R, tvec); if (result) - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); return result; } else if (flags == SOLVEPNP_UPNP) { upnp PnP(cameraMatrix, opoints, ipoints); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); double f = PnP.compute_pose(R, tvec); - cv::Rodrigues(R, rvec); + Rodrigues(R, rvec); if(cameraMatrix.type() == CV_32F) cameraMatrix.at(0,0) = cameraMatrix.at(1,1) = (float)f; else @@ -131,7 +133,7 @@ class PnPRansacCallback : public PointSetRegistrator::Callback public: - PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::SOLVEPNP_ITERATIVE, + PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=SOLVEPNP_ITERATIVE, bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() ) : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess), rvec(_rvec), tvec(_tvec) {} @@ -142,12 +144,11 @@ public: { Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); - - bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs, + bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags ); Mat _local_model; - cv::hconcat(rvec, tvec, _local_model); + hconcat(rvec, tvec, _local_model); _local_model.copyTo(_model); return correspondence; @@ -166,7 +167,7 @@ public: Mat projpoints(count, 2, CV_32FC1); - cv::projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); + projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); const Point2f* ipoints_ptr = ipoints.ptr(); const Point2f* projpoints_ptr = projpoints.ptr(); @@ -175,7 +176,7 @@ public: float* err = _err.getMat().ptr(); for ( i = 0; i < count; ++i) - err[i] = (float)cv::norm( ipoints_ptr[i] - projpoints_ptr[i] ); + err[i] = (float)norm( ipoints_ptr[i] - projpoints_ptr[i] ); } @@ -188,7 +189,7 @@ public: Mat tvec; }; -bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, +bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, @@ -214,23 +215,45 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); - Ptr cb; // pointer to callback - cb = makePtr( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec); + int model_points = 5; + int ransac_kernel_method = SOLVEPNP_EPNP; - int model_points = 4; // minimum of number of model points - if( flags == cv::SOLVEPNP_ITERATIVE ) model_points = 6; - else if( flags == cv::SOLVEPNP_UPNP ) model_points = 6; - else if( flags == cv::SOLVEPNP_EPNP ) model_points = 5; + if( npoints == 4 ) + { + model_points = 4; + ransac_kernel_method = SOLVEPNP_P3P; + } + + Ptr cb; // pointer to callback + cb = makePtr( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec); double param1 = reprojectionError; // reprojection error double param2 = confidence; // confidence int param3 = iterationsCount; // number maximum iterations - cv::Mat _local_model(3, 2, CV_64FC1); - cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); + Mat _local_model(3, 2, CV_64FC1); + Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); // call Ransac - int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); + int result = createRANSACPointSetRegistrator(cb, model_points, + param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); + + if( result > 0 ) + { + vector opoints_inliers; + vector ipoints_inliers; + opoints.convertTo(opoints_inliers, CV_64F); + ipoints.convertTo(ipoints_inliers, CV_64F); + + const uchar* mask = _mask_local_inliers.ptr(); + int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints); + compressElems(&ipoints_inliers[0], mask, 1, npoints); + + opoints_inliers.resize(npoints1); + ipoints_inliers.resize(npoints1); + result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, + distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1; + } if( result <= 0 || _local_model.rows <= 0) { @@ -260,3 +283,5 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, } return true; } + +} From 52a8d37f111298672aafbfb571dbf8439625f468 Mon Sep 17 00:00:00 2001 From: Dmitry-Me Date: Mon, 16 Mar 2015 18:36:12 +0300 Subject: [PATCH 093/171] Fix potential null pointer dereference --- modules/calib3d/src/calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 5a86624c5..70f2aafed 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1595,7 +1595,10 @@ void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize, my = imgHeight / apertureHeight; } else { mx = 1.0; - my = *pasp; + if(pasp) + my = *pasp; + else + my = 1.0; } /* Calculate fovx and fovy. */ From d0ad599a24bb56433ebc99e171759ab998fae44b Mon Sep 17 00:00:00 2001 From: StevenPuttemans Date: Tue, 17 Mar 2015 09:00:41 +0100 Subject: [PATCH 094/171] update traincascade assigned memory buffers --- apps/traincascade/traincascade.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/traincascade/traincascade.cpp b/apps/traincascade/traincascade.cpp index d1c3e4e87..f77f30dec 100644 --- a/apps/traincascade/traincascade.cpp +++ b/apps/traincascade/traincascade.cpp @@ -12,8 +12,8 @@ int main( int argc, char* argv[] ) int numNeg = 1000; int numStages = 20; int numThreads = getNumThreads(); - int precalcValBufSize = 256, - precalcIdxBufSize = 256; + int precalcValBufSize = 1024, + precalcIdxBufSize = 1024; bool baseFormatSave = false; CvCascadeParams cascadeParams; From 0c5faa6d24ceede74d2297497ebba846cb1df2cf Mon Sep 17 00:00:00 2001 From: Dmitry-Me Date: Tue, 17 Mar 2015 14:47:00 +0300 Subject: [PATCH 095/171] Fix uninitialized members, fix log output typo --- modules/androidcamera/camera_wrapper/camera_wrapper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/modules/androidcamera/camera_wrapper/camera_wrapper.cpp b/modules/androidcamera/camera_wrapper/camera_wrapper.cpp index 2a7d01b22..66678c558 100644 --- a/modules/androidcamera/camera_wrapper/camera_wrapper.cpp +++ b/modules/androidcamera/camera_wrapper/camera_wrapper.cpp @@ -314,7 +314,9 @@ public: cameraId(0), cameraCallback(callback), userData(_userData), - emptyCameraCallbackReported(0) + emptyCameraCallbackReported(0), + width(), + height() { LOGD("Instantiated new CameraHandler (%p, %p)", callback, _userData); void* params_buffer = operator new(sizeof(CameraParameters) + MAGIC_TAIL); @@ -1122,7 +1124,7 @@ void CameraHandler::applyProperties(CameraHandler** ppcameraHandler) if (handler == NULL) { LOGE("ERROR in applyProperties --- cannot reinit camera"); handler=initCameraConnect(cameraCallback, cameraId, userData, NULL); - LOGD("CameraHandler::applyProperties(): repeate initCameraConnect after ERROR, handler=0x%x", (int)handler); + LOGD("CameraHandler::applyProperties(): repeat initCameraConnect after ERROR, handler=0x%x", (int)handler); if (handler == NULL) { LOGE("ERROR in applyProperties --- cannot reinit camera AGAIN --- cannot do anything else"); } From 9fbc92aace761ad950339f81dd661ce8c3d50fe3 Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Mon, 16 Mar 2015 12:45:21 +0300 Subject: [PATCH 096/171] Added cmake option for abi descriptor generating (GENERATE_ABI_DESCRIPTOR) --- CMakeLists.txt | 4 ++++ cmake/OpenCVGenABI.cmake | 29 ++++++++++++++++++++++ cmake/templates/opencv_abi.xml.in | 40 +++++++++++++++++++++++++++++++ include/opencv/cvaux.h | 6 ----- 4 files changed, 73 insertions(+), 6 deletions(-) create mode 100644 cmake/OpenCVGenABI.cmake create mode 100644 cmake/templates/opencv_abi.xml.in diff --git a/CMakeLists.txt b/CMakeLists.txt index 20a1f6fd8..d06ef1acc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -244,6 +244,7 @@ OCV_OPTION(ENABLE_NOISY_WARNINGS "Show all warnings even if they are too no OCV_OPTION(OPENCV_WARNINGS_ARE_ERRORS "Treat warnings as errors" OFF ) OCV_OPTION(ANDROID_EXAMPLES_WITH_LIBS "Build binaries of Android examples with native libraries" OFF IF ANDROID ) OCV_OPTION(ENABLE_IMPL_COLLECTION "Collect implementation data on function call" OFF ) +OCV_OPTION(GENERATE_ABI_DESCRIPTOR "Generate XML file for abi_compliance_checker tool" OFF IF UNIX) if(ENABLE_IMPL_COLLECTION) add_definitions(-DCV_COLLECT_IMPL_DATA) @@ -639,6 +640,9 @@ include(cmake/OpenCVGenConfig.cmake) # Generate Info.plist for the IOS framework include(cmake/OpenCVGenInfoPlist.cmake) +# Generate ABI descriptor +include(cmake/OpenCVGenABI.cmake) + # Generate environment setup file if(INSTALL_TESTS AND OPENCV_TEST_DATA_PATH) if(ANDROID) diff --git a/cmake/OpenCVGenABI.cmake b/cmake/OpenCVGenABI.cmake new file mode 100644 index 000000000..ebf5bd655 --- /dev/null +++ b/cmake/OpenCVGenABI.cmake @@ -0,0 +1,29 @@ +if (NOT GENERATE_ABI_DESCRIPTOR) + return() +endif() + +set(filename "opencv_abi.xml") +set(path1 "${CMAKE_BINARY_DIR}/${filename}") + +set(modules "${OPENCV_MODULES_PUBLIC}") +ocv_list_filterout(modules "opencv_ts") + +message(STATUS "Generating ABI compliance checker configuration: ${filename}") + +if (OPENCV_VCSVERSION AND NOT OPENCV_VCSVERSION STREQUAL "unknown") + set(OPENCV_ABI_VERSION "${OPENCV_VCSVERSION}") +else() + set(OPENCV_ABI_VERSION "${OPENCV_VERSION}") +endif() + +# Headers +set(OPENCV_ABI_HEADERS "{RELPATH}/${OPENCV_INCLUDE_INSTALL_PATH}") + +# Libraries +set(OPENCV_ABI_LIBRARIES "{RELPATH}/${OPENCV_LIB_INSTALL_PATH}") + +# Options +set(OPENCV_ABI_GCC_OPTIONS "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_RELEASE}") +string(REGEX REPLACE "([^ ]) +([^ ])" "\\1\\n \\2" OPENCV_ABI_GCC_OPTIONS "${OPENCV_ABI_GCC_OPTIONS}") + +configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/templates/opencv_abi.xml.in" "${path1}") diff --git a/cmake/templates/opencv_abi.xml.in b/cmake/templates/opencv_abi.xml.in new file mode 100644 index 000000000..49702cc7b --- /dev/null +++ b/cmake/templates/opencv_abi.xml.in @@ -0,0 +1,40 @@ + + + + + + + + @OPENCV_ABI_VERSION@ + + + + @OPENCV_ABI_HEADERS@ + + + + @OPENCV_ABI_LIBRARIES@ + + + + opencv2/core/cuda* + opencv2/core/private* + opencv/cxeigen.hpp + opencv2/core/eigen.hpp + opencv2/flann/hdf5.h + opencv2/imgcodecs/ios.h + opencv2/videoio/cap_ios.h + opencv2/ts.hpp + opencv2/ts/* + opencv2/xobjdetect/private.hpp + + + + @OPENCV_ABI_GCC_OPTIONS@ + + + diff --git a/include/opencv/cvaux.h b/include/opencv/cvaux.h index cb49c086b..fe86c5d98 100644 --- a/include/opencv/cvaux.h +++ b/include/opencv/cvaux.h @@ -51,12 +51,6 @@ #include "opencv2/photo/photo_c.h" #include "opencv2/video/tracking_c.h" #include "opencv2/objdetect/objdetect_c.h" -#include "opencv2/contrib/compat.hpp" - -#include "opencv2/legacy.hpp" -#include "opencv2/legacy/compat.hpp" -#include "opencv2/legacy/blobtrack.hpp" - #endif From e0efda714500b3645d8ab47086d7419f23ff0d82 Mon Sep 17 00:00:00 2001 From: Philipp Hasper Date: Tue, 17 Mar 2015 15:28:37 +0100 Subject: [PATCH 097/171] Fixed persistent.hpp documentation Small error - object fs does not exist in this code example --- modules/core/include/opencv2/core/persistence.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/core/include/opencv2/core/persistence.hpp b/modules/core/include/opencv2/core/persistence.hpp index fd691cc6c..eec304d6b 100644 --- a/modules/core/include/opencv2/core/persistence.hpp +++ b/modules/core/include/opencv2/core/persistence.hpp @@ -257,7 +257,7 @@ Here is how to read the file created by the code sample above: cout << " " << (int)lbpval[i]; cout << ")" << endl; } - fs.release(); + fs2.release(); @endcode Format specification {#format_spec} From 6d27d488bf98bbf9698787f5aea33e9f9747026e Mon Sep 17 00:00:00 2001 From: Olexa Bilaniuk Date: Wed, 18 Mar 2015 02:21:16 -0400 Subject: [PATCH 098/171] Bugfix in n* optimization. Similar to the problem in LevMarq, arg.inl was being used instead of best.inl. This opened us up to a potential segfault. --- modules/calib3d/src/rho.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index 8ac22df33..81d641c3f 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -1664,7 +1664,7 @@ inline void RHO_HEST_REFC::nStarOptimize(void){ best_n = test_n; bestNumInl = testNumInl; } - testNumInl -= !!arg.inl[test_n-1];/* FIXME: Probable bug! */ + testNumInl -= !!best.inl[test_n-1]; } if(bestNumInl*ctrl.phMax > ctrl.phNumInl*best_n){ From d1a81710119ee3c3da661b4a13e4abe64d3d05ed Mon Sep 17 00:00:00 2001 From: Maxim Kostin Date: Wed, 18 Mar 2015 15:13:08 +0300 Subject: [PATCH 099/171] Added WinRT concurrency support. --- modules/core/src/parallel.cpp | 33 ++++++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/modules/core/src/parallel.cpp b/modules/core/src/parallel.cpp index 4dde2adea..582c7cd03 100644 --- a/modules/core/src/parallel.cpp +++ b/modules/core/src/parallel.cpp @@ -69,7 +69,7 @@ #define HAVE_GCD #endif -#if defined _MSC_VER && _MSC_VER >= 1600 && !defined(WINRT) +#if defined _MSC_VER && _MSC_VER >= 1600 #define HAVE_CONCURRENCY #endif @@ -78,7 +78,8 @@ 2. HAVE_CSTRIPES - 3rdparty library, should be explicitly enabled 3. HAVE_OPENMP - integrated to compiler, should be explicitly enabled 4. HAVE_GCD - system wide, used automatically (APPLE only) - 5. HAVE_CONCURRENCY - part of runtime, used automatically (Windows only - MSVS 10, MSVS 11) + 5. WINRT - system wide, used automatically (Windows RT only) + 6. HAVE_CONCURRENCY - part of runtime, used automatically (Windows only - MSVS 10, MSVS 11) */ #if defined HAVE_TBB @@ -105,6 +106,8 @@ #elif defined HAVE_GCD #include #include + #elif defined WINRT + #include #elif defined HAVE_CONCURRENCY #include #endif @@ -118,6 +121,8 @@ # define CV_PARALLEL_FRAMEWORK "openmp" #elif defined HAVE_GCD # define CV_PARALLEL_FRAMEWORK "gcd" +#elif defined WINRT +# define CV_PARALLEL_FRAMEWORK "winrt-concurrency" #elif defined HAVE_CONCURRENCY # define CV_PARALLEL_FRAMEWORK "ms-concurrency" #endif @@ -179,7 +184,7 @@ namespace ProxyLoopBody* ptr_body = static_cast(context); (*ptr_body)(cv::Range((int)index, (int)index + 1)); } -#elif defined HAVE_CONCURRENCY +#elif defined WINRT || defined HAVE_CONCURRENCY class ProxyLoopBody : public ParallelLoopBodyWrapper { public: @@ -206,7 +211,10 @@ static tbb::task_scheduler_init tbbScheduler(tbb::task_scheduler_init::deferred) static int numThreadsMax = omp_get_max_threads(); #elif defined HAVE_GCD // nothing for GCD +#elif defined WINRT +// nothing for WINRT #elif defined HAVE_CONCURRENCY + class SchedPtr { Concurrency::Scheduler* sched_; @@ -224,6 +232,7 @@ public: ~SchedPtr() { *this = 0; } }; static SchedPtr pplScheduler; + #endif #endif // CV_PARALLEL_FRAMEWORK @@ -272,6 +281,10 @@ void cv::parallel_for_(const cv::Range& range, const cv::ParallelLoopBody& body, dispatch_queue_t concurrent_queue = dispatch_get_global_queue(DISPATCH_QUEUE_PRIORITY_DEFAULT, 0); dispatch_apply_f(stripeRange.end - stripeRange.start, concurrent_queue, &pbody, block_function); +#elif defined WINRT + + Concurrency::parallel_for(stripeRange.start, stripeRange.end, pbody); + #elif defined HAVE_CONCURRENCY if(!pplScheduler || pplScheduler->Id() == Concurrency::CurrentScheduler::Id()) @@ -330,11 +343,15 @@ int cv::getNumThreads(void) return 512; // the GCD thread pool limit +#elif defined WINRT + + return 0; + #elif defined HAVE_CONCURRENCY return 1 + (pplScheduler == 0 - ? Concurrency::CurrentScheduler::Get()->GetNumberOfVirtualProcessors() - : pplScheduler->GetNumberOfVirtualProcessors()); + ? Concurrency::CurrentScheduler::Get()->GetNumberOfVirtualProcessors() + : pplScheduler->GetNumberOfVirtualProcessors()); #else @@ -371,6 +388,10 @@ void cv::setNumThreads( int threads ) // unsupported // there is only private dispatch_queue_set_width() and only for desktop +#elif defined WINRT + + return; + #elif defined HAVE_CONCURRENCY if (threads <= 0) @@ -407,6 +428,8 @@ int cv::getThreadNum(void) return omp_get_thread_num(); #elif defined HAVE_GCD return (int)(size_t)(void*)pthread_self(); // no zero-based indexing +#elif defined WINRT + return 0; #elif defined HAVE_CONCURRENCY return std::max(0, (int)Concurrency::Context::VirtualProcessorId()); // zero for master thread, unique number for others but not necessary 1,2,3,... #else From 5ec908c5f0651ccb5928deed84b54efcd9ccf986 Mon Sep 17 00:00:00 2001 From: Maxim Kostin Date: Wed, 18 Mar 2015 16:31:27 +0300 Subject: [PATCH 100/171] Added WinRT 8.1 Modern Desktop JavaScript sample --- .../OcvTransform/OcvTransform.cpp | 23 +- .../JavaScript/MediaCaptureJavaScript.jsproj | 115 ++++++++++ .../JavaScript/MediaCaptureJavaScript.sln | 84 +++++++ .../MediaCaptureJavaScript_TemporaryKey.pfx | Bin 0 -> 2512 bytes samples/winrt/JavaScript/css/default.css | 6 + samples/winrt/JavaScript/default.html | 31 +++ .../JavaScript/html/AdvancedCapture.html | 40 ++++ .../JavaScript/images/logo.scale-100.png | Bin 0 -> 4609 bytes .../winrt/JavaScript/images/microsoft-sdk.png | Bin 0 -> 3405 bytes .../winrt/JavaScript/images/smallTile-sdk.png | Bin 0 -> 1248 bytes .../JavaScript/images/smalllogo.scale-100.png | Bin 0 -> 1099 bytes .../winrt/JavaScript/images/splash-sdk.png | Bin 0 -> 5068 bytes .../images/splashscreen.scale-100.png | Bin 0 -> 2146 bytes .../JavaScript/images/squareTile-sdk.png | Bin 0 -> 2482 bytes .../winrt/JavaScript/images/storeLogo-sdk.png | Bin 0 -> 1550 bytes .../JavaScript/images/storelogo.scale-100.png | Bin 0 -> 429 bytes samples/winrt/JavaScript/images/tile-sdk.png | Bin 0 -> 2665 bytes .../winrt/JavaScript/images/windows-sdk.png | Bin 0 -> 1426 bytes .../winrt/JavaScript/js/AdvancedCapture.js | 161 +++++++++++++ samples/winrt/JavaScript/js/default.js | 74 ++++++ samples/winrt/JavaScript/package.appxmanifest | 35 +++ .../JavaScript/sample-utils/sample-utils.css | 215 ++++++++++++++++++ .../JavaScript/sample-utils/sample-utils.js | 204 +++++++++++++++++ .../sample-utils/scenario-select.html | 15 ++ 24 files changed, 997 insertions(+), 6 deletions(-) create mode 100644 samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj create mode 100644 samples/winrt/JavaScript/MediaCaptureJavaScript.sln create mode 100644 samples/winrt/JavaScript/MediaCaptureJavaScript_TemporaryKey.pfx create mode 100644 samples/winrt/JavaScript/css/default.css create mode 100644 samples/winrt/JavaScript/default.html create mode 100644 samples/winrt/JavaScript/html/AdvancedCapture.html create mode 100644 samples/winrt/JavaScript/images/logo.scale-100.png create mode 100644 samples/winrt/JavaScript/images/microsoft-sdk.png create mode 100644 samples/winrt/JavaScript/images/smallTile-sdk.png create mode 100644 samples/winrt/JavaScript/images/smalllogo.scale-100.png create mode 100644 samples/winrt/JavaScript/images/splash-sdk.png create mode 100644 samples/winrt/JavaScript/images/splashscreen.scale-100.png create mode 100644 samples/winrt/JavaScript/images/squareTile-sdk.png create mode 100644 samples/winrt/JavaScript/images/storeLogo-sdk.png create mode 100644 samples/winrt/JavaScript/images/storelogo.scale-100.png create mode 100644 samples/winrt/JavaScript/images/tile-sdk.png create mode 100644 samples/winrt/JavaScript/images/windows-sdk.png create mode 100644 samples/winrt/JavaScript/js/AdvancedCapture.js create mode 100644 samples/winrt/JavaScript/js/default.js create mode 100644 samples/winrt/JavaScript/package.appxmanifest create mode 100644 samples/winrt/JavaScript/sample-utils/sample-utils.css create mode 100644 samples/winrt/JavaScript/sample-utils/sample-utils.js create mode 100644 samples/winrt/JavaScript/sample-utils/scenario-select.html diff --git a/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp b/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp index 438b2c6d1..40335e2cd 100644 --- a/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp +++ b/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp @@ -120,13 +120,24 @@ HRESULT OcvImageManipulations::SetProperties(ABI::Windows::Foundation::Collectio if (found) { - IInspectable* value; - spSetting->Lookup(key, &value); + Microsoft::WRL::ComPtr spPropVal; + Microsoft::WRL::ComPtr spInsp; + + spSetting->Lookup(key, spInsp.ReleaseAndGetAddressOf()); + + hr = spInsp.As(&spPropVal); + if (hr != S_OK) + { + return hr; + } + + INT32 effect; + hr = spPropVal->GetInt32(&effect); + if (hr != S_OK) + { + return hr; + } - Microsoft::WRL::ComPtr> ref; - hr = value->QueryInterface(IID_PPV_ARGS(&ref)); - int effect = InvalidEffect; - hr = ref->get_Value(&effect); if ((effect >= 0) && (effect < InvalidEffect)) { m_TransformType = (ProcessingType)effect; diff --git a/samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj b/samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj new file mode 100644 index 000000000..56e6936ac --- /dev/null +++ b/samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj @@ -0,0 +1,115 @@ + + + + + Debug + AnyCPU + + + Debug + ARM + + + Debug + x64 + + + Debug + x86 + + + Release + AnyCPU + + + Release + ARM + + + Release + x64 + + + Release + x86 + + + + d70a3790-48ce-4e58-af60-ebefc22e9c7a + + + + 12.0 + + + + + Windows + 8.1 + 8.1 + $(VersionNumberMajor).$(VersionNumberMinor) + en-US + MediaCaptureJavaScript_TemporaryKey.pfx + + + + Designer + + + + + + + + + + + + + + + + + + + + + + + + + + + $(OPENCV_WINRT_INSTALL_DIR)WS\8.1\$(PlatformTarget)\$(PlatformTarget)\vc12\bin\ + $(OPENCV_WINRT_INSTALL_DIR)WS\8.1\$(PlatformTarget)\$(PlatformTarget)\vc12\lib\ + $(OPENCV_WINRT_INSTALL_DIR)WS\8.1\$(PlatformTarget)\include\ + + d + + + + + + + PreserveNewest + + + PreserveNewest + + + + + + + + \ No newline at end of file diff --git a/samples/winrt/JavaScript/MediaCaptureJavaScript.sln b/samples/winrt/JavaScript/MediaCaptureJavaScript.sln new file mode 100644 index 000000000..cb5c347fa --- /dev/null +++ b/samples/winrt/JavaScript/MediaCaptureJavaScript.sln @@ -0,0 +1,84 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 2013 +VisualStudioVersion = 12.0.31101.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "OcvTransform", "..\ImageManipulations\MediaExtensions\OcvTransform\OcvTransform.vcxproj", "{BA69218F-DA5C-4D14-A78D-21A9E4DEC669}" +EndProject +Project("{262852C6-CD72-467D-83FE-5EEB1973A190}") = "MediaCaptureJavaScript", "MediaCaptureJavaScript.jsproj", "{D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|ARM = Debug|ARM + Debug|Win32 = Debug|Win32 + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|ARM = Release|ARM + Release|Win32 = Release|Win32 + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|ARM.ActiveCfg = Debug|ARM + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|ARM.Build.0 = Debug|ARM + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|ARM.Deploy.0 = Debug|ARM + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|Win32.ActiveCfg = Debug|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|Win32.Build.0 = Debug|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|Win32.Deploy.0 = Debug|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|x64.ActiveCfg = Debug|x64 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|x64.Build.0 = Debug|x64 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Debug|x64.Deploy.0 = Debug|x64 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|Any CPU.ActiveCfg = Release|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|ARM.ActiveCfg = Release|ARM + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|ARM.Build.0 = Release|ARM + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|ARM.Deploy.0 = Release|ARM + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|Win32.ActiveCfg = Release|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|Win32.Build.0 = Release|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|Win32.Deploy.0 = Release|Win32 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|x64.ActiveCfg = Release|x64 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|x64.Build.0 = Release|x64 + {C5B886A7-8300-46FF-B533-9613DE2AF637}.Release|x64.Deploy.0 = Release|x64 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Debug|ARM.ActiveCfg = Debug|ARM + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Debug|ARM.Build.0 = Debug|ARM + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Debug|Win32.ActiveCfg = Debug|Win32 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Debug|Win32.Build.0 = Debug|Win32 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Debug|x64.ActiveCfg = Debug|x64 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Debug|x64.Build.0 = Debug|x64 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Release|Any CPU.ActiveCfg = Release|Win32 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Release|ARM.ActiveCfg = Release|ARM + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Release|ARM.Build.0 = Release|ARM + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Release|Win32.ActiveCfg = Release|Win32 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Release|Win32.Build.0 = Release|Win32 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Release|x64.ActiveCfg = Release|x64 + {BA69218F-DA5C-4D14-A78D-21A9E4DEC669}.Release|x64.Build.0 = Release|x64 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|Any CPU.Deploy.0 = Debug|Any CPU + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|ARM.ActiveCfg = Debug|ARM + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|ARM.Build.0 = Debug|ARM + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|ARM.Deploy.0 = Debug|ARM + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|Win32.ActiveCfg = Debug|x86 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|Win32.Build.0 = Debug|x86 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|Win32.Deploy.0 = Debug|x86 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|x64.ActiveCfg = Debug|x64 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|x64.Build.0 = Debug|x64 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Debug|x64.Deploy.0 = Debug|x64 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|Any CPU.Build.0 = Release|Any CPU + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|Any CPU.Deploy.0 = Release|Any CPU + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|ARM.ActiveCfg = Release|ARM + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|ARM.Build.0 = Release|ARM + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|ARM.Deploy.0 = Release|ARM + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|Win32.ActiveCfg = Release|x86 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|Win32.Build.0 = Release|x86 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|Win32.Deploy.0 = Release|x86 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|x64.ActiveCfg = Release|x64 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|x64.Build.0 = Release|x64 + {D70A3790-48CE-4E58-AF60-EBEFC22E9C7A}.Release|x64.Deploy.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/samples/winrt/JavaScript/MediaCaptureJavaScript_TemporaryKey.pfx b/samples/winrt/JavaScript/MediaCaptureJavaScript_TemporaryKey.pfx new file mode 100644 index 0000000000000000000000000000000000000000..242376d0edd4a63294d20055556a3b41ea2f632b GIT binary patch literal 2512 zcmY+Ec{r5o8^_;S%rv%=5wb63N-~T>qtw{fv4k9B8OFY4ZLBfb4~nB4k%VMPWQmj| zvNRFn2qU|6B4jDcWH@i<`u)zi&h=jR`+V=u_kN!1`RDm!X{cT(1cs%dk`Snq3Hk|J zJP;(LfQAZ#(@-HG4#3hlSpTs&zQAc5e}Z@ptSJQ7|8{Y6LZAgS_#T!9XJJJVod3t* z;k@tzV@UtNvD=)vP$+T$PJ@qrO5cC8DPbBh93IRS*JsQPh(oiEgjJnu7^5c7p2M&d7y6cP(N9M50g{d=$_u%# z*E^ZBH|e4D;Yyn4ShZ@K@G?x39uX(gRUzFc_7E!D7Ft{D7EmXAxVD8oqZ_qP;B%*s z$}t^e`Sv>rzR@BDN-y6=oKBGtVN;EfM(@g){I+LG+gjOeWw#fsI!B*)9Xn$zl7pJ| z9r#B0@QA24RJob+Ok=)fyGy;K)O<^7=0X19p5qHk_10R9fZUXJ3@jcuQ%3jj;Xm(?mx@c7Yn6px%sz1|1~hgV3v*l$*QN zspKwX;zDD0iLHSHu{d6~qov5vWBEeolhHRmkj1DRFF>T?fu>+8~u@o}HGTx1jms9i5Siz;mwB*6u`-0$PkRZDu=r zg&x&8@iGZ{J%-nveKH+hxcDq`ZJbvr5pr8FLy)R?x-U=MGV!TM{JfWRm8u`(%aUq` zN3%z6)IIjh-6<>OOLN8C@tT}`Sd(HrqbD~RUbrtFw&ee+bUv4mTW8Xky@F%k=pR&b zzZ4rm7mU=8sa$g@;WjLo;(CS~zliu$Am$~Cgtzj=bwN{#$6 z?%9ojdH4CDn@kJWLX1oIx}!|Fm39B3{I?>lr)HIAbd$=#AE$a1N0z7^b90n0x$Kn# z!&c^yqw>M~Xmp-6r7EDsX_D6}9Y!J!Gq)}=go0G#c8nHyuGX=Ze;?pC#**wcogTd- zxz<&)5=+!1SQ{en^fSRC9wm&v%p2*5ZYqD4Ao>;d!iyP=m=iu{f8R`JIOVmF>r5$`?fziQpJo0bF(SRQ<)>;B8#1_yJ?dwT@TOx7?yGV9J{ zPWWY0-d6&oF{Ri(EdM`oi4sskz~K-80I)XyLbQ^hxISAv9G!h?1GODPRJ4f&+no zKM(|#8xRag0+!(aC-DQ^!A=X*VZc?8`Gb8RKnAtP-@7CMv%lv;!Hxo6A`nZ1`Tuis zB%*)@a{>_w3IV{y>R$))pU6l2jC}jM)OfVdWckm?kD=6Uslx&WmyxBbSAL!wU(AHn1$}1AW2WtK95aa1#`+M_P1~%BsV}4PR$*}uq6J!w7 zn0!PMww=*A9ZX;=eAs>4cRiNx7*jgbfKvQ?I5tNKsX|O3k8J*)K5tjZc*-iI>vQ>; zsJ6TgC%mCHmf4>u)fn{PjDM_lv5U&3H3x`ZeLbH071CWCwpKhtd-g81hgd|8q*ye3 zw{v@h(Clbx(K{dItp7T*+4%F3SCg{kDo3t(AD>s+FT$Sg zU}~L^HhBWzTq}^-jTj<5FwyrOUZ5UM+m73`mU~$LZXh8?Li0!TV z*bMc<|7JHnugRR8dlcX+;4~ScsvJF&+DQMZoB28Rc%80m0%$)PyLkE76QMV$<##Kvwh)fYr`3jCvX07oNIDdE}9# zO}@C@9hdUE|J_dT^uB#E7)8p4wn50!V9r^E2lu{~r0ovJCbiPCMgvNH$FC2@yF56u~NV}q_bED9aN{ ztD1CKz;l!&*CMGx55NP3U-+7YcXPJ<_exXJmwZ2Dc9cP8e z)ML@J$E*u?Vwp3fb}Sw%fkh%X + + + + + OpenCV for Windows RT + + + + + + + + + + + + + +
+
+

+ + OpenCV for Windows RT +

+

+
+
+
+ + diff --git a/samples/winrt/JavaScript/html/AdvancedCapture.html b/samples/winrt/JavaScript/html/AdvancedCapture.html new file mode 100644 index 000000000..3f1b03c2d --- /dev/null +++ b/samples/winrt/JavaScript/html/AdvancedCapture.html @@ -0,0 +1,40 @@ + + + + + + + + +
+

+ This scenario shows how to enumerate cameras in the system. + Choose a camera from the list to start previewing from that camera. You can add additional effect + using the dropdown provided. +

+ + + + +
+
+ + + + + + + +
Preview
+ +
+
+ + diff --git a/samples/winrt/JavaScript/images/logo.scale-100.png b/samples/winrt/JavaScript/images/logo.scale-100.png new file mode 100644 index 0000000000000000000000000000000000000000..ea685d651ace9ce7ed2463b84bbff6ea2d891f9c GIT binary patch literal 4609 zcmV+c68`OpP)Px#24YJ`L;(K){{a7>y{D4^000SaNLh0L04^f{04^f|c%?sf00007bV*G`2i*n} z3K|J({%g2XSww<@1dJVIOJMBShDvZ1 zwi5>zR3gB3IVvS~z)((6Nq{OVDa8rIl&cbA62~@ncqD;}9SlN_LV=8Mcm+{NmJ|@M z=z)YJ*wyY%pOZhjd!}c0c6T24_Vmo&qiXePHQRgpp5HyMd+s^l$&)8fo;-Q-RqCGV6A%(1#6-ZDrSL(8PXus@!crgrdMbXbU?5aD zDzG(xM}bES>`ee>JhrAD9ZP*JaD~8w3a<*N##j8cXzF_vmq__%Rp1?grv&~agp*Uh z!&9e{lz+WKU}FVZDj-}Lis!?IN=VDGlrSNxc1gbG%3V}6V zE#^%9h46s3C#cdv*5MGAd9LOsoN!;B?WsD!6$&iz+-<$YTHgtv(y`k^r+AgvI`y}z zoz>qY`;gDKGmqMO6JFu4!YtE)$pji*_jkc(+c8@rXwwNd3R@Lso6gNp(EvWJ@PT%G z!M0?Iz(v55W(uS}DGU!oe}9|3XwYVWLZQn7Vj|*3oL8`REhL50(ZJx00J94BJ2U{5?5KA3Sj|GNo+YwYI~!)TL?!y zBJ45hFC1<3s=(?-CDlYWn8I@o)uB(Tu_UqnSKO@rJ8Xig0$?Y`NASggJ-E&r@OGHw z!UKF)9iLDR;buoF{Cgf1&4X%*BQNlQjK)9cVEkM1&bQo0-VL1VNSa)o(fI#a?LYHW zS1NqU01J4ZgT0gO-7uJeZHU58js54&%{%jQ4J?VSg~tAhe<>qrg~HYt3^v!t46h2C z99#gWT4o9guV(cQUGPSnjc9Xy^r;6{(;x^OZ0;V-_%((c2}BR9oXuX)&hu=5!JH>L zwuA9LYI6qTXU)w};&uUjmY*rpyee$kfIndDu68w1(b`Phb%8S+I3LH22UgRx*EU}i z&dcKEW21eIgXbReUSabC-02{|xX5+!jpSUKfzMd>YSqAg53J_tB^OK&;GbNo!#IW& z0GC_#YK_8x2Uhb8+U>wbQQ$&>nNE}E;a?p+dWQ#=9R@w)z|QIc9yK^io0(_ea&;3H zc*0kru>-7I9c7QIz?W>PL&hlum z3#^4%RPPh`N`vFOGJ^xF2R#xj$DQJ0YkM3rP6VEfRc}M8P@`5RE>CN(@n~@4!rv@z z)y&E!ZNViLThvvK?=6=BT`#@hfTIe@o0^rXG23*XPM+aiYQ@aEvW^a5BJgg8QgwPg;SqsBf!UtBIRTfzA4~?Wl zFwb)}e`@iMWO9jcIBVc5^h(sJY$EmdrotliY|I^~5=O347^z~@NNlv9-=gtdAmiJ6 z`=rDB0(AJn98E?hPo$Do;-PK#Ax!= z7GyF3L|2)iF$cp(P%Z+_#Bd7AEDSSHx-m&T4UQA_jd=&<|1dlYJdH^m*zEXLrK>1-+mS^)Pkzz*?E@-2b z*bVaGoG{aXTu)0^XDMH$3t_zjb=N4kMo~^{6Ht8!B#d{RX0%Rf6>XUB#oL&_1fAbH zaD~i9c_SsEq~)*@zMF4VSX_}0mS@h$Z7ue;7DXUW_QqudWNmifDwzadLMh}xN~K}- zR{rfNnFIQKdDcuZ31eyN>>SxPPP*7nvYel9AvnK{sF}vs77D&>8i4Qx8f)+sL*#&;dQ2);s zd3?2m9CGXl$}-TQ$8JBOf3ptU)l_LKs0W*w)$ z5(ZwAjU?56(82m@tjItFd^?8v#+<4@K^a2rREn`rBL-w&os5g`g?@L2e37yDH&~hA zbqIT#ld8fcxs_DWxbDbZ(6}V6{-=Qo0Ldw}@4AbIhk%~nU}=eXd~$c=b0aLsL3C;& zd9^Rcp@ly!KgZEX$@JoV zu{*x&EtM8K3L4ZaEx(*gIaLDJ%lT7*MfZ72^S~;qoJ$;x?+ts^Hp8dtRfU5&wX2f- zz$H0VfnT;*gwj-wAz$IKqW5x+ZIctCWidin-5GL1oZSK z?7oSv9ktBQ9F(ElN-J^F5#AQKzTmJiMXv>E^+QJ`@)h(`lS!+aT409Po|G)yl0VxD zK>I*P9RlkojlZ-4;iV3ndI;#x(^Az`fT|zEkmExN>8Bq+d4TAU#LFlLF^rePc1jB5E9Clii%YYv{nz&@nmVw#T*pKQt?evU`Y{Fzl+3M zFtymCQpG;bU)*Yz+ZH;appSu`*n}#a7Nx(qQimd-`#`(Pk4z1KV`+7akQMo~kDA`h zy}@nMFZrvnQlO!+8qj+kFM_3&^Rvz&s0LAHb?&h&*<@eE6f+(Ew)s%QxB*-j!1_VO zqT?9{5-WM)sdO~yAva74_mnoYZEipV$RS56dx(^8)rB(OfkR_}yD;qQ+?qYIz+UoO zTHk2_f|bz}+GI?UVPxXCld4^I(+@H~h+m@{m}`AQay1NY61)p)_N$J7_SSu_`6piC@_jP`; z4&&nR!)7qM-HLeucn!>cyGEQU1d9_@c%F4>Wf;J)JFiK@c4(OBpr^6BDB&LWV-EmR z0DaM}p<)c^;zVLCb0E#{?|d!T+XJS@z8d7E3?=S=*;j|=v_yfQp_VoPqVe*lIxcB*1zFbC5GSjMD8iBqMy=QO4`%Ya2lLF-Ip~c9vF;Z3+QB#X)kS zlqdTM0m^HR81(N&|H-zrQZ8(u6+tay4v{$9wKjCSN}Bs~ci@umG*}d*mP$E&>6QPu z!y^jLc!sb4n_#84(YlpNJ|VJ3Fm&X|>-SI%Jk2T;4RGlxb+O4yrgB z?@jgDE3(Gn0R;g$3i4Zxsl~Ep#@D+&0w%{myT*3g5FT)NQYR@rv=K>dy-1Agt^ zajzW7;K={a8dNp6IX+gEcwB=ZKLV*CT_;=!@(AdN`@^cLpx35@4wask#61IDMxT=3 zJ&f`=@GQzJ1PmaA1k3?e04sqHqbve69+lf|f8{FR-umKi3h0t3>r>U;G4@pWHlm%t zD^ZYEkHph@QZyD;f>|8@i0d%{dKiMa6G?|JX5Ln5tSwgUyY6L`aJb3%&9hx}T}mzf`$`fQOrKS?2(Urq;A20<_QvT_Z`xPf#Ty ze+1=QOueSKL%>fP4;Kj#90v0^oqLFKoGl!#gPxHzoXofY6kf-$pyL6i#W(&90v>Dr zfrYuy^)^$cW3|2wG9QA`G_ZnkHednPO%bVOfG1HNv$HeMeF$`Y^jy`BUXl@g6uL(% zp=8F5`E>!pKLXb{_7YofAPCdxnkWUi5t~B>z?|0ExH1n#NA<@FXERB_b|!{{Ku^bl zO1X-Fd+k>V)RUw15!2g&l3$~D3ztLpaJAPPe<}kY;3&$k#+Hb79H`(S0`8q;CK}bS z2f(b465l#$?uxJSQRo?NY|WG!iC>%R^4yKZt+o@oOhYX0MctH9)H{Q#82!JvWHUA6 zMMwAB#{@Ln6HtP(JDb&=OuXK1p;JX(!1a~pK~*_F!d;n0!`d?~sXzVFg+jz-4sG`4LZ%xU{qFVfZn}SO~KA*)R{6ZVbt*0rp(z`7t+Fjp9 z;7V+&HTEUdxQma~2t&D7jnT;d+WEZHH+ie${>pg;SOTaWAj@gVW}`fe z@J@y;dd=n_16 rvTwV2^5n^rCr_R{dGh4RQx*DuhTcF|6)JJM00000NkvXXu0mjftt+lf literal 0 HcmV?d00001 diff --git a/samples/winrt/JavaScript/images/microsoft-sdk.png b/samples/winrt/JavaScript/images/microsoft-sdk.png new file mode 100644 index 0000000000000000000000000000000000000000..380a0102660b7ad3a9ba5418dd1bb81da99bce2b GIT binary patch literal 3405 zcmV-T4YKlyP)KLZ*U+IBfRsybQWXdwQbLP>6pAqfylh#{fb6;Z(vMMVS~$e@S=j*ftg6;Uhf59&ghTmgWD0l;*T zI709Y^p6lP1rIRMx#05C~cW=H_Aw*bJ-5DT&Z2n+x)QHX^p z00esgV8|mQcmRZ%02D^@S3L16t`O%c004NIvOKvYIYoh62rY33S640`D9%Y2D-rV&neh&#Q1i z007~1e$oCcFS8neI|hJl{-P!B1ZZ9hpmq0)X0i`JwE&>$+E?>%_LC6RbVIkUx0b+_+BaR3cnT7Zv!AJxW zizFb)h!jyGOOZ85F;a?DAXP{m@;!0_IfqH8(HlgRxt7s3}k3K`kFu>>-2Q$QMFfPW!La{h336o>X zu_CMttHv6zR;&ZNiS=X8v3CR#fknUxHUxJ0uoBa_M6WNWeqIg~6QE69c9o#eyhGvpiOA@W-aonk<7r1(?fC{oI5N*U!4 zfg=2N-7=cNnjjOr{yriy6mMFgG#l znCF=fnQv8CDz++o6_Lscl}eQ+l^ZHARH>?_s@|##Rr6KLRFA1%Q+=*RRWnoLsR`7U zt5vFIcfW3@?wFpwUVxrVZ>QdQz32KIeJ}k~{cZZE^+ya? z2D1z#2HOnI7(B%_ac?{wFUQ;QQA1tBKtrWrm0_3Rgps+?Jfqb{jYbcQX~taRB;#$y zZN{S}1|}gUOHJxc?wV3fxuz+mJ4`!F$IZ;mqRrNsHJd##*D~ju=bP7?-?v~|cv>vB zsJ6IeNwVZxrdjT`yl#bBIa#GxRa#xMMy;K#CDyyGyQdMSxlWT#tDe?p!?5wT$+oGt z8L;Kp2HUQ-ZMJ=3XJQv;x5ci*?vuTfeY$;({XGW_huIFR9a(?@3)XSs8O^N5RyOM=TTmp(3=8^+zpz2r)C z^>JO{deZfso3oq3?Wo(Y?l$ge?uXo;%ru`Vo>?<<(8I_>;8Eq#KMS9gFl*neeosSB zfoHYnBQIkwkyowPu(zdms`p{<7e4kra-ZWq<2*OsGTvEV%s0Td$hXT+!*8Bnh2KMe zBmZRodjHV?r+_5^X9J0WL4jKW`}lf%A-|44I@@LTvf1rHjG(ze6+w@Jt%Bvjts!X0 z?2xS?_ve_-kiKB_KiJlZ$9G`c^=E@oNG)mWWaNo-3TIW8)$Hg0Ub-~8?KhvJ>$ z3*&nim@mj(aCxE5!t{lw7O5^0EIO7zOo&c6l<+|iDySBWCGrz@C5{St!X3hAA}`T4 z(TLbXTq+(;@<=L8dXnssyft|w#WSTW<++3>sgS%(4NTpeI-VAqb|7ssJvzNHgOZVu zaYCvgO_R1~>SyL=cFU|~g|hy|Zi}}s9+d~lYqOB71z9Z$wnC=pR9Yz4DhIM>Wmjgu z&56o6maCpC&F##y%G;1PobR9i?GnNg;gYtchD%p19a!eQtZF&3JaKv33gZ<8D~47E ztUS1iwkmDaPpj=$m#%)jCVEY4fnLGNg2A-`YwHVD3gv};>)hAvT~AmqS>Lr``i7kw zJ{5_It`yrBmlc25DBO7E8;5VoznR>Ww5hAaxn$2~(q`%A-YuS64wkBy=9dm`4cXeX z4c}I@?e+FW+b@^RDBHV(wnMq2zdX3SWv9u`%{xC-q*U}&`cyXV(%rRT*Z6MH?i+i& z_B8C(+grT%{XWUQ+f@NoP1R=AW&26{v-dx)iK^-Nmiuj8txj!m?Z*Ss1N{dh4z}01 z)YTo*JycSU)+_5r4#yw9{+;i4Ee$peRgIj+;v;ZGdF1K$3E%e~4LaI(jC-u%2h$&R z9cLXcYC@Xwnns&bn)_Q~Te?roKGD|d-g^8;+aC{{G(1^(O7m37Y1-+6)01cN&y1aw zoqc{T`P^XJqPBbIW6s}d4{z_f5Om?vMgNQEJG?v2T=KYd^0M3I6IZxbny)%vZR&LD zJpPl@Psh8QyPB@KTx+@RdcC!KX7}kEo;S|j^u2lU7XQ}Oo;f|;z4Ll+_r>@1-xl3| zawq-H%e&ckC+@AhPrP6BKT#_XdT7&;F71j}Joy zkC~6lh7E@6o;W@^IpRNZ{ptLtL(gQ-CY~4mqW;US7Zxvm_|@yz&e53Bp_lTPlfP|z zrTyx_>lv@x#=^!PzR7qqF<$gm`|ZJZ+;<)Cqu&ot2z=0000WV@Og>004R=004l4008;_004mL004C`008P>0026e000+nl3&F} z0007bNklvu8|2V6@1@j2~ALpk{m{<$``0fS4|e;N7h86U<8m*waQr zDUl`j3IOKfZj<0$O{u!Gj-Z6*44(NDz!Q$GwXtuGXS^jxE}TclYyxy9g6bS<-crN?E{Zk0vMB#l10B6 z5WFiXeVVl?J7nEMc=;Di>nSw8%W^giS3=_E9eab#f~-+BfLt4!iQp>{_|A1Z&Zc-mj#PnPRIHZt82`Ti~3Uk?B!Ylp0*+7m{3+ootz+WN)WnQ(*-(AUCxn zQK2F?C$HG5!d3}vt`(3C64qBz04piUwpD^SD#ABF!8yMuRl!uxKsVXI%s|1+P|wiV z#N6CmN5ROz&_Lh7NZ-&%*U;R`*vQJjKmiJrfVLH-q*(>IxIyg#@@$ndN=gc>^!3Zj z%k|2Q_413-^$jg8EkR}&8R-I5=oVMzl_XZ^<`pZ$OmImpPA){ffi_eM3D1{oGuTzrd=COM+4n&cLd=IHa;5RX-@T zIKQ+g85kdF$}r8qu)}W=NFmTQR{lkqz(`5Vami0E%}vcK@pQ3O0?O#6WTse|n3$Sa zTDmy9nj4$D8X6i}IGH%PI2*Va8n{>*8k#!7%)qAC)y34v)Xdz{*vZAn(9qS;($vJl z)xyQx%*e^f(b2>Zrq?sCxFj(zITdDaCeU7}UJJZ>t(=Qe6HD@oLh|!-U@0IVBfliS zI3vG6!8zDeAv`lLCBM8F6gd#Tx}+9mmZhe+73JqDfJ4_R6N~M}u1=N)CT<3nuErMF zeGSo@LQaV310ACeN*YK>1ttVce;_72;R8AFtdp7t%r8a2jPa$hZ3Y7aqmZYIV@SoV zH(|GVn+U)puN1)>GjWDYb(7@t?Y2iuo7SC^e(wK3>@~xg zMI8?7PRhyOSNeDN&u*(nlZ&MfRzH8rq}_NqyH&<=^8B|I4LQ+k9H%eLW)>+)vZ|V=`jsDXVcc>oTy0n3v?M%&Q3sa2+aW|74&4eCtU0$WI7BOyoPGRM@CzIPx#24YJ`L;(K){{a7>y{D4^000SaNLh0L04^f{04^f|c%?sf00007bV*G`2i*n} z3K=Tmv`v@*00YZOL_t(Y$F0^~h*eb-2k_s%cf!W3RQk}Uv@kObDhL8G@F9etFUrbZ zg9>^nA$lt6p|@U&C@P2^f?oO}p#rHV3nJ)?K2%ByiWEyLourzXwR*Vc&fVwEokkhY z;js7FYwz{nYwh)4dlkp=BudjZ-A+=M=B%Annb4P`JqjU9M5nJQd|+X|;PU>6te#D`@8XS_Igqwmhf>bJ82 zX*^Pr7l%CZesaP8>F&&fy|S>p_CoSp@^_^m@$`_Rmf_rm0FOHN&U||EIAUu);Qh(r zp|J22iIuWdmexBs) z`>LdsH-xp5@m?&l^XB`zzUrq0BLSbMj`eVuW{=22Td}ofv2g z%}ib4BI$BrX$8>vcWm~ONnP38Sfj%^=jabKtrA|8{*Ez#8u=$@=X9M?bD-*Xf-@VP!el@fdtE)NZk&D_@RNhZ|e#o^S3TGS_ z8_IN-++A5SMLs(s9lcCUR$lpL3>Q=t+7!OhaimqXrPHwyXEwX;O7H1({=)ecz#)9x zj`$Aj!cAD-C_R5nb;U_trLnfO%FC)G{Td{9My9)pICP(!TIQd&%E34(}N-e_yy&h_tq=Dpr8?}sTnpr6>ge0Hke>!%qVM5&;0f2Jn_m;*}^(UWfQgfpiQ(a>V(NLXiYC;HWpw3k|fv zB7M=P(Mazw{BLLz06@|k}+8u999Rp=A%Q#28p8q026a^5EA8&CIP+BzL-E$ z5U;WZ1jKlof}9QPAa+5PXg|zx3IXj%v3EjI{87fAFNQ9A1 zK_78p;`_B_T@di23(4OU^obP0?j+C>M?eD&bRgO&h#>@M2-Sfe(l;_P0t59RP>3!> zPZz4E4K;!t(t|-Dz|S9$I2*y+2X-26^*NW=G6ngONI@`N-O$ibozO!%ID)S()Y#Z~ z4MR^)TkN4t3=1S7$=ZQLjdcV#nusD`f=C!#AaD&4>4gg>nS#VjKb3$Dva|aWIFR_c zQR0^Al955WP#uUa7Q0s0M{gqOH2S}3eAS!i6c&WmJ&h*ff(a<`JbX0PgT=l3`$TJw zVm2@b0!BO(WB?q83dW)XNj7j(kobv?H^v*LZ)BivYz((NW^~Bt7!(RUY@u%fw}il< z5V$cEYH)NN<7=@7`bK)jhI)pF;D&lo=rQO~OSpjr#K6GP$k4#T0197^wFxAWkbx-l zdR~k;@8?*9zm0`i63|Ezj^KpD1+4FYlYTf7j_8LA0$Ms60G%;`-ndZW+HgMA3P%$# zA!u(a0uBrO*j*UrE7||K>QAxW|C^5Kipl7%ZH+Is%KDOc|E=Brx$(uupGOBBC_XO) z@xhS4pqLB*>{zjZA9W&g#@{61hY*Sv+m5)T0+b}=Bvd41E{U%VaThHi>Y1|RvbI|+ zw@k=ZJvsko2$u+`1`HY(#4C0`53{LTETb$>3x&4;fIUU4nHvFs7-;~Yb`t>D0|3a1 ze+9M}el#z7O8#kXR06D-KRn&=(MxQu1+ANFLH`+J&1((ve~j^EVll1t#Qz!NOY)z2 z{5{5(2tgdVsvny!d+2N)KOgOxdVkw-BraF?;GNO za9OEniGGuBu)16 z$>uEcspvWKb+0@@sF06pX&NeVHU5rX6F5-wvIBVp@ zXDRl~j_4e_Tl^-YaYilOeAiD*|K{A7WFK{->m?vKBL~rCZb32)S%{f#^HRvUdFrq( zF0A_!TEVVQ%bto6a@0<*o(eD9Z+(q`Yx}O|?v9-S398A59#s`S^kVXz!=CBA<#efc zsh*F5mcKvl*k8a87xt2+cFx_95oNe*csmMV5oS9BAV0eeD^a=;)cksx&SxE1;xJex z3wOC8&0-3NQYL47c*lZlZ|>N#8YC}AlMrn;e9b7ds0 zO=3?fGo`f!A>THu9iij=VC8rhBir(2m&*Q)G18kB4f{i%{Q-Im3^7&J+DvUTXz<&` z$QthwWf3$J|BwodRn6LgnC8PlO<8#n^%8z&Sb8y zQU82iy-(NBbaZCNYb9~*MYZXxKbI;D#^-uf&J@SvUb9JulO3szm|X#>5Xw)KrOB$j zVE(B{%LyRk{LX3n_Y3se%3-NtmJ9(j84SmsymYJRBCNpecmXlOe>fn z-V&UdsI)Q4u0Qn{UlT7kT2cO{cVd~p>h99dE2$bdxKxv4u*!QqQC`(5x;5Yup=t|T z37oE2SurUjFOR9RY2?BTh)Pn})*}D*p~z0tfyL(V#qIpi{waKAbK?*l%qvy(Jom7F zWKfVKtgQDB0WPQ8z+!qJ)-vc1zPo#b)TZssqlgaQ8mv> zNr(42u_MpFy7LOPAHSLV!OylId_>z?Ha@2>J1_EFl8J4SiM6syQ!Z{G{5t(i4zEWu zeq=C=A7LDtROB`+=u4`2wHc9~l=z{ey0vP_o`pFwyW&KJJjvji$(Y=aY7iyS=^|vLO=7Bu6BRl!*};Rl)2P-*Ypn~=+y3I%H7uAD&Encu{-I9<0~hp z6!VcCa|I~|rA0x+-!}+gb#$Du9$_4KX`v;uZ3u6sbGVKU#!*Qt+sE;H{f*v7FSZ!RZKar(0Rb1AwH~z2Bvw?@iEVL%fBVvE5Pr(^8w6 zqDY5&m9Q{vb~?+YN;%_%y?W2v22NH#Y9|BR{lvR8O&*_`HmB(&$0lW?ORD@txg7$}W<9QLg5|L$UsO*#y?2d=r0jJkz76iRoo$<`Ze(f9^@2+< zkRs}<#-+pJ!@_fxsUx!5hni{nXODp!C9UEG)1en?!or(t4Y%=m2g)V3#z_f4 zUzU6NS2(yTyH&pb!zN=-&zZBX1)w}!kYVC)!xt}6of)aJm)u5CO9Je$>7B}fa=RhX zcOKstJ|PzkwDWemc%%)|7eLW+W)~Q)mpm^k`4etER6a0TDD3@yWkizMc=G7Ck-zXY z5SHKgvQuc>ifW|H*prLq>r9QV^A{{eSPX!Xj& zjbjL_{JknQo-Z^K62j)Qet25D|DLwWIR(SsD`!gUvIU59+vu0Mi4xPVM_N#uWwJOg z%xoqMuZ{9s&Nw`nzR^%xgMda48S@$-dh$E6xq3s&J79=-!;MO>-CcjPcYt>>hEbhN zj|`j$6<=be^{Y;+QTY(RByl0KrOHH}g8zli6&;A;-MxNyPoVI*B0EJ}!-4zM7$5cS zscT1b20hB@9yU$PswIGp+~|u3^>7*n9@B294gJyNvUq&L0UYko>g=}n^y2dE9J};u z%BWO1L0?FOY4G5Dc9PmOMJsuqnL~HSJge)0AB!EK&5;cVatYQzv1MvQ0&>dok5E@# zUSIHdoKGb=xXrzFC95c*m4>8tLthj2m?c9v{edPqzL*{jT20(A(zK2%9g`THSn=|F z2OL37>n!%H^i@;$iQ7`3!$)5|8K7rgnl=oyT6!R9o%D_9c|DEvp(OGV{6*$?{cHuZ zcJretn4zS$V8>wO6ZYX9fDN?`8M~9GvYgLOVr%X?TkqcMvA-u)(HNr`fxqbw`;|!F zO0S4iqPph&JUOeHQKo#Z%BlMSyM3t-T>qS`Or}KZt|}E7NySJPnt)EKC~y_e;Rcfz z@6l)s!Pu?_xz>4wxeJQB3+8pl7qjei6XkcvAe_>#P27?f^`)RO;g^K?N6SmM84UkP ziIcXMO*m!czt2Y9hg0RoKs|4phJLK!Gy5y8&MMz2Z(#NpX-@w7CfGdtSVGy=>1X5R z1lL&kunb|Q^}ncD*1DTqOdifoc+8xOA62dzAu?iHCz7{W`Kgx6VYe8Tow~kV{#>Vg z$fAYTnBA5ldagS&9(#wASn*!&rM;sdvLu^5IOFA@GK!)m(7UxyWMrND8P7r>gX zoo!?PdTi=Ccu2O@c;DLmj^X8cXrodE6iRY3siuH`pCZNM85^i+vvPN`I|2vpB~OXm z>^&=_`21otc*Kr|t>3SE!cE7z%jbK%LVnxttnPy)1I@5)eBbavbkuE`-Ps;L{zN(6 z-Md|>)!697yCYMQ8lr(mRs4^O3qx3yUWzl0xQ{$ec11uVRs|-cAvhA z714Q^UzNf3b6XKHA{c7i3Q;o?-P9?&r`p_3g7K5OhCg+3^~0vd_iW{=*t9vlWxfe9 ztfmNqs5Q+Cq$U$?K5lBvDyTP$?Mx%R{&xA4f;5+A3x47Psqt0!#Uky*9y$Z61r$hz=1>QzG{9d0Z^ zW;>GYm&C3>T=2&Vb?eonS=roU>(8h^m6LA`gpjizS!|0;ZqDalbgDVS5OZxH) z7xqdS3rAA(= z2C_=Yb1{1xy*PSgpEKpjnjB+*HmiIRWM2r7wFjS7%U2ZG-z%SWG4%z;KPaDnS73ip zK7Y|2ANAr_^v@UMzowr49b?VwbK-SH`u7g@n*v@1|s#GQ665=9@Rxy?u0YW0&WN+~=RXpPbVXXL4m7Aq=E6I0%{06TwRn=U9d8>exk> zD-Z%M3DNQ`bTLSEF=%NFyoHcAkD*CiXqljo*0E?o$GiDC4q}}|%*0WghLlK#npw?hecrM}Mw?`E(z5C8< z8&*b^!{>5?4aT89vdrgBgSc-x6JZD3F^l#*G(@OO*^1D%Eu7?HAy<3kTLqW9N{^#6vso zVQwY48q7)m{~xQ64RV7{E7Y=&T~?^05Ky`5oNQ8bLgFCPq9co^R09BVRS1OAmH;hU zC#q(N!gNqm!zU#%sv{r5mm-Uv8b-~a1F-;p^>)pnXfKge4s9?;;MFIr*fixPG}NBA z6_G5BEmeO6XXh(emkciB{7tA;iwC2^s^VzyU_h0@ae84ACMY`cIDEju=<`q|2QAEv zW_)W|i|9aknqdmS=#w73eW_csQ$8IhT^vY1^1;X3&J0{%*tcQq!gJpr3w?TJc~@5= zKV5sM{$3k>b#S$@CTkhIF*{v*u(F&$&Yq1naHxt8Mz2N%7aQ3(^VNRZahk1||7?Bl z*idzO_u)FhRj4cPzDO>YA>>lxAGaciEiX8Xzp1SVPv91};$OG3cC&8!v3{Jq^kH@8 UTIccK;hzT5*3#}uZuEx!0OwrBv;Y7A literal 0 HcmV?d00001 diff --git a/samples/winrt/JavaScript/images/squareTile-sdk.png b/samples/winrt/JavaScript/images/squareTile-sdk.png new file mode 100644 index 0000000000000000000000000000000000000000..126cf70d83e3973f6a4b040fb4af58f546692252 GIT binary patch literal 2482 zcmd5;Yg7|w8cxLvC|D2`1g$X?C`v99E-?Ybgb5G?fe;OBLNFwQ1WYE*BnA-0T1BWL zMnSEBiomi}7ez!wj7SZoLW{S9>mp!LQZc9$K@p>LCn)az@bsMBUpvp4neYAPeV_OG zoin*nt3zyNI?cr4a5kJ!kc+(!ns3YL*wg>BjSsQc0u?(}6$2-!G-3q8F{N-Kgy+b` z>me>AmZopK4*BD77E&26Ruvl&&XB-zlGv<6N|P%wYm6o^O(~Y7LMnVBv|gqNAPhD% z67Vu<0D<)tejG8aN&H6p?Kk>oOIjMeVX9hnueXc&=*3~P#b0|-mha!8@# zfB^*T3rQ-IGAK;84@jZ2*iPVL;(Q`NCyBP<|N0bY-zzv7By%&(~AXq z0RS6d0>M;_jYbLb@ecM{?mcPCQJ^ZZLIO?tm0^BA*wX*hmJx(NVik<=V0gpi1Vkmn zDi}?MmH40-A3R^Ckir_&+?@Bhf)FCx1WAJtSdO2lE<^SS?<|TJ)0@p^(y=fHktOG8PQkJR1KoDwA8-{F#>@2OkqY9vw)5ofiZ<81CJbt~lJZJ`Tv_rFA|n&f8e# zZ@W@WORzY%u6mJLVvFA)i~7r>AmGH>_)+3VyyBxWv!Q_^*Yg+TjCkuY*?EXJ%C&ZLc_TFUhB@tlPKZ ztuQIYLNMqid6%PJV$8LS%K7SL_)v88<3F-zz=d?{D?YBDk z(@kr0US82Vnfi$6EjP7mr9NXVqdWecyFu9bQ+-vz8~*Jvx9S@}PI#nNetuSS!i8wk z+2%t+i}JEQOZ7g#c_LBGC{o4ST9^^)NT7?J5)0N#T*h;_Wx;TqK{z%zf-s~q+YwB9u^OgKBG`gUUdml{92j!Tihu~5eK)(TydZIShZuokRiT}2N0 zcDc2}bHhff=9HgJPtLf-jEF`G_WFA6JJQ)tb$hmVhTVW;aR_-}cG$zM?sa9##irpc z<&R9Eq<CAi4DOoQ%=KG!OUUvWW#nHkpQ#kO{GHv}x)0)lUe%p1v zL%`CXz;=(464l$UN(Pd~u6Y!+_QyDFdT=0p(Y!NmdYhq;8m)2hzSOXmU)tVLhkyGc zF~;%vBmOe}1}F9bBmR~m!XbgLKWR8P+QBib-tiabEv)6`w%^YW{6}_dhW_G+^FC7K zkvB!3J8O+ct8^=>yBb0}b`Ge|l+VpAZvSFftI+ibddotU*X!`{FA_jPVWDxa^xdUq zq+T0wnf!A6Rlfo5wU>UwtMlr*`CVPU#q2q2kjo0e5r6pT543r02EiydC0Asaw!_BW zB(j9N38zY zVSnnVYg@*N=YGSlDqVZC^-JyRY6^JHUNANN_oKml)nKDbi%`78&Lwkq>3n8lOaK(M z%XIY)`(*v_R>j{Wk6lNV(-IeGdy{*z&OsHogMSnZ|3Tg#&#R$#Tys~HmZY1|0$u&~ z8{Z!cZXd0F3%?F04BZXB^SFofI5YqCEdel3^w)^1&PVosU-?Y zsp*+{wo31J?^jaDOtDo8H}y5}EpSfF$n>ZxN)4{^3rViZPPR-@vbR&PsjvbXkegbP zs8ErclUHn2VXFi-*9yo63F|8Xy|HaX=q~T zXkuhyoy6KF3~uM1wiR?bDKi6!|(A^G_^uoMuGkzbNu zoRMFk;2dnK5T2Qrl3!j7iX4bvT~doO%TiO^it=+6z@clEiN$tTLlY-cQ*#SPb7Kqa zzJ};cAt%K2fsWA!B@Lvc0uutJKM)h1@PQn7)=A9+=9eO1#>h2t<6vN5a`1F<45_&F zCgNglvw^_UQ1K63?>IoPgvE$SQc9`RFR@5oEMEQcuh2HP zp6FYXzh@K|KXK{P|2wZ_Iz!UOKY}UZOO97`J6F2O-)1bnXSiIfU8r#i)0E?L&d-t2 z)c7~uKsr%c^Nr5aQ$j_iEzTP2CUULicAD~e(?KcrHI`iRGF9`Xqr9aHn7pUHd@)Un zYj5;f1(o7sYHzMT6tnTW>+t@;Wxnk!*_3;D;aQ7@>=7RYwcvWU3HaL8rD zamEmP#-v^oIY;GJrcGr_YyWP$|7PL>Nd~R3E+*rLwl`M(?ya};313mc{QT!#{=kD~ z*_z*~*>3GM0X=d#Wzp$P!Xu`&Pv literal 0 HcmV?d00001 diff --git a/samples/winrt/JavaScript/images/storelogo.scale-100.png b/samples/winrt/JavaScript/images/storelogo.scale-100.png new file mode 100644 index 0000000000000000000000000000000000000000..dcb672712c6823a0c91548ded70a8acb85536b4d GIT binary patch literal 429 zcmV;e0aE^nP)NtYJa1l)bQ5qwGXpZbs7%2oRMd4y35$s&66(fxhNg8W02!vSn zdlrL2h^Fx+3=$z;kK{0D#MyeJ8WRWZcLSf(PcQ_mLOhrmC}O-tX^0c>5`YvCUZVsc zG-6#78ubjJ5nA;OX&^K(q=i6ZNE3m?kTwE^AqxZoLskfB3|S&1F=UO9!cY$g2@Lgu z;9{sJ1P9|X2L`r1#Gs8R{E^$PRrMaC86q|HL$5^q~u7S+3& zTD-Ok5}6Q6nB-Pe79nQGbi)dHH^jK^u&Le8=e6%Xf1Kx>^ZS0kzu!6M_xYW3%GG6$ z)~d~`5D0{plcPNyfmlXV_0O-+RJHCMS4UL8O+p86p&J}1jAZa3gbfo8fB+{Bg9Xtc z22&LF5VAlZ)B(1;x6qsBOa@^NnlX<-M{#&6HUeQ`6~$wKA&?LVfLLrU1vz|IiUin9 z3ewArhNbcBpdhwmG#_${c5w%zLqKyT(#jIBh$5>5IFOJ5L~%m70&)}uxu8o{_2;KC zNMHdX451)@56YY73fRGX2rxrqQ6QFx1&BB_j|#1#k`To7`yryx~)G@H#N6A9)HW;k;^k$|?KuS)BlTqYX-%Ae=h|P1LEg9mWlnv{$CwipSRw5W!7=Gl{Lv|$c&&wI(fkRs zU|z&5m=`hsM?z)(;O1{jWK<1uq7yUx#%3OaYE=>Oc0k_VAnz%Ys)BE!w}l;RWk)8z z(VENsO!gwpvfivAyHe0KY8rXrP=K=c_{hkX#>B^dA*a(?jrF?TX05mq{A2T$rv-;I z$g)GLQthe-50+`?nAorF8~K&f1xcDT?^m3L8awoS^t+|B+_)o2M|S0bvx#3RHK)Q97{jhg}3G8uH@@X&CJqU`E#W{P30#``^{%(OwJD-=-tyauopL0;VGqI3c5q4 zr*$ObhOaD=K;y>j@OB(*Y&F$>fHn1Sym(M#EK#3=slAk%n1|!amZGMfQyrkraP~!> z)-W|mGF&kTS}6x41?jgc3I-VXbg1n0kWp%YDJoLz5y0_884jFCy2t_@V*^{WqK&Vj z#j#K7hFadS*9^0nR!#e8^3SKrl7nC8{Bpi&c{jLWmq)wKWPA5!c}o(GC~7L-m_Cc3yhuqVprfBOTg1$Tmq++%`0NgQG)Qb0okCcS zk9B*@D2`=mr`HE}>8<&1fBcQj`m7M-x`@t+M|s|8?Qg!+3NgcXlQ8jD%v&B1)xof_ zvtup#a{1X2R~F;h=%WFHf!+0#aD@pljC}qB!EZ!qJ$7e9@Zl7H8MCd!Rj(*Pv;3cG z%mXw7edEMLDMZOdby>zc2ah&@JVyxK?m{O#8snd zB#U9q_hK$7E?2vRqqBV^4jt&zytD1ruf;jKdWR&rv&!4_1MZjmDJ7le!AZhu7>vQR ziRvEGNi$i|(y(#Xj$-A#llvm;X70rrh34!dIf$7uw@y^y^HeXtkYnZ^naWn(qCtz- zRfkyezQXmBx(s)(K7(yCY7o(?6Gf}9*p%~{3*O1hsxMwH+d6PUivpeTLWS#Ui1`h) zOGVoG;&%DQ#y)xL)3(9rUc+2ZFQ0p;CVlP^Kc4g1q`SM{{bQW6-Q`7Hn^&lZr_L>t z#!LgqOV2XP*X;0WYFjJkQF7Q(Tf|d!5BC1jHIkC(IW;IO;F=`B;PJKz%Ko~rjCVcv zvZ7<@FI+af-v2L@hhJkH+H-cbpZ4+lUtE7MhDOPlqpA&Q6Tso6^`10k( zB-VRlVRfHXfnwSz-fc%$z3I=vkI#gxDJkFIxg}>XP@`eQuV@r|{P(a>u6% zbshAL;`_fWk*`fVne`@i^!iZ$+wJrA7$N>k51Z_K`RLmbsqYJ5Ae zpDHirz501}!eNS?kO8L1^C!)ktV)`Jj^`)VJh@*TSEK%sN(#lz}UAsoG zRciR3+%kN&g+^=xSy?_x3snMB(~e+cy^QWP;%#Cq>n)f`f3<94`odnN$+D3+?MvMI zW0MyJYI6zx>iRu%7OIN=he92%^@l>eWF=PB=s(r^zbEmR*TvEjf4XsIX4m^8zOJ*{ V^V80WL-UV6CkGe%d|Utc-vFTWTn+#L literal 0 HcmV?d00001 diff --git a/samples/winrt/JavaScript/images/windows-sdk.png b/samples/winrt/JavaScript/images/windows-sdk.png new file mode 100644 index 0000000000000000000000000000000000000000..af64bf00ad4a8bfec7ee89ab8a7b61d55988dfd6 GIT binary patch literal 1426 zcmV;D1#S9?P)Px#1ZP1_K>z@;j|==^1poj532;bRa{vGi!~g&e!~vBn4jTXf00v@9M??Vs0RI60 zpuMM)00007bV*G`2i*n}3K|J({%opWa9o|%z`{{H+k-?{nD$#?FZ$C=4>1YrLIKI=rpojc5V z^Tvy7YM9gB?#7=5Igw21?-yd_O8M?+=5o7inGh#VP;z!)3hYQWk+Crf z4D8)YK?FN@Qea?ah5|d1P2|G|3UuU}Dwe36xj72#NH&q$S_*WW$B!wBV0@ec165TN zSVG5&9apcK4^-cAR92e%Nq2Wsk{#%o5K|GpTUsc|z>Xc}KE5A5q(nQ=Ga+l%=tU^z z&CQgkaPp)vx5Noa-t)!{y$IFQ)07y%l?+>56lG$9vF2uK#2~NW71$v%x~D07BI9D5%|P7Y ztOoi9T2RP|gmIpq7p)MEV57c*PC*L+<3z?Fwct3l@>c3;$BP&8LeHrc&*Y@Q&89hD zn@BhBo-eRm{R7!@@uGLEOUnYwp}xM@CrHUlKSOGP<>)8KmTT9F1xe>}^ot0tTq*4% ztuo*I15yhtSGLT~$|AT{)W85Ggb>V^ZfGd(W@vh-#g(hyAX}C%FD*uO7`?sbeCfVE zZ!!px2&n~z>077`lJOUiEjMrKg{YR-$RLG+IajsB-7sD()}+HwkiQ{Y+S*DB2)eVA z5*1#(5@OLJbH4A}x3QfN6H3Xt`k|~a72&(9EBZ8S>f?LYE^IH0%Szd$S_);w^XGa2 zs^d6wMn6ILZf_T&;sJX=*La;Dp{#iFgaQK_Hc${jXJ`1DY;qFIw)f&aHFds*;uwK) zP*Xs)P#5GQ-@7mtFQy=Zs3%AtKVl+{P9rsS2BA3qgbL79Kz~8AkWbW8o{

BDi;t z0t0n*_%S4gl%qmd9F>rouj&^_CAe>^zIBU|LfE)5{1}mU*4W4&;abPnxdWN_9#V6I z8i7=z9%o78{>;cJEU-=azR#Y;Rwg#`#`!;PQZ=s@zeAt+CwJgcs2);tObW7pzg|Sp zy}evKdnUx&w;b08F4nFM|0H~KoVl?PVh~abh*ZqQ)7No++%3M8_@Zpx(cz7Z(4&br z%@>hcm~0q_u}Cl#l+->-C>z;m8j51Yiqc||`qCvD5TIFPicifKlT^b{ImmkuQ*q@w z_~i+_R7wF-3oBdLo$`ugvs7N|}-R0ZNK|g1Qt-I&;ygx!AUi zIvGGpGy_lv^d;n;vEVA`JLp#^E1o_T;=lp#OAxsn$2EtGx;pNjl}?|IUKvZWeL(jg glKR>2!ho@V0n*p4SbNWbssI2007*qoM6N<$f*ydI?EnA( literal 0 HcmV?d00001 diff --git a/samples/winrt/JavaScript/js/AdvancedCapture.js b/samples/winrt/JavaScript/js/AdvancedCapture.js new file mode 100644 index 000000000..6d168c79c --- /dev/null +++ b/samples/winrt/JavaScript/js/AdvancedCapture.js @@ -0,0 +1,161 @@ +//// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +//// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +//// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +//// PARTICULAR PURPOSE. +//// +//// Copyright (c) Microsoft Corporation. All rights reserved + +(function () { + "use strict"; + + var cameraList = null; + var mediaCaptureMgr = null; + var captureInitSettings = null; + + var page = WinJS.UI.Pages.define("/html/AdvancedCapture.html", { + + ready: function (element, options) { + scenarioInitialize(); + }, + + unload: function (element, options) { + // release resources + releaseMediaCapture(); + } + }); + + function scenarioInitialize() { + // Initialize the UI elements + id("btnStartDevice").disabled = false; + id("btnStartDevice").addEventListener("click", startDevice, false); + id("btnStartPreview").disabled = true; + id("videoEffect").disabled = true; + id("btnStartPreview").addEventListener("click", startPreview, false); + id("cameraSelect").addEventListener("change", onDeviceChange, false); + + id("videoEffect").addEventListener('change', addEffectToImageStream, false); + + enumerateCameras(); + } + + function initCameraSettings() { + captureInitSettings = new Windows.Media.Capture.MediaCaptureInitializationSettings(); + captureInitSettings.streamingCaptureMode = Windows.Media.Capture.StreamingCaptureMode.video + + // If the user chose another capture device, use it by default + var selectedIndex = id("cameraSelect").selectedIndex; + var deviceInfo = cameraList[selectedIndex]; + captureInitSettings.videoDeviceId = deviceInfo.id; + } + + // this function takes care of releasing the resources associated with media capturing + function releaseMediaCapture() { + if (mediaCaptureMgr) { + mediaCaptureMgr.close(); + mediaCaptureMgr = null; + } + } + + //Initialize media capture with the current settings + function startDevice() { + displayStatus("Starting device"); + releaseMediaCapture(); + initCameraSettings(); + + mediaCaptureMgr = new Windows.Media.Capture.MediaCapture(); + mediaCaptureMgr.initializeAsync(captureInitSettings).done(function (result) { + // Update the UI + id("btnStartPreview").disabled = false; + id("btnStartDevice").disabled = true; + displayStatus("Device started"); + }); + } + + function startPreview() { + displayStatus("Starting preview"); + id("btnStartPreview").disabled = true; + id("videoEffect").disabled = false; + var video = id("previewVideo"); + video.src = URL.createObjectURL(mediaCaptureMgr, { oneTimeOnly: true }); + video.play(); + displayStatus("Preview started"); + } + + function addEffectToImageStream() { + var effectId = id("videoEffect").selectedIndex; + var props = new Windows.Foundation.Collections.PropertySet(); + props.insert("{698649BE-8EAE-4551-A4CB-3EC98FBD3D86}", effectId); + + mediaCaptureMgr.clearEffectsAsync(Windows.Media.Capture.MediaStreamType.videoPreview).then(function () { + return mediaCaptureMgr.addEffectAsync(Windows.Media.Capture.MediaStreamType.videoPreview, 'OcvTransform.OcvImageManipulations', props); + }).then(function () { + displayStatus('Effect has been successfully added'); + }, errorHandler); + } + + function enumerateCameras() { + displayStatus("Enumerating capture devices"); + var cameraSelect = id("cameraSelect"); + cameraList = null; + cameraList = new Array(); + + // Clear the previous list of capture devices if any + while (cameraSelect.length > 0) { + cameraSelect.remove(0); + } + + // Enumerate cameras and add them to the list + var deviceInfo = Windows.Devices.Enumeration.DeviceInformation; + deviceInfo.findAllAsync(Windows.Devices.Enumeration.DeviceClass.videoCapture).done(function (cameras) { + if (cameras.length === 0) { + cameraSelect.disabled = true; + displayError("No camera was found"); + id("btnStartDevice").disabled = true; + cameraSelect.add(new Option("No cameras available")); + } else { + cameras.forEach(function (camera) { + cameraList.push(camera); + cameraSelect.add(new Option(camera.name)); + }); + } + }, errorHandler); + } + + function onDeviceChange() { + releaseMediaCapture(); + id("btnStartDevice").disabled = false; + id("btnStartPreview").disabled = true; + id("videoEffect").disabled = true; + displayStatus(""); + } + + function suspendingHandler(suspendArg) { + displayStatus("Suspended"); + releaseMediaCapture(); + } + + function resumingHandler(resumeArg) { + displayStatus("Resumed"); + scenarioInitialize(); + } + + function errorHandler(err) { + displayError(err.message); + } + + function failedEventHandler(e) { + displayError("Fatal error", e.message); + } + + function displayStatus(statusText) { + SdkSample.displayStatus(statusText); + } + + function displayError(error) { + SdkSample.displayError(error); + } + + function id(elementId) { + return document.getElementById(elementId); + } +})(); diff --git a/samples/winrt/JavaScript/js/default.js b/samples/winrt/JavaScript/js/default.js new file mode 100644 index 000000000..ffe7248bb --- /dev/null +++ b/samples/winrt/JavaScript/js/default.js @@ -0,0 +1,74 @@ +//// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +//// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +//// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +//// PARTICULAR PURPOSE. +//// +//// Copyright (c) Microsoft Corporation. All rights reserved + + +(function () { + "use strict"; + + var sampleTitle = "OpenCV Image Manipulations sample"; + + var scenarios = [ + { url: "/html/AdvancedCapture.html", title: "Enumerate cameras and add a video effect" }, + ]; + + function activated(eventObject) { + if (eventObject.detail.kind === Windows.ApplicationModel.Activation.ActivationKind.launch) { + // Use setPromise to indicate to the system that the splash screen must not be torn down + // until after processAll and navigate complete asynchronously. + eventObject.setPromise(WinJS.UI.processAll().then(function () { + // Navigate to either the first scenario or to the last running scenario + // before suspension or termination. + var url = WinJS.Application.sessionState.lastUrl || scenarios[0].url; + return WinJS.Navigation.navigate(url); + })); + } + } + + WinJS.Navigation.addEventListener("navigated", function (eventObject) { + var url = eventObject.detail.location; + var host = document.getElementById("contentHost"); + // Call unload method on current scenario, if there is one + host.winControl && host.winControl.unload && host.winControl.unload(); + WinJS.Utilities.empty(host); + eventObject.detail.setPromise(WinJS.UI.Pages.render(url, host, eventObject.detail.state).then(function () { + WinJS.Application.sessionState.lastUrl = url; + })); + }); + + WinJS.Namespace.define("SdkSample", { + sampleTitle: sampleTitle, + scenarios: scenarios, + mediaCaptureMgr: null, + photoFile: "photo.jpg", + deviceList: null, + recordState: null, + captureInitSettings: null, + encodingProfile: null, + storageFile: null, + photoStorage: null, + cameraControlSliders: null, + + + displayStatus: function (statusText) { + WinJS.log && WinJS.log(statusText, "MediaCapture", "status"); + }, + + displayError: function (error) { + WinJS.log && WinJS.log(error, "MediaCapture", "error"); + }, + + id: function (elementId) { + return document.getElementById(elementId); + }, + + }); + + WinJS.Application.addEventListener("activated", activated, false); + WinJS.Application.start(); + Windows.UI.WebUI.WebUIApplication.addEventListener("suspending", SdkSample.suspendingHandler, false); + Windows.UI.WebUI.WebUIApplication.addEventListener("resuming", SdkSample.resumingHandler, false); +})(); diff --git a/samples/winrt/JavaScript/package.appxmanifest b/samples/winrt/JavaScript/package.appxmanifest new file mode 100644 index 000000000..aa4526c2a --- /dev/null +++ b/samples/winrt/JavaScript/package.appxmanifest @@ -0,0 +1,35 @@ + + + + + MediaCaptureJavaScript + Sergei + images\storelogo.png + + + 6.3.0 + 6.3.0 + + + + + + + + + + + + + + + + + + + OcvTransform.dll + + + + + \ No newline at end of file diff --git a/samples/winrt/JavaScript/sample-utils/sample-utils.css b/samples/winrt/JavaScript/sample-utils/sample-utils.css new file mode 100644 index 000000000..4baf32c80 --- /dev/null +++ b/samples/winrt/JavaScript/sample-utils/sample-utils.css @@ -0,0 +1,215 @@ +/* Copyright (c) Microsoft Corporation. All rights reserved. */ +html +{ + cursor: default; +} + +#featureLabel +{ + font: 20pt/24pt "Segoe UI Semilight"; + margin:0; + padding:5px 0 10px 0; + font-weight: normal; +} + +#inputLabel, #outputLabel +{ + font: 11pt/15pt "Segoe UI"; + margin:0; + padding:0; + font-weight: normal; +} + +#listLabel, #descLabel +{ + font: 11pt/15pt "Segoe UI Semilight"; + font-weight:normal; +} + +#rootGrid +{ + width: 100%; + height: 100%; + display: -ms-grid; + -ms-grid-columns: 100px 1fr 100px; + -ms-grid-rows: 20px auto 1fr auto 20px; +} + +#header +{ + -ms-grid-column: 2; + -ms-grid-row: 2; +} + +#content +{ + padding-right:20px; + padding-bottom:20px; + overflow:auto; + display:-ms-grid; + -ms-grid-columns:1fr; + -ms-grid-rows: auto 1fr; + -ms-grid-column: 2; + -ms-grid-row: 3; +} + +#footer +{ + -ms-grid-column: 2; + -ms-grid-row: 4; + padding-bottom:10px; +} + +#featureLabel +{ + -ms-grid-row: 1; +} + +#contentHost +{ + display:-ms-grid; + -ms-grid-columns:1fr; + -ms-grid-rows: auto auto auto 1fr; + -ms-grid-row: 2; +} + +#inputLabel +{ + -ms-grid-row: 1; +} + + +#input +{ + -ms-grid-row: 2; + display: -ms-grid; + -ms-grid-columns: auto auto; + -ms-grid-rows: auto; + margin-top:10px; +} + +#outputLabel +{ + -ms-grid-row: 3; + padding-top:10px; + padding-bottom:10px; +} + +#output +{ + height:100%; + -ms-grid-row: 4; + -ms-grid-row-align:stretch; +} + +.clear +{ + clear:both; +} + + +#footer span +{ + font-size:12px; +} + +#footer .company +{ + float:left; +} + +#footer .links +{ + float:right; +} + +#footer .links a +{ + font-size:12px; + margin-left:8px; + text-decoration:none; +} + +#footer .links .pipe +{ + font-size:9px; + margin-left:8px; +} + +#statusMessage +{ + margin-bottom:5px; +} + +#input .options +{ + -ms-grid-row: 1; + -ms-grid-column: 1; +} + +#input .details +{ + -ms-grid-row: 1; + -ms-grid-column: 2; + cursor:text; +} + +.imageHolder +{ + max-width:382px; +} + +.imageHolder.withText +{ + float:left; + margin-right:10px; +} + +#scenarios +{ + margin-right:20px; +} + + + +@media screen and (min-width: 800px) and (max-width: 1024px) +{ + #rootGrid + { + -ms-grid-columns: 40px 1fr 40px; + } +} + +@media screen and (max-width: 799px) +{ + #rootGrid + { + -ms-grid-columns: 20px 1fr 20px; + } + + #output + { + padding-bottom:20px; + } + + #input + { + -ms-grid-columns: auto; + -ms-grid-rows: auto auto; + } + + #input .options + { + -ms-grid-row: 1; + -ms-grid-column: 1; + margin-bottom:10px; + } + + #input .details + { + -ms-grid-row: 2; + -ms-grid-column: 1; + } +} + + diff --git a/samples/winrt/JavaScript/sample-utils/sample-utils.js b/samples/winrt/JavaScript/sample-utils/sample-utils.js new file mode 100644 index 000000000..ad2834d8c --- /dev/null +++ b/samples/winrt/JavaScript/sample-utils/sample-utils.js @@ -0,0 +1,204 @@ +//// Copyright (c) Microsoft Corporation. All rights reserved + +// This file is a part of the SDK sample framework. For code demonstrating scenarios in this particular sample, +// please see the html, css and js folders. + +(function () { + + // + // Helper controls used in the sample pages + // + + // The ScenarioInput control inserts the appropriate markup to get labels & controls + // hooked into the input section of a scenario page so that it's not repeated in + // every one. + + var lastError = ""; + var lastStatus = ""; + var ScenarioInput = WinJS.Class.define( + function (element, options) { + element.winControl = this; + this.element = element; + + new WinJS.Utilities.QueryCollection(element) + .setAttribute("role", "main") + .setAttribute("aria-labelledby", "inputLabel"); + element.id = "input"; + + this.addInputLabel(element); + this.addDetailsElement(element); + this.addScenariosPicker(element); + }, { + addInputLabel: function (element) { + var label = document.createElement("h2"); + label.textContent = "Input"; + label.id = "inputLabel"; + element.parentNode.insertBefore(label, element); + }, + addScenariosPicker: function (parentElement) { + var scenarios = document.createElement("div"); + scenarios.id = "scenarios"; + var control = new ScenarioSelect(scenarios); + + parentElement.insertBefore(scenarios, parentElement.childNodes[0]); + }, + + addDetailsElement: function (sourceElement) { + var detailsDiv = this._createDetailsDiv(); + while (sourceElement.childNodes.length > 0) { + detailsDiv.appendChild(sourceElement.removeChild(sourceElement.childNodes[0])); + } + sourceElement.appendChild(detailsDiv); + }, + _createDetailsDiv: function () { + var detailsDiv = document.createElement("div"); + + new WinJS.Utilities.QueryCollection(detailsDiv) + .addClass("details") + .setAttribute("role", "region") + .setAttribute("aria-labelledby", "descLabel") + .setAttribute("aria-live", "assertive"); + + var label = document.createElement("h3"); + label.textContent = "Description"; + label.id = "descLabel"; + + detailsDiv.appendChild(label); + return detailsDiv; + }, + } + ); + + // The ScenarioOutput control inserts the appropriate markup to get labels & controls + // hooked into the output section of a scenario page so that it's not repeated in + // every one. + + var ScenarioOutput = WinJS.Class.define( + function (element, options) { + element.winControl = this; + this.element = element; + new WinJS.Utilities.QueryCollection(element) + .setAttribute("role", "region") + .setAttribute("aria-labelledby", "outputLabel") + .setAttribute("aria-live", "assertive"); + element.id = "output"; + + this._addOutputLabel(element); + this._addStatusOutput(element); + }, { + _addOutputLabel: function (element) { + var label = document.createElement("h2"); + label.id = "outputLabel"; + label.textContent = "Output"; + element.parentNode.insertBefore(label, element); + }, + _addStatusOutput: function (element) { + var statusDiv = document.createElement("div"); + statusDiv.id = "statusMessage"; + statusDiv.setAttribute("role", "textbox"); + element.insertBefore(statusDiv, element.childNodes[0]); + } + } + ); + + + // Sample infrastructure internals + + var currentScenarioUrl = null; + + WinJS.Navigation.addEventListener("navigating", function (evt) { + currentScenarioUrl = evt.detail.location; + }); + + WinJS.log = function (message, tag, type) { + var isError = (type === "error"); + var isStatus = (type === "status"); + + if (isError || isStatus) { + var statusDiv = /* @type(HTMLElement) */ document.getElementById("statusMessage"); + if (statusDiv) { + statusDiv.innerText = message; + if (isError) { + lastError = message; + statusDiv.style.color = "blue"; + } else if (isStatus) { + lastStatus = message; + statusDiv.style.color = "green"; + } + } + } + }; + + // Control that populates and runs the scenario selector + + var ScenarioSelect = WinJS.UI.Pages.define("/sample-utils/scenario-select.html", { + ready: function (element, options) { + var that = this; + var selectElement = WinJS.Utilities.query("#scenarioSelect", element); + this._selectElement = selectElement[0]; + + SdkSample.scenarios.forEach(function (s, index) { + that._addScenario(index, s); + }); + + selectElement.listen("change", function (evt) { + var select = evt.target; + if (select.selectedIndex >= 0) { + var newUrl = select.options[select.selectedIndex].value; + WinJS.Navigation.navigate(newUrl); + } + }); + selectElement[0].size = (SdkSample.scenarios.length > 5 ? 5 : SdkSample.scenarios.length); + if (SdkSample.scenarios.length === 1) { + // Avoid showing down arrow when there is only one scenario + selectElement[0].setAttribute("multiple", "multiple"); + } + + // Use setImmediate to ensure that the select element is set as active only after + // the scenario page has been constructed. + setImmediate(function () { + that._selectElement.setActive(); + }); + }, + + _addScenario: function (index, info) { + var option = document.createElement("option"); + if (info.url === currentScenarioUrl) { + option.selected = "selected"; + } + option.text = (index + 1) + ") " + info.title; + option.value = info.url; + this._selectElement.appendChild(option); + } + }); + + function activated(e) { + WinJS.Utilities.query("#featureLabel")[0].textContent = SdkSample.sampleTitle; + } + + WinJS.Application.addEventListener("activated", activated, false); + + // Export public methods & controls + WinJS.Namespace.define("SdkSample", { + ScenarioInput: ScenarioInput, + ScenarioOutput: ScenarioOutput + }); + + // SDK Sample Test helper + document.TestSdkSample = { + getLastError: function () { + return lastError; + }, + + getLastStatus: function () { + return lastStatus; + }, + + selectScenario: function (scenarioID) { + scenarioID = scenarioID >> 0; + var select = document.getElementById("scenarioSelect"); + var newUrl = select.options[scenarioID - 1].value; + WinJS.Navigation.navigate(newUrl); + } + }; +})(); diff --git a/samples/winrt/JavaScript/sample-utils/scenario-select.html b/samples/winrt/JavaScript/sample-utils/scenario-select.html new file mode 100644 index 000000000..011fabee6 --- /dev/null +++ b/samples/winrt/JavaScript/sample-utils/scenario-select.html @@ -0,0 +1,15 @@ + + + + + + + +

+

Select scenario:

+ +
+ + From 7335a40a61e29edf1d1c48300197c39b6bdf7b4d Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Wed, 18 Mar 2015 17:23:42 +0300 Subject: [PATCH 101/171] Replaced CV_PURE_PROPERTY macros with corresponding code --- doc/Doxyfile.in | 6 +- modules/core/include/opencv2/core.hpp | 13 - modules/ml/include/opencv2/ml.hpp | 225 ++++++++++++++---- modules/superres/include/opencv2/superres.hpp | 50 +++- .../include/opencv2/superres/optical_flow.hpp | 120 ++++++++-- .../video/include/opencv2/video/tracking.hpp | 60 ++++- 6 files changed, 365 insertions(+), 109 deletions(-) diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index 93ccafaae..192081e9b 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -243,11 +243,7 @@ PREDEFINED = __cplusplus=1 \ CV_NORETURN= \ CV_DEFAULT(x)=" = x" \ CV_NEON=1 \ - FLANN_DEPRECATED= \ - "CV_PURE_PROPERTY(type, name)= /** \@see set##name */ virtual type get##name() const = 0; /** \@copybrief get##name \@see get##name */ virtual void set##name(type val) = 0;" \ - "CV_IMPL_PROPERTY(type, name, x)= /** \@see set##name */ virtual type get##name() const = 0; /** \@copybrief get##name \@see get##name */ virtual void set##name(type val) = 0;" \ - "CV_IMPL_PROPERTY_S(type, name, x)= /** \@see set##name */ virtual type get##name() const = 0; /** \@copybrief get##name \@see get##name */ virtual void set##name(const type & val);" \ - "CV_IMPL_PROPERTY_RO(type, name, x)= virtual type get##name() const;" + FLANN_DEPRECATED= EXPAND_AS_DEFINED = SKIP_FUNCTION_MACROS = YES TAGFILES = diff --git a/modules/core/include/opencv2/core.hpp b/modules/core/include/opencv2/core.hpp index 896aa12bf..ae6de3a97 100644 --- a/modules/core/include/opencv2/core.hpp +++ b/modules/core/include/opencv2/core.hpp @@ -2821,19 +2821,6 @@ public: virtual void read(const FileNode& fn) { (void)fn; } }; -// define properties - -#define CV_PURE_PROPERTY(type, name) \ - CV_WRAP virtual type get##name() const = 0; \ - CV_WRAP virtual void set##name(type val) = 0; - -#define CV_PURE_PROPERTY_S(type, name) \ - CV_WRAP virtual type get##name() const = 0; \ - CV_WRAP virtual void set##name(const type & val) = 0; - -#define CV_PURE_PROPERTY_RO(type, name) \ - CV_WRAP virtual type get##name() const = 0; - // basic property implementation #define CV_IMPL_PROPERTY_RO(type, name, member) \ diff --git a/modules/ml/include/opencv2/ml.hpp b/modules/ml/include/opencv2/ml.hpp index 8959c58d8..f5aa85064 100644 --- a/modules/ml/include/opencv2/ml.hpp +++ b/modules/ml/include/opencv2/ml.hpp @@ -440,16 +440,28 @@ class CV_EXPORTS_W KNearest : public StatModel public: /** Default number of neighbors to use in predict method. */ - CV_PURE_PROPERTY(int, DefaultK) + /** @see setDefaultK */ + virtual int getDefaultK() const = 0; + /** @copybrief getDefaultK @see getDefaultK */ + virtual void setDefaultK(int val) = 0; /** Whether classification or regression model should be trained. */ - CV_PURE_PROPERTY(bool, IsClassifier) + /** @see setIsClassifier */ + virtual bool getIsClassifier() const = 0; + /** @copybrief getIsClassifier @see getIsClassifier */ + virtual void setIsClassifier(bool val) = 0; /** Parameter for KDTree implementation. */ - CV_PURE_PROPERTY(int, Emax) + /** @see setEmax */ + virtual int getEmax() const = 0; + /** @copybrief getEmax @see getEmax */ + virtual void setEmax(int val) = 0; /** %Algorithm type, one of KNearest::Types. */ - CV_PURE_PROPERTY(int, AlgorithmType) + /** @see setAlgorithmType */ + virtual int getAlgorithmType() const = 0; + /** @copybrief getAlgorithmType @see getAlgorithmType */ + virtual void setAlgorithmType(int val) = 0; /** @brief Finds the neighbors and predicts responses for input vectors. @@ -518,44 +530,71 @@ public: /** Type of a %SVM formulation. See SVM::Types. Default value is SVM::C_SVC. */ - CV_PURE_PROPERTY(int, Type) + /** @see setType */ + virtual int getType() const = 0; + /** @copybrief getType @see getType */ + virtual void setType(int val) = 0; /** Parameter \f$\gamma\f$ of a kernel function. For SVM::POLY, SVM::RBF, SVM::SIGMOID or SVM::CHI2. Default value is 1. */ - CV_PURE_PROPERTY(double, Gamma) + /** @see setGamma */ + virtual double getGamma() const = 0; + /** @copybrief getGamma @see getGamma */ + virtual void setGamma(double val) = 0; /** Parameter _coef0_ of a kernel function. For SVM::POLY or SVM::SIGMOID. Default value is 0.*/ - CV_PURE_PROPERTY(double, Coef0) + /** @see setCoef0 */ + virtual double getCoef0() const = 0; + /** @copybrief getCoef0 @see getCoef0 */ + virtual void setCoef0(double val) = 0; /** Parameter _degree_ of a kernel function. For SVM::POLY. Default value is 0. */ - CV_PURE_PROPERTY(double, Degree) + /** @see setDegree */ + virtual double getDegree() const = 0; + /** @copybrief getDegree @see getDegree */ + virtual void setDegree(double val) = 0; /** Parameter _C_ of a %SVM optimization problem. For SVM::C_SVC, SVM::EPS_SVR or SVM::NU_SVR. Default value is 0. */ - CV_PURE_PROPERTY(double, C) + /** @see setC */ + virtual double getC() const = 0; + /** @copybrief getC @see getC */ + virtual void setC(double val) = 0; /** Parameter \f$\nu\f$ of a %SVM optimization problem. For SVM::NU_SVC, SVM::ONE_CLASS or SVM::NU_SVR. Default value is 0. */ - CV_PURE_PROPERTY(double, Nu) + /** @see setNu */ + virtual double getNu() const = 0; + /** @copybrief getNu @see getNu */ + virtual void setNu(double val) = 0; /** Parameter \f$\epsilon\f$ of a %SVM optimization problem. For SVM::EPS_SVR. Default value is 0. */ - CV_PURE_PROPERTY(double, P) + /** @see setP */ + virtual double getP() const = 0; + /** @copybrief getP @see getP */ + virtual void setP(double val) = 0; /** Optional weights in the SVM::C_SVC problem, assigned to particular classes. They are multiplied by _C_ so the parameter _C_ of class _i_ becomes `classWeights(i) * C`. Thus these weights affect the misclassification penalty for different classes. The larger weight, the larger penalty on misclassification of data from the corresponding class. Default value is empty Mat. */ - CV_PURE_PROPERTY_S(cv::Mat, ClassWeights) + /** @see setClassWeights */ + virtual cv::Mat getClassWeights() const = 0; + /** @copybrief getClassWeights @see getClassWeights */ + virtual void setClassWeights(const cv::Mat &val) = 0; /** Termination criteria of the iterative %SVM training procedure which solves a partial case of constrained quadratic optimization problem. You can specify tolerance and/or the maximum number of iterations. Default value is `TermCriteria( TermCriteria::MAX_ITER + TermCriteria::EPS, 1000, FLT_EPSILON )`; */ - CV_PURE_PROPERTY_S(cv::TermCriteria, TermCriteria) + /** @see setTermCriteria */ + virtual cv::TermCriteria getTermCriteria() const = 0; + /** @copybrief getTermCriteria @see getTermCriteria */ + virtual void setTermCriteria(const cv::TermCriteria &val) = 0; /** Type of a %SVM kernel. See SVM::KernelTypes. Default value is SVM::RBF. */ @@ -755,17 +794,26 @@ public: Default value of the parameter is EM::DEFAULT_NCLUSTERS=5. Some of %EM implementation could determine the optimal number of mixtures within a specified value range, but that is not the case in ML yet. */ - CV_PURE_PROPERTY(int, ClustersNumber) + /** @see setClustersNumber */ + virtual int getClustersNumber() const = 0; + /** @copybrief getClustersNumber @see getClustersNumber */ + virtual void setClustersNumber(int val) = 0; /** Constraint on covariance matrices which defines type of matrices. See EM::Types. */ - CV_PURE_PROPERTY(int, CovarianceMatrixType) + /** @see setCovarianceMatrixType */ + virtual int getCovarianceMatrixType() const = 0; + /** @copybrief getCovarianceMatrixType @see getCovarianceMatrixType */ + virtual void setCovarianceMatrixType(int val) = 0; /** The termination criteria of the %EM algorithm. The %EM algorithm can be terminated by the number of iterations termCrit.maxCount (number of M-steps) or when relative change of likelihood logarithm is less than termCrit.epsilon. Default maximum number of iterations is EM::DEFAULT_MAX_ITERS=100. */ - CV_PURE_PROPERTY_S(TermCriteria, TermCriteria) + /** @see setTermCriteria */ + virtual TermCriteria getTermCriteria() const = 0; + /** @copybrief getTermCriteria @see getTermCriteria */ + virtual void setTermCriteria(const TermCriteria &val) = 0; /** @brief Returns weights of the mixtures @@ -926,46 +974,70 @@ public: values. In case of regression and 2-class classification the optimal split can be found efficiently without employing clustering, thus the parameter is not used in these cases. Default value is 10.*/ - CV_PURE_PROPERTY(int, MaxCategories) + /** @see setMaxCategories */ + virtual int getMaxCategories() const = 0; + /** @copybrief getMaxCategories @see getMaxCategories */ + virtual void setMaxCategories(int val) = 0; /** The maximum possible depth of the tree. That is the training algorithms attempts to split a node while its depth is less than maxDepth. The root node has zero depth. The actual depth may be smaller if the other termination criteria are met (see the outline of the training procedure @ref ml_intro_trees "here"), and/or if the tree is pruned. Default value is INT_MAX.*/ - CV_PURE_PROPERTY(int, MaxDepth) + /** @see setMaxDepth */ + virtual int getMaxDepth() const = 0; + /** @copybrief getMaxDepth @see getMaxDepth */ + virtual void setMaxDepth(int val) = 0; /** If the number of samples in a node is less than this parameter then the node will not be split. Default value is 10.*/ - CV_PURE_PROPERTY(int, MinSampleCount) + /** @see setMinSampleCount */ + virtual int getMinSampleCount() const = 0; + /** @copybrief getMinSampleCount @see getMinSampleCount */ + virtual void setMinSampleCount(int val) = 0; /** If CVFolds \> 1 then algorithms prunes the built decision tree using K-fold cross-validation procedure where K is equal to CVFolds. Default value is 10.*/ - CV_PURE_PROPERTY(int, CVFolds) + /** @see setCVFolds */ + virtual int getCVFolds() const = 0; + /** @copybrief getCVFolds @see getCVFolds */ + virtual void setCVFolds(int val) = 0; /** If true then surrogate splits will be built. These splits allow to work with missing data and compute variable importance correctly. Default value is false. @note currently it's not implemented.*/ - CV_PURE_PROPERTY(bool, UseSurrogates) + /** @see setUseSurrogates */ + virtual bool getUseSurrogates() const = 0; + /** @copybrief getUseSurrogates @see getUseSurrogates */ + virtual void setUseSurrogates(bool val) = 0; /** If true then a pruning will be harsher. This will make a tree more compact and more resistant to the training data noise but a bit less accurate. Default value is true.*/ - CV_PURE_PROPERTY(bool, Use1SERule) + /** @see setUse1SERule */ + virtual bool getUse1SERule() const = 0; + /** @copybrief getUse1SERule @see getUse1SERule */ + virtual void setUse1SERule(bool val) = 0; /** If true then pruned branches are physically removed from the tree. Otherwise they are retained and it is possible to get results from the original unpruned (or pruned less aggressively) tree. Default value is true.*/ - CV_PURE_PROPERTY(bool, TruncatePrunedTree) + /** @see setTruncatePrunedTree */ + virtual bool getTruncatePrunedTree() const = 0; + /** @copybrief getTruncatePrunedTree @see getTruncatePrunedTree */ + virtual void setTruncatePrunedTree(bool val) = 0; /** Termination criteria for regression trees. If all absolute differences between an estimated value in a node and values of train samples in this node are less than this parameter then the node will not be split further. Default value is 0.01f*/ - CV_PURE_PROPERTY(float, RegressionAccuracy) + /** @see setRegressionAccuracy */ + virtual float getRegressionAccuracy() const = 0; + /** @copybrief getRegressionAccuracy @see getRegressionAccuracy */ + virtual void setRegressionAccuracy(float val) = 0; /** @brief The array of a priori class probabilities, sorted by the class label value. @@ -982,7 +1054,10 @@ public: category is 1 and the weight of the second category is 10, then each mistake in predicting the second category is equivalent to making 10 mistakes in predicting the first category. Default value is empty Mat.*/ - CV_PURE_PROPERTY_S(cv::Mat, Priors) + /** @see setPriors */ + virtual cv::Mat getPriors() const = 0; + /** @copybrief getPriors @see getPriors */ + virtual void setPriors(const cv::Mat &val) = 0; /** @brief The class represents a decision tree node. */ @@ -1071,13 +1146,19 @@ public: /** If true then variable importance will be calculated and then it can be retrieved by RTrees::getVarImportance. Default value is false.*/ - CV_PURE_PROPERTY(bool, CalculateVarImportance) + /** @see setCalculateVarImportance */ + virtual bool getCalculateVarImportance() const = 0; + /** @copybrief getCalculateVarImportance @see getCalculateVarImportance */ + virtual void setCalculateVarImportance(bool val) = 0; /** The size of the randomly selected subset of features at each tree node and that are used to find the best split(s). If you set it to 0 then the size will be set to the square root of the total number of features. Default value is 0.*/ - CV_PURE_PROPERTY(int, ActiveVarCount) + /** @see setActiveVarCount */ + virtual int getActiveVarCount() const = 0; + /** @copybrief getActiveVarCount @see getActiveVarCount */ + virtual void setActiveVarCount(int val) = 0; /** The termination criteria that specifies when the training algorithm stops. Either when the specified number of trees is trained and added to the ensemble or when @@ -1086,7 +1167,10 @@ public: pass a certain number of trees. Also to keep in mind, the number of tree increases the prediction time linearly. Default value is TermCriteria(TermCriteria::MAX_ITERS + TermCriteria::EPS, 50, 0.1)*/ - CV_PURE_PROPERTY_S(TermCriteria, TermCriteria) + /** @see setTermCriteria */ + virtual TermCriteria getTermCriteria() const = 0; + /** @copybrief getTermCriteria @see getTermCriteria */ + virtual void setTermCriteria(const TermCriteria &val) = 0; /** Returns the variable importance array. The method returns the variable importance vector, computed at the training stage when @@ -1115,16 +1199,25 @@ class CV_EXPORTS_W Boost : public DTrees public: /** Type of the boosting algorithm. See Boost::Types. Default value is Boost::REAL. */ - CV_PURE_PROPERTY(int, BoostType) + /** @see setBoostType */ + virtual int getBoostType() const = 0; + /** @copybrief getBoostType @see getBoostType */ + virtual void setBoostType(int val) = 0; /** The number of weak classifiers. Default value is 100. */ - CV_PURE_PROPERTY(int, WeakCount) + /** @see setWeakCount */ + virtual int getWeakCount() const = 0; + /** @copybrief getWeakCount @see getWeakCount */ + virtual void setWeakCount(int val) = 0; /** A threshold between 0 and 1 used to save computational time. Samples with summary weight \f$\leq 1 - weight_trim_rate\f$ do not participate in the *next* iteration of training. Set this parameter to 0 to turn off this functionality. Default value is 0.95.*/ - CV_PURE_PROPERTY(double, WeightTrimRate) + /** @see setWeightTrimRate */ + virtual double getWeightTrimRate() const = 0; + /** @copybrief getWeightTrimRate @see getWeightTrimRate */ + virtual void setWeightTrimRate(double val) = 0; /** Boosting type. Gentle AdaBoost and Real AdaBoost are often the preferable choices. */ @@ -1232,37 +1325,61 @@ public: You can specify the maximum number of iterations (maxCount) and/or how much the error could change between the iterations to make the algorithm continue (epsilon). Default value is TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 1000, 0.01).*/ - CV_PURE_PROPERTY(TermCriteria, TermCriteria) + /** @see setTermCriteria */ + virtual TermCriteria getTermCriteria() const = 0; + /** @copybrief getTermCriteria @see getTermCriteria */ + virtual void setTermCriteria(TermCriteria val) = 0; /** BPROP: Strength of the weight gradient term. The recommended value is about 0.1. Default value is 0.1.*/ - CV_PURE_PROPERTY(double, BackpropWeightScale) + /** @see setBackpropWeightScale */ + virtual double getBackpropWeightScale() const = 0; + /** @copybrief getBackpropWeightScale @see getBackpropWeightScale */ + virtual void setBackpropWeightScale(double val) = 0; /** BPROP: Strength of the momentum term (the difference between weights on the 2 previous iterations). This parameter provides some inertia to smooth the random fluctuations of the weights. It can vary from 0 (the feature is disabled) to 1 and beyond. The value 0.1 or so is good enough. Default value is 0.1.*/ - CV_PURE_PROPERTY(double, BackpropMomentumScale) + /** @see setBackpropMomentumScale */ + virtual double getBackpropMomentumScale() const = 0; + /** @copybrief getBackpropMomentumScale @see getBackpropMomentumScale */ + virtual void setBackpropMomentumScale(double val) = 0; /** RPROP: Initial value \f$\Delta_0\f$ of update-values \f$\Delta_{ij}\f$. Default value is 0.1.*/ - CV_PURE_PROPERTY(double, RpropDW0) + /** @see setRpropDW0 */ + virtual double getRpropDW0() const = 0; + /** @copybrief getRpropDW0 @see getRpropDW0 */ + virtual void setRpropDW0(double val) = 0; /** RPROP: Increase factor \f$\eta^+\f$. It must be \>1. Default value is 1.2.*/ - CV_PURE_PROPERTY(double, RpropDWPlus) + /** @see setRpropDWPlus */ + virtual double getRpropDWPlus() const = 0; + /** @copybrief getRpropDWPlus @see getRpropDWPlus */ + virtual void setRpropDWPlus(double val) = 0; /** RPROP: Decrease factor \f$\eta^-\f$. It must be \<1. Default value is 0.5.*/ - CV_PURE_PROPERTY(double, RpropDWMinus) + /** @see setRpropDWMinus */ + virtual double getRpropDWMinus() const = 0; + /** @copybrief getRpropDWMinus @see getRpropDWMinus */ + virtual void setRpropDWMinus(double val) = 0; /** RPROP: Update-values lower limit \f$\Delta_{min}\f$. It must be positive. Default value is FLT_EPSILON.*/ - CV_PURE_PROPERTY(double, RpropDWMin) + /** @see setRpropDWMin */ + virtual double getRpropDWMin() const = 0; + /** @copybrief getRpropDWMin @see getRpropDWMin */ + virtual void setRpropDWMin(double val) = 0; /** RPROP: Update-values upper limit \f$\Delta_{max}\f$. It must be \>1. Default value is 50.*/ - CV_PURE_PROPERTY(double, RpropDWMax) + /** @see setRpropDWMax */ + virtual double getRpropDWMax() const = 0; + /** @copybrief getRpropDWMax @see getRpropDWMax */ + virtual void setRpropDWMax(double val) = 0; /** possible activation functions */ enum ActivationFunctions { @@ -1318,24 +1435,42 @@ class CV_EXPORTS LogisticRegression : public StatModel public: /** Learning rate. */ - CV_PURE_PROPERTY(double, LearningRate) + /** @see setLearningRate */ + virtual double getLearningRate() const = 0; + /** @copybrief getLearningRate @see getLearningRate */ + virtual void setLearningRate(double val) = 0; /** Number of iterations. */ - CV_PURE_PROPERTY(int, Iterations) + /** @see setIterations */ + virtual int getIterations() const = 0; + /** @copybrief getIterations @see getIterations */ + virtual void setIterations(int val) = 0; /** Kind of regularization to be applied. See LogisticRegression::RegKinds. */ - CV_PURE_PROPERTY(int, Regularization) + /** @see setRegularization */ + virtual int getRegularization() const = 0; + /** @copybrief getRegularization @see getRegularization */ + virtual void setRegularization(int val) = 0; /** Kind of training method used. See LogisticRegression::Methods. */ - CV_PURE_PROPERTY(int, TrainMethod) + /** @see setTrainMethod */ + virtual int getTrainMethod() const = 0; + /** @copybrief getTrainMethod @see getTrainMethod */ + virtual void setTrainMethod(int val) = 0; /** Specifies the number of training samples taken in each step of Mini-Batch Gradient Descent. Will only be used if using LogisticRegression::MINI_BATCH training algorithm. It has to take values less than the total number of training samples. */ - CV_PURE_PROPERTY(int, MiniBatchSize) + /** @see setMiniBatchSize */ + virtual int getMiniBatchSize() const = 0; + /** @copybrief getMiniBatchSize @see getMiniBatchSize */ + virtual void setMiniBatchSize(int val) = 0; /** Termination criteria of the algorithm. */ - CV_PURE_PROPERTY(TermCriteria, TermCriteria) + /** @see setTermCriteria */ + virtual TermCriteria getTermCriteria() const = 0; + /** @copybrief getTermCriteria @see getTermCriteria */ + virtual void setTermCriteria(TermCriteria val) = 0; //! Regularization kinds enum RegKinds { diff --git a/modules/superres/include/opencv2/superres.hpp b/modules/superres/include/opencv2/superres.hpp index acc067302..dec8e4eda 100644 --- a/modules/superres/include/opencv2/superres.hpp +++ b/modules/superres/include/opencv2/superres.hpp @@ -105,34 +105,64 @@ namespace cv virtual void collectGarbage(); //! @brief Scale factor - CV_PURE_PROPERTY(int, Scale) + /** @see setScale */ + virtual int getScale() const = 0; + /** @copybrief getScale @see getScale */ + virtual void setScale(int val) = 0; //! @brief Iterations count - CV_PURE_PROPERTY(int, Iterations) + /** @see setIterations */ + virtual int getIterations() const = 0; + /** @copybrief getIterations @see getIterations */ + virtual void setIterations(int val) = 0; //! @brief Asymptotic value of steepest descent method - CV_PURE_PROPERTY(double, Tau) + /** @see setTau */ + virtual double getTau() const = 0; + /** @copybrief getTau @see getTau */ + virtual void setTau(double val) = 0; //! @brief Weight parameter to balance data term and smoothness term - CV_PURE_PROPERTY(double, Labmda) + /** @see setLabmda */ + virtual double getLabmda() const = 0; + /** @copybrief getLabmda @see getLabmda */ + virtual void setLabmda(double val) = 0; //! @brief Parameter of spacial distribution in Bilateral-TV - CV_PURE_PROPERTY(double, Alpha) + /** @see setAlpha */ + virtual double getAlpha() const = 0; + /** @copybrief getAlpha @see getAlpha */ + virtual void setAlpha(double val) = 0; //! @brief Kernel size of Bilateral-TV filter - CV_PURE_PROPERTY(int, KernelSize) + /** @see setKernelSize */ + virtual int getKernelSize() const = 0; + /** @copybrief getKernelSize @see getKernelSize */ + virtual void setKernelSize(int val) = 0; //! @brief Gaussian blur kernel size - CV_PURE_PROPERTY(int, BlurKernelSize) + /** @see setBlurKernelSize */ + virtual int getBlurKernelSize() const = 0; + /** @copybrief getBlurKernelSize @see getBlurKernelSize */ + virtual void setBlurKernelSize(int val) = 0; //! @brief Gaussian blur sigma - CV_PURE_PROPERTY(double, BlurSigma) + /** @see setBlurSigma */ + virtual double getBlurSigma() const = 0; + /** @copybrief getBlurSigma @see getBlurSigma */ + virtual void setBlurSigma(double val) = 0; //! @brief Radius of the temporal search area - CV_PURE_PROPERTY(int, TemporalAreaRadius) + /** @see setTemporalAreaRadius */ + virtual int getTemporalAreaRadius() const = 0; + /** @copybrief getTemporalAreaRadius @see getTemporalAreaRadius */ + virtual void setTemporalAreaRadius(int val) = 0; //! @brief Dense optical flow algorithm - CV_PURE_PROPERTY_S(Ptr, OpticalFlow) + /** @see setOpticalFlow */ + virtual Ptr getOpticalFlow() const = 0; + /** @copybrief getOpticalFlow @see getOpticalFlow */ + virtual void setOpticalFlow(const Ptr &val) = 0; protected: SuperResolution(); diff --git a/modules/superres/include/opencv2/superres/optical_flow.hpp b/modules/superres/include/opencv2/superres/optical_flow.hpp index add606c02..d2f29a39b 100644 --- a/modules/superres/include/opencv2/superres/optical_flow.hpp +++ b/modules/superres/include/opencv2/superres/optical_flow.hpp @@ -64,13 +64,34 @@ namespace cv class CV_EXPORTS FarnebackOpticalFlow : public virtual DenseOpticalFlowExt { public: - CV_PURE_PROPERTY(double, PyrScale) - CV_PURE_PROPERTY(int, LevelsNumber) - CV_PURE_PROPERTY(int, WindowSize) - CV_PURE_PROPERTY(int, Iterations) - CV_PURE_PROPERTY(int, PolyN) - CV_PURE_PROPERTY(double, PolySigma) - CV_PURE_PROPERTY(int, Flags) + /** @see setPyrScale */ + virtual double getPyrScale() const = 0; + /** @copybrief getPyrScale @see getPyrScale */ + virtual void setPyrScale(double val) = 0; + /** @see setLevelsNumber */ + virtual int getLevelsNumber() const = 0; + /** @copybrief getLevelsNumber @see getLevelsNumber */ + virtual void setLevelsNumber(int val) = 0; + /** @see setWindowSize */ + virtual int getWindowSize() const = 0; + /** @copybrief getWindowSize @see getWindowSize */ + virtual void setWindowSize(int val) = 0; + /** @see setIterations */ + virtual int getIterations() const = 0; + /** @copybrief getIterations @see getIterations */ + virtual void setIterations(int val) = 0; + /** @see setPolyN */ + virtual int getPolyN() const = 0; + /** @copybrief getPolyN @see getPolyN */ + virtual void setPolyN(int val) = 0; + /** @see setPolySigma */ + virtual double getPolySigma() const = 0; + /** @copybrief getPolySigma @see getPolySigma */ + virtual void setPolySigma(double val) = 0; + /** @see setFlags */ + virtual int getFlags() const = 0; + /** @copybrief getFlags @see getFlags */ + virtual void setFlags(int val) = 0; }; CV_EXPORTS Ptr createOptFlow_Farneback(); CV_EXPORTS Ptr createOptFlow_Farneback_CUDA(); @@ -82,14 +103,38 @@ namespace cv class CV_EXPORTS DualTVL1OpticalFlow : public virtual DenseOpticalFlowExt { public: - CV_PURE_PROPERTY(double, Tau) - CV_PURE_PROPERTY(double, Lambda) - CV_PURE_PROPERTY(double, Theta) - CV_PURE_PROPERTY(int, ScalesNumber) - CV_PURE_PROPERTY(int, WarpingsNumber) - CV_PURE_PROPERTY(double, Epsilon) - CV_PURE_PROPERTY(int, Iterations) - CV_PURE_PROPERTY(bool, UseInitialFlow) + /** @see setTau */ + virtual double getTau() const = 0; + /** @copybrief getTau @see getTau */ + virtual void setTau(double val) = 0; + /** @see setLambda */ + virtual double getLambda() const = 0; + /** @copybrief getLambda @see getLambda */ + virtual void setLambda(double val) = 0; + /** @see setTheta */ + virtual double getTheta() const = 0; + /** @copybrief getTheta @see getTheta */ + virtual void setTheta(double val) = 0; + /** @see setScalesNumber */ + virtual int getScalesNumber() const = 0; + /** @copybrief getScalesNumber @see getScalesNumber */ + virtual void setScalesNumber(int val) = 0; + /** @see setWarpingsNumber */ + virtual int getWarpingsNumber() const = 0; + /** @copybrief getWarpingsNumber @see getWarpingsNumber */ + virtual void setWarpingsNumber(int val) = 0; + /** @see setEpsilon */ + virtual double getEpsilon() const = 0; + /** @copybrief getEpsilon @see getEpsilon */ + virtual void setEpsilon(double val) = 0; + /** @see setIterations */ + virtual int getIterations() const = 0; + /** @copybrief getIterations @see getIterations */ + virtual void setIterations(int val) = 0; + /** @see setUseInitialFlow */ + virtual bool getUseInitialFlow() const = 0; + /** @copybrief getUseInitialFlow @see getUseInitialFlow */ + virtual void setUseInitialFlow(bool val) = 0; }; CV_EXPORTS Ptr createOptFlow_DualTVL1(); CV_EXPORTS Ptr createOptFlow_DualTVL1_CUDA(); @@ -99,17 +144,35 @@ namespace cv { public: //! @brief Flow smoothness - CV_PURE_PROPERTY(double, Alpha) + /** @see setAlpha */ + virtual double getAlpha() const = 0; + /** @copybrief getAlpha @see getAlpha */ + virtual void setAlpha(double val) = 0; //! @brief Gradient constancy importance - CV_PURE_PROPERTY(double, Gamma) + /** @see setGamma */ + virtual double getGamma() const = 0; + /** @copybrief getGamma @see getGamma */ + virtual void setGamma(double val) = 0; //! @brief Pyramid scale factor - CV_PURE_PROPERTY(double, ScaleFactor) + /** @see setScaleFactor */ + virtual double getScaleFactor() const = 0; + /** @copybrief getScaleFactor @see getScaleFactor */ + virtual void setScaleFactor(double val) = 0; //! @brief Number of lagged non-linearity iterations (inner loop) - CV_PURE_PROPERTY(int, InnerIterations) + /** @see setInnerIterations */ + virtual int getInnerIterations() const = 0; + /** @copybrief getInnerIterations @see getInnerIterations */ + virtual void setInnerIterations(int val) = 0; //! @brief Number of warping iterations (number of pyramid levels) - CV_PURE_PROPERTY(int, OuterIterations) + /** @see setOuterIterations */ + virtual int getOuterIterations() const = 0; + /** @copybrief getOuterIterations @see getOuterIterations */ + virtual void setOuterIterations(int val) = 0; //! @brief Number of linear system solver iterations - CV_PURE_PROPERTY(int, SolverIterations) + /** @see setSolverIterations */ + virtual int getSolverIterations() const = 0; + /** @copybrief getSolverIterations @see getSolverIterations */ + virtual void setSolverIterations(int val) = 0; }; CV_EXPORTS Ptr createOptFlow_Brox_CUDA(); @@ -117,9 +180,18 @@ namespace cv class PyrLKOpticalFlow : public virtual DenseOpticalFlowExt { public: - CV_PURE_PROPERTY(int, WindowSize) - CV_PURE_PROPERTY(int, MaxLevel) - CV_PURE_PROPERTY(int, Iterations) + /** @see setWindowSize */ + virtual int getWindowSize() const = 0; + /** @copybrief getWindowSize @see getWindowSize */ + virtual void setWindowSize(int val) = 0; + /** @see setMaxLevel */ + virtual int getMaxLevel() const = 0; + /** @copybrief getMaxLevel @see getMaxLevel */ + virtual void setMaxLevel(int val) = 0; + /** @see setIterations */ + virtual int getIterations() const = 0; + /** @copybrief getIterations @see getIterations */ + virtual void setIterations(int val) = 0; }; CV_EXPORTS Ptr createOptFlow_PyrLK_CUDA(); diff --git a/modules/video/include/opencv2/video/tracking.hpp b/modules/video/include/opencv2/video/tracking.hpp index 90be72ea2..718d76523 100644 --- a/modules/video/include/opencv2/video/tracking.hpp +++ b/modules/video/include/opencv2/video/tracking.hpp @@ -441,29 +441,65 @@ class CV_EXPORTS_W DualTVL1OpticalFlow : public DenseOpticalFlow { public: //! @brief Time step of the numerical scheme - CV_PURE_PROPERTY(double, Tau) + /** @see setTau */ + virtual double getTau() const = 0; + /** @copybrief getTau @see getTau */ + virtual void setTau(double val) = 0; //! @brief Weight parameter for the data term, attachment parameter - CV_PURE_PROPERTY(double, Lambda) + /** @see setLambda */ + virtual double getLambda() const = 0; + /** @copybrief getLambda @see getLambda */ + virtual void setLambda(double val) = 0; //! @brief Weight parameter for (u - v)^2, tightness parameter - CV_PURE_PROPERTY(double, Theta) + /** @see setTheta */ + virtual double getTheta() const = 0; + /** @copybrief getTheta @see getTheta */ + virtual void setTheta(double val) = 0; //! @brief coefficient for additional illumination variation term - CV_PURE_PROPERTY(double, Gamma) + /** @see setGamma */ + virtual double getGamma() const = 0; + /** @copybrief getGamma @see getGamma */ + virtual void setGamma(double val) = 0; //! @brief Number of scales used to create the pyramid of images - CV_PURE_PROPERTY(int, ScalesNumber) + /** @see setScalesNumber */ + virtual int getScalesNumber() const = 0; + /** @copybrief getScalesNumber @see getScalesNumber */ + virtual void setScalesNumber(int val) = 0; //! @brief Number of warpings per scale - CV_PURE_PROPERTY(int, WarpingsNumber) + /** @see setWarpingsNumber */ + virtual int getWarpingsNumber() const = 0; + /** @copybrief getWarpingsNumber @see getWarpingsNumber */ + virtual void setWarpingsNumber(int val) = 0; //! @brief Stopping criterion threshold used in the numerical scheme, which is a trade-off between precision and running time - CV_PURE_PROPERTY(double, Epsilon) + /** @see setEpsilon */ + virtual double getEpsilon() const = 0; + /** @copybrief getEpsilon @see getEpsilon */ + virtual void setEpsilon(double val) = 0; //! @brief Inner iterations (between outlier filtering) used in the numerical scheme - CV_PURE_PROPERTY(int, InnerIterations) + /** @see setInnerIterations */ + virtual int getInnerIterations() const = 0; + /** @copybrief getInnerIterations @see getInnerIterations */ + virtual void setInnerIterations(int val) = 0; //! @brief Outer iterations (number of inner loops) used in the numerical scheme - CV_PURE_PROPERTY(int, OuterIterations) + /** @see setOuterIterations */ + virtual int getOuterIterations() const = 0; + /** @copybrief getOuterIterations @see getOuterIterations */ + virtual void setOuterIterations(int val) = 0; //! @brief Use initial flow - CV_PURE_PROPERTY(bool, UseInitialFlow) + /** @see setUseInitialFlow */ + virtual bool getUseInitialFlow() const = 0; + /** @copybrief getUseInitialFlow @see getUseInitialFlow */ + virtual void setUseInitialFlow(bool val) = 0; //! @brief Step between scales (<1) - CV_PURE_PROPERTY(double, ScaleStep) + /** @see setScaleStep */ + virtual double getScaleStep() const = 0; + /** @copybrief getScaleStep @see getScaleStep */ + virtual void setScaleStep(double val) = 0; //! @brief Median filter kernel size (1 = no filter) (3 or 5) - CV_PURE_PROPERTY(int, MedianFiltering) + /** @see setMedianFiltering */ + virtual int getMedianFiltering() const = 0; + /** @copybrief getMedianFiltering @see getMedianFiltering */ + virtual void setMedianFiltering(int val) = 0; }; /** @brief Creates instance of cv::DenseOpticalFlow From d5b954c2d4c3bf9c513ea33687f192a7ebb6ea14 Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Wed, 18 Mar 2015 17:33:12 +0300 Subject: [PATCH 102/171] Moved property implementation macros to private header --- modules/core/include/opencv2/core.hpp | 22 ------------------- modules/core/include/opencv2/core/private.hpp | 21 ++++++++++++++++++ 2 files changed, 21 insertions(+), 22 deletions(-) diff --git a/modules/core/include/opencv2/core.hpp b/modules/core/include/opencv2/core.hpp index ae6de3a97..5990213dc 100644 --- a/modules/core/include/opencv2/core.hpp +++ b/modules/core/include/opencv2/core.hpp @@ -2821,28 +2821,6 @@ public: virtual void read(const FileNode& fn) { (void)fn; } }; -// basic property implementation - -#define CV_IMPL_PROPERTY_RO(type, name, member) \ - inline type get##name() const { return member; } - -#define CV_HELP_IMPL_PROPERTY(r_type, w_type, name, member) \ - CV_IMPL_PROPERTY_RO(r_type, name, member) \ - inline void set##name(w_type val) { member = val; } - -#define CV_HELP_WRAP_PROPERTY(r_type, w_type, name, internal_name, internal_obj) \ - r_type get##name() const { return internal_obj.get##internal_name(); } \ - void set##name(w_type val) { internal_obj.set##internal_name(val); } - -#define CV_IMPL_PROPERTY(type, name, member) CV_HELP_IMPL_PROPERTY(type, type, name, member) -#define CV_IMPL_PROPERTY_S(type, name, member) CV_HELP_IMPL_PROPERTY(type, const type &, name, member) - -#define CV_WRAP_PROPERTY(type, name, internal_name, internal_obj) CV_HELP_WRAP_PROPERTY(type, type, name, internal_name, internal_obj) -#define CV_WRAP_PROPERTY_S(type, name, internal_name, internal_obj) CV_HELP_WRAP_PROPERTY(type, const type &, name, internal_name, internal_obj) - -#define CV_WRAP_SAME_PROPERTY(type, name, internal_obj) CV_WRAP_PROPERTY(type, name, name, internal_obj) -#define CV_WRAP_SAME_PROPERTY_S(type, name, internal_obj) CV_WRAP_PROPERTY_S(type, name, name, internal_obj) - struct Param { enum { INT=0, BOOLEAN=1, REAL=2, STRING=3, MAT=4, MAT_VECTOR=5, ALGORITHM=6, FLOAT=7, UNSIGNED_INT=8, UINT64=9, UCHAR=11 }; diff --git a/modules/core/include/opencv2/core/private.hpp b/modules/core/include/opencv2/core/private.hpp index 03d69bd43..58d78e584 100644 --- a/modules/core/include/opencv2/core/private.hpp +++ b/modules/core/include/opencv2/core/private.hpp @@ -172,6 +172,27 @@ namespace cv CV_EXPORTS void scalarToRawData(const cv::Scalar& s, void* buf, int type, int unroll_to = 0); } +// property implementation macros + +#define CV_IMPL_PROPERTY_RO(type, name, member) \ + inline type get##name() const { return member; } + +#define CV_HELP_IMPL_PROPERTY(r_type, w_type, name, member) \ + CV_IMPL_PROPERTY_RO(r_type, name, member) \ + inline void set##name(w_type val) { member = val; } + +#define CV_HELP_WRAP_PROPERTY(r_type, w_type, name, internal_name, internal_obj) \ + r_type get##name() const { return internal_obj.get##internal_name(); } \ + void set##name(w_type val) { internal_obj.set##internal_name(val); } + +#define CV_IMPL_PROPERTY(type, name, member) CV_HELP_IMPL_PROPERTY(type, type, name, member) +#define CV_IMPL_PROPERTY_S(type, name, member) CV_HELP_IMPL_PROPERTY(type, const type &, name, member) + +#define CV_WRAP_PROPERTY(type, name, internal_name, internal_obj) CV_HELP_WRAP_PROPERTY(type, type, name, internal_name, internal_obj) +#define CV_WRAP_PROPERTY_S(type, name, internal_name, internal_obj) CV_HELP_WRAP_PROPERTY(type, const type &, name, internal_name, internal_obj) + +#define CV_WRAP_SAME_PROPERTY(type, name, internal_obj) CV_WRAP_PROPERTY(type, name, name, internal_obj) +#define CV_WRAP_SAME_PROPERTY_S(type, name, internal_obj) CV_WRAP_PROPERTY_S(type, name, name, internal_obj) /****************************************************************************************\ * Structures and macros for integration with IPP * From 349e3b546a1a13eefa9bed52c6a6222b4deb92a7 Mon Sep 17 00:00:00 2001 From: Maxim Kostin Date: Wed, 18 Mar 2015 17:37:42 +0300 Subject: [PATCH 103/171] Fixed 'doc' builder warnings. --- .../OcvTransform/OcvTransform.cpp | 2 +- .../JavaScript/MediaCaptureJavaScript.jsproj | 2 +- samples/winrt/JavaScript/css/default.css | 2 +- samples/winrt/JavaScript/default.html | 8 +-- .../JavaScript/html/AdvancedCapture.html | 14 ++-- .../winrt/JavaScript/js/AdvancedCapture.js | 66 +++++++++---------- samples/winrt/JavaScript/js/default.js | 2 +- .../JavaScript/sample-utils/sample-utils.css | 6 +- 8 files changed, 50 insertions(+), 52 deletions(-) diff --git a/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp b/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp index 40335e2cd..56193c1cb 100644 --- a/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp +++ b/samples/winrt/ImageManipulations/MediaExtensions/OcvTransform/OcvTransform.cpp @@ -126,7 +126,7 @@ HRESULT OcvImageManipulations::SetProperties(ABI::Windows::Foundation::Collectio spSetting->Lookup(key, spInsp.ReleaseAndGetAddressOf()); hr = spInsp.As(&spPropVal); - if (hr != S_OK) + if (hr != S_OK) { return hr; } diff --git a/samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj b/samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj index 56e6936ac..8bf1b03d9 100644 --- a/samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj +++ b/samples/winrt/JavaScript/MediaCaptureJavaScript.jsproj @@ -102,7 +102,7 @@ From 75a65542ba1c605a1feaf69b00e5abdc6452f2e7 Mon Sep 17 00:00:00 2001 From: Dmitry-Me Date: Wed, 25 Mar 2015 17:17:06 +0300 Subject: [PATCH 137/171] Reduce variables scope --- modules/core/src/pca.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/modules/core/src/pca.cpp b/modules/core/src/pca.cpp index 95efd5718..85ba44324 100644 --- a/modules/core/src/pca.cpp +++ b/modules/core/src/pca.cpp @@ -66,7 +66,7 @@ PCA& PCA::operator()(InputArray _data, InputArray __mean, int flags, int maxComp { Mat data = _data.getMat(), _mean = __mean.getMat(); int covar_flags = CV_COVAR_SCALE; - int i, len, in_count; + int len, in_count; Size mean_sz; CV_Assert( data.channels() == 1 ); @@ -131,6 +131,7 @@ PCA& PCA::operator()(InputArray _data, InputArray __mean, int flags, int maxComp eigenvectors = evects1; // normalize eigenvectors + int i; for( i = 0; i < out_count; i++ ) { Mat vec = eigenvectors.row(i); @@ -202,7 +203,7 @@ PCA& PCA::operator()(InputArray _data, InputArray __mean, int flags, double reta { Mat data = _data.getMat(), _mean = __mean.getMat(); int covar_flags = CV_COVAR_SCALE; - int i, len, in_count; + int len, in_count; Size mean_sz; CV_Assert( data.channels() == 1 ); @@ -266,6 +267,7 @@ PCA& PCA::operator()(InputArray _data, InputArray __mean, int flags, double reta eigenvectors = evects1; // normalize all eigenvectors + int i; for( i = 0; i < eigenvectors.rows; i++ ) { Mat vec = eigenvectors.row(i); From 32da602bf7f78eb363401174db9cc377c98ae6d8 Mon Sep 17 00:00:00 2001 From: Alexander Alekhin Date: Wed, 25 Mar 2015 19:09:53 +0300 Subject: [PATCH 138/171] fix build (related to PR #3814) --- .../src/fast_nlmeans_denoising_invoker_commons.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index 8f31e8b02..2322919ef 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -44,6 +44,14 @@ using namespace cv; +// std::isnan is a part of C++11 and it is not supported in MSVS2010/2012 +#if defined _MSC_VER && _MSC_VER < 1800 /* MSVC 2013 */ +#include +namespace std { +template bool isnan(T value) { return _isnan(value) != 0; } +} +#endif + template struct pixelInfo_ { static const int channels = 1; @@ -130,7 +138,7 @@ class DistAbs if (std::isnan(w)) w = 1.0; // Handle h = 0.0 static const double WEIGHT_THRESHOLD = 0.001; - WT weight = (WT)round(fixed_point_mult * w); + WT weight = (WT)cvRound(fixed_point_mult * w); if (weight < WEIGHT_THRESHOLD * fixed_point_mult) weight = 0; return weight; @@ -252,7 +260,7 @@ class DistSquared if (std::isnan(w)) w = 1.0; // Handle h = 0.0 static const double WEIGHT_THRESHOLD = 0.001; - WT weight = (WT)round(fixed_point_mult * w); + WT weight = (WT)cvRound(fixed_point_mult * w); if (weight < WEIGHT_THRESHOLD * fixed_point_mult) weight = 0; return weight; From 3df6b6fdcff0b240d94e88fadb7666d2a616d77d Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Thu, 26 Mar 2015 00:39:29 +0300 Subject: [PATCH 139/171] added self-contained motion jpeg encoder (filename should end with .avi; fourcc should be "MJPG" --- modules/videoio/CMakeLists.txt | 2 + modules/videoio/include/opencv2/videoio.hpp | 8 +- modules/videoio/src/cap.cpp | 132 +- modules/videoio/src/cap_dshow.hpp | 2 +- modules/videoio/src/cap_intelperc.hpp | 2 +- modules/videoio/src/cap_mjpeg_decoder.cpp | 52 + modules/videoio/src/cap_mjpeg_encoder.cpp | 1443 +++++++++++++++++++ modules/videoio/src/precomp.hpp | 17 +- 8 files changed, 1596 insertions(+), 62 deletions(-) create mode 100644 modules/videoio/src/cap_mjpeg_decoder.cpp create mode 100644 modules/videoio/src/cap_mjpeg_encoder.cpp diff --git a/modules/videoio/CMakeLists.txt b/modules/videoio/CMakeLists.txt index 0b4f391c5..9332f2860 100644 --- a/modules/videoio/CMakeLists.txt +++ b/modules/videoio/CMakeLists.txt @@ -27,6 +27,8 @@ set(videoio_hdrs set(videoio_srcs ${CMAKE_CURRENT_LIST_DIR}/src/cap.cpp ${CMAKE_CURRENT_LIST_DIR}/src/cap_images.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/cap_mjpeg_encoder.cpp + ${CMAKE_CURRENT_LIST_DIR}/src/cap_mjpeg_decoder.cpp ) file(GLOB videoio_ext_hdrs diff --git a/modules/videoio/include/opencv2/videoio.hpp b/modules/videoio/include/opencv2/videoio.hpp index b3e2a28cf..85021163a 100644 --- a/modules/videoio/include/opencv2/videoio.hpp +++ b/modules/videoio/include/opencv2/videoio.hpp @@ -586,10 +586,10 @@ public: protected: Ptr cap; Ptr icap; -private: - static Ptr createCameraCapture(int index); }; +class IVideoWriter; + /** @brief Video writer class. */ class CV_EXPORTS_W VideoWriter @@ -651,6 +651,10 @@ public: protected: Ptr writer; + Ptr iwriter; + + static Ptr create(const String& filename, int fourcc, double fps, + Size frameSize, bool isColor = true); }; template<> CV_EXPORTS void DefaultDeleter::operator ()(CvCapture* obj) const; diff --git a/modules/videoio/src/cap.cpp b/modules/videoio/src/cap.cpp index 09fa1c081..28470c6a5 100644 --- a/modules/videoio/src/cap.cpp +++ b/modules/videoio/src/cap.cpp @@ -499,6 +499,67 @@ CV_IMPL void cvReleaseVideoWriter( CvVideoWriter** pwriter ) namespace cv { +static Ptr IVideoCapture_create(int index) +{ + int domains[] = + { +#ifdef HAVE_DSHOW + CV_CAP_DSHOW, +#endif +#ifdef HAVE_INTELPERC + CV_CAP_INTELPERC, +#endif + -1, -1 + }; + + // interpret preferred interface (0 = autodetect) + int pref = (index / 100) * 100; + if (pref) + { + domains[0]=pref; + index %= 100; + domains[1]=-1; + } + + // try every possibly installed camera API + for (int i = 0; domains[i] >= 0; i++) + { +#if defined(HAVE_DSHOW) || \ + defined(HAVE_INTELPERC) || \ + (0) + Ptr capture; + + switch (domains[i]) + { +#ifdef HAVE_DSHOW + case CV_CAP_DSHOW: + capture = makePtr(index); + break; // CV_CAP_DSHOW +#endif +#ifdef HAVE_INTELPERC + case CV_CAP_INTELPERC: + capture = makePtr(); + break; // CV_CAP_INTEL_PERC +#endif + } + if (capture && capture->isOpened()) + return capture; +#endif + } + + // failed open a camera + return Ptr(); +} + + +static Ptr IVideoWriter_create(const String& filename, int _fourcc, double fps, Size frameSize, bool isColor) +{ + Ptr iwriter; + if( _fourcc == CV_FOURCC('M', 'J', 'P', 'G') ) + iwriter = createMotionJpegWriter(filename, fps, frameSize, isColor); + return iwriter; +} + VideoCapture::VideoCapture() {} @@ -528,7 +589,7 @@ bool VideoCapture::open(const String& filename) bool VideoCapture::open(int device) { if (isOpened()) release(); - icap = createCameraCapture(device); + icap = IVideoCapture_create(device); if (!icap.empty()) return true; cap.reset(cvCreateCameraCapture(device)); @@ -609,59 +670,6 @@ double VideoCapture::get(int propId) const return icvGetCaptureProperty(cap, propId); } -Ptr VideoCapture::createCameraCapture(int index) -{ - int domains[] = - { -#ifdef HAVE_DSHOW - CV_CAP_DSHOW, -#endif -#ifdef HAVE_INTELPERC - CV_CAP_INTELPERC, -#endif - -1, -1 - }; - - // interpret preferred interface (0 = autodetect) - int pref = (index / 100) * 100; - if (pref) - { - domains[0]=pref; - index %= 100; - domains[1]=-1; - } - - // try every possibly installed camera API - for (int i = 0; domains[i] >= 0; i++) - { -#if defined(HAVE_DSHOW) || \ - defined(HAVE_INTELPERC) || \ - (0) - Ptr capture; - - switch (domains[i]) - { -#ifdef HAVE_DSHOW - case CV_CAP_DSHOW: - capture = makePtr(index); - if (capture && capture.dynamicCast()->isOpened()) - return capture; - break; // CV_CAP_DSHOW -#endif -#ifdef HAVE_INTELPERC - case CV_CAP_INTELPERC: - capture = makePtr(); - if (capture && capture.dynamicCast()->isOpened()) - return capture; - break; // CV_CAP_INTEL_PERC -#endif - } -#endif - } - - // failed open a camera - return Ptr(); -} VideoWriter::VideoWriter() {} @@ -673,6 +681,7 @@ VideoWriter::VideoWriter(const String& filename, int _fourcc, double fps, Size f void VideoWriter::release() { + iwriter.release(); writer.release(); } @@ -683,19 +692,28 @@ VideoWriter::~VideoWriter() bool VideoWriter::open(const String& filename, int _fourcc, double fps, Size frameSize, bool isColor) { + if (isOpened()) release(); + iwriter = IVideoWriter_create(filename, _fourcc, fps, frameSize, isColor); + if (!iwriter.empty()) + return true; writer.reset(cvCreateVideoWriter(filename.c_str(), _fourcc, fps, frameSize, isColor)); return isOpened(); } bool VideoWriter::isOpened() const { - return !writer.empty(); + return !iwriter.empty() || !writer.empty(); } void VideoWriter::write(const Mat& image) { - IplImage _img = image; - cvWriteFrame(writer, &_img); + if( iwriter ) + iwriter->write(image); + else + { + IplImage _img = image; + cvWriteFrame(writer, &_img); + } } VideoWriter& VideoWriter::operator << (const Mat& image) diff --git a/modules/videoio/src/cap_dshow.hpp b/modules/videoio/src/cap_dshow.hpp index 9b906c8bf..46998c186 100644 --- a/modules/videoio/src/cap_dshow.hpp +++ b/modules/videoio/src/cap_dshow.hpp @@ -32,7 +32,7 @@ public: virtual bool grabFrame(); virtual bool retrieveFrame(int outputType, OutputArray frame); virtual int getCaptureDomain(); - bool isOpened() const; + virtual bool isOpened() const; protected: void open(int index); void close(); diff --git a/modules/videoio/src/cap_intelperc.hpp b/modules/videoio/src/cap_intelperc.hpp index e154fa320..430a714f0 100644 --- a/modules/videoio/src/cap_intelperc.hpp +++ b/modules/videoio/src/cap_intelperc.hpp @@ -100,7 +100,7 @@ public: virtual bool grabFrame(); virtual bool retrieveFrame(int outputType, OutputArray frame); virtual int getCaptureDomain(); - bool isOpened() const; + virtual bool isOpened() const; protected: bool m_contextOpened; diff --git a/modules/videoio/src/cap_mjpeg_decoder.cpp b/modules/videoio/src/cap_mjpeg_decoder.cpp new file mode 100644 index 000000000..ac5247e6a --- /dev/null +++ b/modules/videoio/src/cap_mjpeg_decoder.cpp @@ -0,0 +1,52 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// 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. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2015, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// 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 Intel Corporation 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. +// +//M*/ + +#include "precomp.hpp" + +namespace cv +{ + +Ptr createMotionJpegCapture(const String& filename) +{ + return Ptr(); +} + +} diff --git a/modules/videoio/src/cap_mjpeg_encoder.cpp b/modules/videoio/src/cap_mjpeg_encoder.cpp new file mode 100644 index 000000000..eafd91388 --- /dev/null +++ b/modules/videoio/src/cap_mjpeg_encoder.cpp @@ -0,0 +1,1443 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// 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. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2015, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// 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 Intel Corporation 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. +// +//M*/ + +#include "precomp.hpp" +#include + +//#define WITH_NEON +#ifdef WITH_NEON +#include "arm_neon.h" +#endif + +namespace cv +{ +namespace mjpeg +{ + +enum { COLORSPACE_GRAY=0, COLORSPACE_RGBA=1, COLORSPACE_BGR=2, COLORSPACE_YUV444P=3 }; + +#define fourCC(a,b,c,d) ((int)((uchar(d)<<24) | (uchar(c)<<16) | (uchar(b)<<8) | uchar(a))) + +static const int AVIH_STRH_SIZE = 56; +static const int STRF_SIZE = 40; +static const int AVI_DWFLAG = 0x00000910; +static const int AVI_DWSCALE = 1; +static const int AVI_DWQUALITY = -1; +static const int JUNK_SEEK = 4096; +static const int AVIIF_KEYFRAME = 0x10; +static const int MAX_BYTES_PER_SEC = 99999999; +static const int SUG_BUFFER_SIZE = 1048576; + +static const unsigned bit_mask[] = +{ + 0, + 0x00000001, 0x00000003, 0x00000007, 0x0000000F, + 0x0000001F, 0x0000003F, 0x0000007F, 0x000000FF, + 0x000001FF, 0x000003FF, 0x000007FF, 0x00000FFF, + 0x00001FFF, 0x00003FFF, 0x00007FFF, 0x0000FFFF, + 0x0001FFFF, 0x0003FFFF, 0x0007FFFF, 0x000FFFFF, + 0x001FFFFF, 0x003FFFFF, 0x007FFFFF, 0x00FFFFFF, + 0x01FFFFFF, 0x03FFFFFF, 0x07FFFFFF, 0x0FFFFFFF, + 0x1FFFFFFF, 0x3FFFFFFF, 0x7FFFFFFF, 0xFFFFFFFF +}; + +class BitStream +{ +public: + enum + { + DEFAULT_BLOCK_SIZE = (1 << 15), + huff_val_shift = 20, + huff_code_mask = (1 << huff_val_shift) - 1 + }; + + BitStream() + { + m_buf.resize(DEFAULT_BLOCK_SIZE + 1024); + m_start = &m_buf[0]; + m_end = m_start + DEFAULT_BLOCK_SIZE; + m_is_opened = false; + m_f = 0; + } + + ~BitStream() + { + close(); + } + + bool open(const String& filename) + { + close(); + m_f = fopen(filename.c_str(), "wb"); + if( !m_f ) + return false; + m_current = m_start; + m_pos = 0; + return true; + } + + bool isOpened() const { return m_f != 0; } + + void close() + { + writeBlock(); + if( m_f ) + fclose(m_f); + m_f = 0; + } + + void writeBlock() + { + size_t wsz0 = m_current - m_start; + if( wsz0 > 0 && m_f ) + { + size_t wsz = fwrite(m_start, 1, wsz0, m_f); + CV_Assert( wsz == wsz0 ); + } + m_pos += wsz0; + m_current = m_start; + } + + size_t getPos() const + { + return (size_t)(m_current - m_start) + m_pos; + } + + void putByte(int val) + { + *m_current++ = (uchar)val; + if( m_current >= m_end ) + writeBlock(); + } + + void putBytes(const uchar* buf, int count) + { + uchar* data = (uchar*)buf; + CV_Assert(m_f && data && m_current && count >= 0); + if( m_current >= m_end ) + writeBlock(); + + while( count ) + { + int l = (int)(m_end - m_current); + + if (l > count) + l = count; + + if( l > 0 ) + { + memcpy(m_current, data, l); + m_current += l; + data += l; + count -= l; + } + if( m_current >= m_end ) + writeBlock(); + } + } + + void putShort(int val) + { + m_current[0] = (uchar)val; + m_current[1] = (uchar)(val >> 8); + m_current += 2; + if( m_current >= m_end ) + writeBlock(); + } + + void putInt(int val) + { + m_current[0] = (uchar)val; + m_current[1] = (uchar)(val >> 8); + m_current[2] = (uchar)(val >> 16); + m_current[3] = (uchar)(val >> 24); + m_current += 4; + if( m_current >= m_end ) + writeBlock(); + } + + void jputShort(int val) + { + m_current[0] = (uchar)(val >> 8); + m_current[1] = (uchar)val; + m_current += 2; + if( m_current >= m_end ) + writeBlock(); + } + + void patchInt(int val, size_t pos) + { + if( pos >= m_pos ) + { + ptrdiff_t delta = pos - m_pos; + CV_Assert( delta < m_current - m_start ); + m_start[delta] = (uchar)val; + m_start[delta+1] = (uchar)(val >> 8); + m_start[delta+2] = (uchar)(val >> 16); + m_start[delta+3] = (uchar)(val >> 24); + } + else + { + size_t fpos = ftell(m_f); + fseek(m_f, pos, SEEK_SET); + uchar buf[] = { (uchar)val, (uchar)(val >> 8), (uchar)(val >> 16), (uchar)(val >> 24) }; + fwrite(buf, 1, 4, m_f); + fseek(m_f, fpos, SEEK_SET); + } + } + + void jput(unsigned currval) + { + uchar v; + uchar* ptr = m_current; + v = (uchar)(currval >> 24); + *ptr++ = v; + if( v == 255 ) + *ptr++ = 0; + v = (uchar)(currval >> 16); + *ptr++ = v; + if( v == 255 ) + *ptr++ = 0; + v = (uchar)(currval >> 8); + *ptr++ = v; + if( v == 255 ) + *ptr++ = 0; + v = (uchar)currval; + *ptr++ = v; + if( v == 255 ) + *ptr++ = 0; + m_current = ptr; + if( m_current >= m_end ) + writeBlock(); + } + + static bool createEncodeHuffmanTable( const int* src, unsigned* table, int max_size ) + { + int i, k; + int min_val = INT_MAX, max_val = INT_MIN; + int size; + + /* calc min and max values in the table */ + for( i = 1, k = 1; src[k] >= 0; i++ ) + { + int code_count = src[k++]; + + for( code_count += k; k < code_count; k++ ) + { + int val = src[k] >> huff_val_shift; + if( val < min_val ) + min_val = val; + if( val > max_val ) + max_val = val; + } + } + + size = max_val - min_val + 3; + + if( size > max_size ) + { + CV_Error(CV_StsOutOfRange, "too big maximum Huffman code size"); + return false; + } + + memset( table, 0, size*sizeof(table[0])); + + table[0] = min_val; + table[1] = size - 2; + + for( i = 1, k = 1; src[k] >= 0; i++ ) + { + int code_count = src[k++]; + + for( code_count += k; k < code_count; k++ ) + { + int val = src[k] >> huff_val_shift; + int code = src[k] & huff_code_mask; + + table[val - min_val + 2] = (code << 8) | i; + } + } + return true; + } + + static int* createSourceHuffmanTable(const uchar* src, int* dst, + int max_bits, int first_bits) + { + int i, val_idx, code = 0; + int* table = dst; + *dst++ = first_bits; + for (i = 1, val_idx = max_bits; i <= max_bits; i++) + { + int code_count = src[i - 1]; + dst[0] = code_count; + code <<= 1; + for (int k = 0; k < code_count; k++) + { + dst[k + 1] = (src[val_idx + k] << huff_val_shift) | (code + k); + } + code += code_count; + dst += code_count + 1; + val_idx += code_count; + } + dst[0] = -1; + return table; + } + +protected: + std::vector m_buf; + uchar* m_start; + uchar* m_end; + uchar* m_current; + size_t m_pos; + bool m_is_opened; + FILE* m_f; +}; + + +class MotionJpegWriter : public IVideoWriter +{ +public: + MotionJpegWriter() { rawstream = false; } + MotionJpegWriter(const String& filename, double fps, Size size, bool iscolor) + { + rawstream = false; + open(filename, fps, size, iscolor); + } + ~MotionJpegWriter() { close(); } + + void close() + { + if( !strm.isOpened() ) + return; + + if( !frameOffset.empty() && !rawstream ) + { + endWriteChunk(); // end LIST 'movi' + writeIndex(); + finishWriteAVI(); + } + strm.close(); + frameOffset.clear(); + frameSize.clear(); + AVIChunkSizeIndex.clear(); + frameNumIndexes.clear(); + } + + bool open(const String& filename, double fps, Size size, bool iscolor) + { + close(); + + if( filename.empty() ) + return false; + const char* ext = strrchr(filename.c_str(), '.'); + if( !ext ) + return false; + if( strcmp(ext, ".avi") != 0 && strcmp(ext, ".AVI") != 0 && strcmp(ext, ".Avi") != 0 ) + return false; + + bool ok = strm.open(filename); + if( !ok ) + return false; + + CV_Assert(fps >= 1); + outfps = cvRound(fps); + width = size.width; + height = size.height; + quality = 8; + rawstream = false; + channels = iscolor ? 3 : 1; + + if( !rawstream ) + { + startWriteAVI(); + writeStreamHeader(); + } + return true; + } + + bool isOpened() const { return strm.isOpened(); } + + void startWriteAVI() + { + startWriteChunk(fourCC('R', 'I', 'F', 'F')); + + strm.putInt(fourCC('A', 'V', 'I', ' ')); + + startWriteChunk(fourCC('L', 'I', 'S', 'T')); + + strm.putInt(fourCC('h', 'd', 'r', 'l')); + strm.putInt(fourCC('a', 'v', 'i', 'h')); + strm.putInt(AVIH_STRH_SIZE); + strm.putInt(cvRound(1e6 / outfps)); + strm.putInt(MAX_BYTES_PER_SEC); + strm.putInt(0); + strm.putInt(AVI_DWFLAG); + + frameNumIndexes.push_back(strm.getPos()); + + strm.putInt(0); + strm.putInt(0); + strm.putInt(1); // number of streams + strm.putInt(SUG_BUFFER_SIZE); + strm.putInt(width); + strm.putInt(height); + strm.putInt(0); + strm.putInt(0); + strm.putInt(0); + strm.putInt(0); + } + + void writeStreamHeader() + { + // strh + startWriteChunk(fourCC('L', 'I', 'S', 'T')); + + strm.putInt(fourCC('s', 't', 'r', 'l')); + strm.putInt(fourCC('s', 't', 'r', 'h')); + strm.putInt(AVIH_STRH_SIZE); + strm.putInt(fourCC('v', 'i', 'd', 's')); + strm.putInt(fourCC('M', 'J', 'P', 'G')); + strm.putInt(0); + strm.putInt(0); + strm.putInt(0); + strm.putInt(AVI_DWSCALE); + strm.putInt(outfps); + strm.putInt(0); + + frameNumIndexes.push_back(strm.getPos()); + + strm.putInt(0); + strm.putInt(SUG_BUFFER_SIZE); + strm.putInt(AVI_DWQUALITY); + strm.putInt(0); + strm.putShort(0); + strm.putShort(0); + strm.putShort(width); + strm.putShort(height); + + // strf (use the BITMAPINFOHEADER for video) + startWriteChunk(fourCC('s', 't', 'r', 'f')); + + strm.putInt(STRF_SIZE); + strm.putInt(width); + strm.putInt(height); + strm.putShort(1); // planes (1 means interleaved data (after decompression)) + + strm.putShort(channels); // bits per pixel + strm.putInt(fourCC('M', 'J', 'P', 'G')); + strm.putInt(width * height * channels); + strm.putInt(0); + strm.putInt(0); + strm.putInt(0); + strm.putInt(0); + // Must be indx chunk + endWriteChunk(); // end strf + endWriteChunk(); // end strl + + // odml + startWriteChunk(fourCC('L', 'I', 'S', 'T')); + strm.putInt(fourCC('o', 'd', 'm', 'l')); + startWriteChunk(fourCC('d', 'm', 'l', 'h')); + + frameNumIndexes.push_back(strm.getPos()); + + strm.putInt(0); + strm.putInt(0); + + endWriteChunk(); // end dmlh + endWriteChunk(); // end odml + + endWriteChunk(); // end hdrl + + // JUNK + startWriteChunk(fourCC('J', 'U', 'N', 'K')); + size_t pos = strm.getPos(); + for( ; pos < (size_t)JUNK_SEEK; pos += 4 ) + strm.putInt(0); + endWriteChunk(); // end JUNK + // movi + startWriteChunk(fourCC('L', 'I', 'S', 'T')); + moviPointer = strm.getPos(); + strm.putInt(fourCC('m', 'o', 'v', 'i')); + } + + void startWriteChunk(int fourcc) + { + CV_Assert(fourcc != 0); + strm.putInt(fourcc); + + AVIChunkSizeIndex.push_back(strm.getPos()); + strm.putInt(0); + } + + void endWriteChunk() + { + if( !AVIChunkSizeIndex.empty() ) + { + size_t currpos = strm.getPos(); + size_t pospos = AVIChunkSizeIndex.back(); + AVIChunkSizeIndex.pop_back(); + int chunksz = (int)(currpos - (pospos + 4)); + strm.patchInt(chunksz, pospos); + } + } + + void writeIndex() + { + // old style AVI index. Must be Open-DML index + startWriteChunk(fourCC('i', 'd', 'x', '1')); + int nframes = (int)frameOffset.size(); + for( int i = 0; i < nframes; i++ ) + { + strm.putInt(fourCC('0', '0', 'd', 'c')); + strm.putInt(AVIIF_KEYFRAME); + strm.putInt((int)frameOffset[i]); + strm.putInt((int)frameSize[i]); + } + endWriteChunk(); // End idx1 + } + + void finishWriteAVI() + { + int nframes = (int)frameOffset.size(); + // Record frames numbers to AVI Header + while (!frameNumIndexes.empty()) + { + size_t ppos = frameNumIndexes.back(); + frameNumIndexes.pop_back(); + strm.patchInt(nframes, ppos); + } + endWriteChunk(); // end RIFF + } + + void write(InputArray _img) + { + Mat img = _img.getMat(); + size_t chunkPointer = strm.getPos(); + int input_channels = img.channels(); + int colorspace = -1; + + if( input_channels == 1 && channels == 1 ) + { + CV_Assert( img.cols == width && img.rows == height ); + colorspace = COLORSPACE_GRAY; + } + else if( input_channels == 4 ) + { + CV_Assert( img.cols == width && img.rows == height && channels == 3 ); + colorspace = COLORSPACE_RGBA; + } + else if( input_channels == 3 ) + { + CV_Assert( img.cols == width && img.rows == height && channels == 3 ); + colorspace = COLORSPACE_BGR; + } + else if( input_channels == 1 && channels == 3 ) + { + CV_Assert( img.cols == width && img.rows == height*3 ); + colorspace = COLORSPACE_YUV444P; + } + else + CV_Error(CV_StsBadArg, "Invalid combination of specified video colorspace and the input image colorspace"); + + if( !rawstream ) + startWriteChunk(fourCC('0', '0', 'd', 'c')); + + writeFrameData(img.data, (int)img.step, colorspace, input_channels); + + if( !rawstream ) + { + frameOffset.push_back(chunkPointer - moviPointer); + frameSize.push_back(strm.getPos() - chunkPointer - 8); // Size excludes '00dc' and size field + endWriteChunk(); // end '00dc' + } + } + + void writeFrameData( const uchar* data, int step, int colorspace, int input_channels ); + +protected: + int outfps; + int width, height, channels; + int quality; + size_t moviPointer; + std::vector frameOffset, frameSize, AVIChunkSizeIndex, frameNumIndexes; + bool rawstream; + + BitStream strm; +}; + +#define DCT_DESCALE(x, n) (((x) + (((int)1) << ((n) - 1))) >> (n)) +#define fix(x, n) (int)((x)*(1 << (n)) + .5); + +enum +{ + fixb = 14, + fixc = 12, + postshift = 14 +}; + +static const int C0_707 = fix(0.707106781f, fixb); +static const int C0_541 = fix(0.541196100f, fixb); +static const int C0_382 = fix(0.382683432f, fixb); +static const int C1_306 = fix(1.306562965f, fixb); + +static const int y_r = fix(0.299, fixc); +static const int y_g = fix(0.587, fixc); +static const int y_b = fix(0.114, fixc); + +static const int cb_r = -fix(0.1687, fixc); +static const int cb_g = -fix(0.3313, fixc); +static const int cb_b = fix(0.5, fixc); + +static const int cr_r = fix(0.5, fixc); +static const int cr_g = -fix(0.4187, fixc); +static const int cr_b = -fix(0.0813, fixc); + +// Standard JPEG quantization tables +static const uchar jpegTableK1_T[] = +{ + 16, 12, 14, 14, 18, 24, 49, 72, + 11, 12, 13, 17, 22, 35, 64, 92, + 10, 14, 16, 22, 37, 55, 78, 95, + 16, 19, 24, 29, 56, 64, 87, 98, + 24, 26, 40, 51, 68, 81, 103, 112, + 40, 58, 57, 87, 109, 104, 121, 100, + 51, 60, 69, 80, 103, 113, 120, 103, + 61, 55, 56, 62, 77, 92, 101, 99 +}; + +static const uchar jpegTableK2_T[] = +{ + 17, 18, 24, 47, 99, 99, 99, 99, + 18, 21, 26, 66, 99, 99, 99, 99, + 24, 26, 56, 99, 99, 99, 99, 99, + 47, 66, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99 +}; + +// Standard Huffman tables + +// ... for luma DCs. +static const uchar jpegTableK3[] = +{ + 0, 1, 5, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 +}; + +// ... for chroma DCs. +static const uchar jpegTableK4[] = +{ + 0, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 +}; + +// ... for luma ACs. +static const uchar jpegTableK5[] = +{ + 0, 2, 1, 3, 3, 2, 4, 3, 5, 5, 4, 4, 0, 0, 1, 125, + 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, + 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, + 0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08, + 0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0, + 0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16, + 0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28, + 0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, + 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, + 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, + 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, + 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, + 0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, + 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, + 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, + 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, + 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5, + 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, + 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2, + 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, + 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, + 0xf9, 0xfa +}; + +// ... for chroma ACs +static const uchar jpegTableK6[] = +{ + 0, 2, 1, 2, 4, 4, 3, 4, 7, 5, 4, 4, 0, 1, 2, 119, + 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, + 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, + 0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, + 0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33, 0x52, 0xf0, + 0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34, + 0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26, + 0x27, 0x28, 0x29, 0x2a, 0x35, 0x36, 0x37, 0x38, + 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, + 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, + 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, + 0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, + 0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, + 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, + 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, + 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, + 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, + 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, + 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, + 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, + 0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, + 0xf9, 0xfa +}; + +static const uchar zigzag[] = +{ + 0, 8, 1, 2, 9, 16, 24, 17, 10, 3, 4, 11, 18, 25, 32, 40, + 33, 26, 19, 12, 5, 6, 13, 20, 27, 34, 41, 48, 56, 49, 42, 35, + 28, 21, 14, 7, 15, 22, 29, 36, 43, 50, 57, 58, 51, 44, 37, 30, + 23, 31, 38, 45, 52, 59, 60, 53, 46, 39, 47, 54, 61, 62, 55, 63, + 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63 +}; + + +static const int idct_prescale[] = +{ + 16384, 22725, 21407, 19266, 16384, 12873, 8867, 4520, + 22725, 31521, 29692, 26722, 22725, 17855, 12299, 6270, + 21407, 29692, 27969, 25172, 21407, 16819, 11585, 5906, + 19266, 26722, 25172, 22654, 19266, 15137, 10426, 5315, + 16384, 22725, 21407, 19266, 16384, 12873, 8867, 4520, + 12873, 17855, 16819, 15137, 12873, 10114, 6967, 3552, + 8867, 12299, 11585, 10426, 8867, 6967, 4799, 2446, + 4520, 6270, 5906, 5315, 4520, 3552, 2446, 1247 +}; + +static const char jpegHeader[] = +"\xFF\xD8" // SOI - start of image +"\xFF\xE0" // APP0 - jfif extention +"\x00\x10" // 2 bytes: length of APP0 segment +"JFIF\x00" // JFIF signature +"\x01\x02" // version of JFIF +"\x00" // units = pixels ( 1 - inch, 2 - cm ) +"\x00\x01\x00\x01" // 2 2-bytes values: x density & y density +"\x00\x00"; // width & height of thumbnail: ( 0x0 means no thumbnail) + +#ifdef WITH_NEON +// FDCT with postscaling +static void aan_fdct8x8( const short *src, short *dst, + int step, const short *postscale ) +{ + // Pass 1: process rows + int16x8_t x0 = vld1q_s16(src); int16x8_t x1 = vld1q_s16(src + step*7); + int16x8_t x2 = vld1q_s16(src + step*3); int16x8_t x3 = vld1q_s16(src + step*4); + + int16x8_t x4 = vaddq_s16(x0, x1); x0 = vsubq_s16(x0, x1); + x1 = vaddq_s16(x2, x3); x2 = vsubq_s16(x2, x3); + + int16x8_t t1 = x0; int16x8_t t2 = x2; + + x2 = vaddq_s16(x4, x1); x4 = vsubq_s16(x4, x1); + + x0 = vld1q_s16(src + step); x3 = vld1q_s16(src + step*6); + + x1 = vaddq_s16(x0, x3); x0 = vsubq_s16(x0, x3); + int16x8_t t3 = x0; + + x0 = vld1q_s16(src + step*2); x3 = vld1q_s16(src + step*5); + + int16x8_t t4 = vsubq_s16(x0, x3); + + x0 = vaddq_s16(x0, x3); + x3 = vaddq_s16(x0, x1); x0 = vsubq_s16(x0, x1); + x1 = vaddq_s16(x2, x3); x2 = vsubq_s16(x2, x3); + + int16x8_t res0 = x1; + int16x8_t res4 = x2; + x0 = vqdmulhq_n_s16(vsubq_s16(x0, x4), (short)(C0_707*2)); + x1 = vaddq_s16(x4, x0); x4 = vsubq_s16(x4, x0); + + int16x8_t res2 = x4; + int16x8_t res6 = x1; + + x0 = t2; x1 = t4; + x2 = t3; x3 = t1; + x0 = vaddq_s16(x0, x1); x1 = vaddq_s16(x1, x2); x2 = vaddq_s16(x2, x3); + x1 =vqdmulhq_n_s16(x1, (short)(C0_707*2)); + + x4 = vaddq_s16(x1, x3); x3 = vsubq_s16(x3, x1); + x1 = vqdmulhq_n_s16(vsubq_s16(x0, x2), (short)(C0_382*2)); + x0 = vaddq_s16(vqdmulhq_n_s16(x0, (short)(C0_541*2)), x1); + x2 = vaddq_s16(vshlq_n_s16(vqdmulhq_n_s16(x2, (short)C1_306), 1), x1); + + x1 = vaddq_s16(x0, x3); x3 = vsubq_s16(x3, x0); + x0 = vaddq_s16(x4, x2); x4 = vsubq_s16(x4, x2); + + int16x8_t res1 = x0; + int16x8_t res3 = x3; + int16x8_t res5 = x1; + int16x8_t res7 = x4; + + //transpose a matrix + /* + res0 00 01 02 03 04 05 06 07 + res1 10 11 12 13 14 15 16 17 + res2 20 21 22 23 24 25 26 27 + res3 30 31 32 33 34 35 36 37 + res4 40 41 42 43 44 45 46 47 + res5 50 51 52 53 54 55 56 57 + res6 60 61 62 63 64 65 66 67 + res7 70 71 72 73 74 75 76 77 + */ + + //transpose elements 00-33 + int16x4_t res0_0 = vget_low_s16(res0); + int16x4_t res1_0 = vget_low_s16(res1); + int16x4x2_t tres = vtrn_s16(res0_0, res1_0); + int32x4_t l0 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + res0_0 = vget_low_s16(res2); + res1_0 = vget_low_s16(res3); + tres = vtrn_s16(res0_0, res1_0); + int32x4_t l1 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + int32x4x2_t tres1 = vtrnq_s32(l0, l1); + + // transpose elements 40-73 + res0_0 = vget_low_s16(res4); + res1_0 = vget_low_s16(res5); + tres = vtrn_s16(res0_0, res1_0); + l0 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + res0_0 = vget_low_s16(res6); + res1_0 = vget_low_s16(res7); + + tres = vtrn_s16(res0_0, res1_0); + l1 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + int32x4x2_t tres2 = vtrnq_s32(l0, l1); + + //combine into 0-3 + int16x8_t transp_res0 = vreinterpretq_s16_s32(vcombine_s32(vget_low_s32(tres1.val[0]), vget_low_s32(tres2.val[0]))); + int16x8_t transp_res1 = vreinterpretq_s16_s32(vcombine_s32(vget_high_s32(tres1.val[0]), vget_high_s32(tres2.val[0]))); + int16x8_t transp_res2 = vreinterpretq_s16_s32(vcombine_s32(vget_low_s32(tres1.val[1]), vget_low_s32(tres2.val[1]))); + int16x8_t transp_res3 = vreinterpretq_s16_s32(vcombine_s32(vget_high_s32(tres1.val[1]), vget_high_s32(tres2.val[1]))); + + // transpose elements 04-37 + res0_0 = vget_high_s16(res0); + res1_0 = vget_high_s16(res1); + tres = vtrn_s16(res0_0, res1_0); + l0 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + res0_0 = vget_high_s16(res2); + res1_0 = vget_high_s16(res3); + + tres = vtrn_s16(res0_0, res1_0); + l1 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + tres1 = vtrnq_s32(l0, l1); + + // transpose elements 44-77 + res0_0 = vget_high_s16(res4); + res1_0 = vget_high_s16(res5); + tres = vtrn_s16(res0_0, res1_0); + l0 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + res0_0 = vget_high_s16(res6); + res1_0 = vget_high_s16(res7); + + tres = vtrn_s16(res0_0, res1_0); + l1 = vcombine_s32(vreinterpret_s32_s16(tres.val[0]),vreinterpret_s32_s16(tres.val[1])); + + tres2 = vtrnq_s32(l0, l1); + + //combine into 4-7 + int16x8_t transp_res4 = vreinterpretq_s16_s32(vcombine_s32(vget_low_s32(tres1.val[0]), vget_low_s32(tres2.val[0]))); + int16x8_t transp_res5 = vreinterpretq_s16_s32(vcombine_s32(vget_high_s32(tres1.val[0]), vget_high_s32(tres2.val[0]))); + int16x8_t transp_res6 = vreinterpretq_s16_s32(vcombine_s32(vget_low_s32(tres1.val[1]), vget_low_s32(tres2.val[1]))); + int16x8_t transp_res7 = vreinterpretq_s16_s32(vcombine_s32(vget_high_s32(tres1.val[1]), vget_high_s32(tres2.val[1]))); + + //special hack for vqdmulhq_s16 command that is producing -1 instead of 0 +#define STORE_DESCALED(addr, reg, mul_addr) postscale_line = vld1q_s16((mul_addr)); \ +mask = vreinterpretq_s16_u16(vcltq_s16((reg), z)); \ +reg = vabsq_s16(reg); \ +reg = vqdmulhq_s16(vqaddq_s16((reg), (reg)), postscale_line); \ +reg = vsubq_s16(veorq_s16(reg, mask), mask); \ +vst1q_s16((addr), reg); + + int16x8_t z = vdupq_n_s16(0), postscale_line, mask; + + // pass 2: process columns + x0 = transp_res0; x1 = transp_res7; + x2 = transp_res3; x3 = transp_res4; + + x4 = vaddq_s16(x0, x1); x0 = vsubq_s16(x0, x1); + x1 = vaddq_s16(x2, x3); x2 = vsubq_s16(x2, x3); + + t1 = x0; t2 = x2; + + x2 = vaddq_s16(x4, x1); x4 = vsubq_s16(x4, x1); + + x0 = transp_res1; + x3 = transp_res6; + + x1 = vaddq_s16(x0, x3); x0 = vsubq_s16(x0, x3); + + t3 = x0; + + x0 = transp_res2; x3 = transp_res5; + + t4 = vsubq_s16(x0, x3); + + x0 = vaddq_s16(x0, x3); + + x3 = vaddq_s16(x0, x1); x0 = vsubq_s16(x0, x1); + x1 = vaddq_s16(x2, x3); x2 = vsubq_s16(x2, x3); + + STORE_DESCALED(dst, x1, postscale); + STORE_DESCALED(dst + 4*8, x2, postscale + 4*8); + + x0 = vqdmulhq_n_s16(vsubq_s16(x0, x4), (short)(C0_707*2)); + + x1 = vaddq_s16(x4, x0); x4 = vsubq_s16(x4, x0); + + STORE_DESCALED(dst + 2*8, x4,postscale + 2*8); + STORE_DESCALED(dst + 6*8, x1,postscale + 6*8); + + x0 = t2; x1 = t4; + x2 = t3; x3 = t1; + + x0 = vaddq_s16(x0, x1); x1 = vaddq_s16(x1, x2); x2 = vaddq_s16(x2, x3); + + x1 =vqdmulhq_n_s16(x1, (short)(C0_707*2)); + + x4 = vaddq_s16(x1, x3); x3 = vsubq_s16(x3, x1); + + x1 = vqdmulhq_n_s16(vsubq_s16(x0, x2), (short)(C0_382*2)); + x0 = vaddq_s16(vqdmulhq_n_s16(x0, (short)(C0_541*2)), x1); + x2 = vaddq_s16(vshlq_n_s16(vqdmulhq_n_s16(x2, (short)C1_306), 1), x1); + + x1 = vaddq_s16(x0, x3); x3 = vsubq_s16(x3, x0); + x0 = vaddq_s16(x4, x2); x4 = vsubq_s16(x4, x2); + + STORE_DESCALED(dst + 5*8, x1,postscale + 5*8); + STORE_DESCALED(dst + 1*8, x0,postscale + 1*8); + STORE_DESCALED(dst + 7*8, x4,postscale + 7*8); + STORE_DESCALED(dst + 3*8, x3,postscale + 3*8); +} + +#else +// FDCT with postscaling +static void aan_fdct8x8( const short *src, short *dst, + int step, const short *postscale ) +{ + short workspace[64], *work = workspace; + int i; + + // Pass 1: process rows + for( i = 8; i > 0; i--, src += step, work += 8 ) + { + int x0 = src[0], x1 = src[7]; + int x2 = src[3], x3 = src[4]; + + int x4 = x0 + x1; x0 -= x1; + x1 = x2 + x3; x2 -= x3; + + work[7] = x0; work[1] = x2; + x2 = x4 + x1; x4 -= x1; + + x0 = src[1]; x3 = src[6]; + x1 = x0 + x3; x0 -= x3; + work[5] = x0; + + x0 = src[2]; x3 = src[5]; + work[3] = x0 - x3; x0 += x3; + + x3 = x0 + x1; x0 -= x1; + x1 = x2 + x3; x2 -= x3; + + work[0] = x1; work[4] = x2; + + x0 = DCT_DESCALE((x0 - x4)*C0_707, fixb); + x1 = x4 + x0; x4 -= x0; + work[2] = x4; work[6] = x1; + + x0 = work[1]; x1 = work[3]; + x2 = work[5]; x3 = work[7]; + + x0 += x1; x1 += x2; x2 += x3; + x1 = DCT_DESCALE(x1*C0_707, fixb); + + x4 = x1 + x3; x3 -= x1; + x1 = (x0 - x2)*C0_382; + x0 = DCT_DESCALE(x0*C0_541 + x1, fixb); + x2 = DCT_DESCALE(x2*C1_306 + x1, fixb); + + x1 = x0 + x3; x3 -= x0; + x0 = x4 + x2; x4 -= x2; + + work[5] = x1; work[1] = x0; + work[7] = x4; work[3] = x3; + } + + work = workspace; + // pass 2: process columns + for( i = 8; i > 0; i--, work++, postscale ++, dst += 8 ) + { + int x0 = work[8*0], x1 = work[8*7]; + int x2 = work[8*3], x3 = work[8*4]; + + int x4 = x0 + x1; x0 -= x1; + x1 = x2 + x3; x2 -= x3; + + work[8*7] = x0; work[8*0] = x2; + x2 = x4 + x1; x4 -= x1; + + x0 = work[8*1]; x3 = work[8*6]; + x1 = x0 + x3; x0 -= x3; + work[8*4] = x0; + + x0 = work[8*2]; x3 = work[8*5]; + work[8*3] = x0 - x3; x0 += x3; + + x3 = x0 + x1; x0 -= x1; + x1 = x2 + x3; x2 -= x3; + + dst[0] = DCT_DESCALE(x1*postscale[0*8], postshift); + dst[4] = DCT_DESCALE(x2*postscale[4*8], postshift); + + x0 = DCT_DESCALE((x0 - x4)*C0_707, fixb); + x1 = x4 + x0; x4 -= x0; + + dst[2] = DCT_DESCALE(x4*postscale[2*8], postshift); + dst[6] = DCT_DESCALE(x1*postscale[6*8], postshift); + + x0 = work[8*0]; x1 = work[8*3]; + x2 = work[8*4]; x3 = work[8*7]; + + x0 += x1; x1 += x2; x2 += x3; + x1 = DCT_DESCALE(x1*C0_707, fixb); + + x4 = x1 + x3; x3 -= x1; + x1 = (x0 - x2)*C0_382; + x0 = DCT_DESCALE(x0*C0_541 + x1, fixb); + x2 = DCT_DESCALE(x2*C1_306 + x1, fixb); + + x1 = x0 + x3; x3 -= x0; + x0 = x4 + x2; x4 -= x2; + + dst[5] = DCT_DESCALE(x1*postscale[5*8], postshift); + dst[1] = DCT_DESCALE(x0*postscale[1*8], postshift); + dst[7] = DCT_DESCALE(x4*postscale[7*8], postshift); + dst[3] = DCT_DESCALE(x3*postscale[3*8], postshift); + } +} +#endif + +void MotionJpegWriter::writeFrameData( const uchar* data, int step, int colorspace, int input_channels ) +{ + //double total_cvt = 0, total_dct = 0; + static bool init_cat_table = false; + const int CAT_TAB_SIZE = 4096; + static uchar cat_table[CAT_TAB_SIZE*2+1]; + if( !init_cat_table ) + { + for( int i = -CAT_TAB_SIZE; i <= CAT_TAB_SIZE; i++ ) + { + float a = (float)i; + cat_table[i+CAT_TAB_SIZE] = (((int&)a >> 23) & 255) - (126 & (i ? -1 : 0)); + } + init_cat_table = true; + } + + //double total_dct = 0, total_cvt = 0; + CV_Assert( data && width > 0 && height > 0 ); + + // encode the header and tables + // for each mcu: + // convert rgb to yuv with downsampling (if color). + // for every block: + // calc dct and quantize + // encode block. + int x, y; + int i, j; + const int max_quality = 12; + short fdct_qtab[2][64]; + unsigned huff_dc_tab[2][16]; + unsigned huff_ac_tab[2][256]; + + int x_scale = channels > 1 ? 2 : 1, y_scale = x_scale; + int dc_pred[] = { 0, 0, 0 }; + int x_step = x_scale * 8; + int y_step = y_scale * 8; + short block[6][64]; + short buffer[4096]; + int* hbuffer = (int*)buffer; + int luma_count = x_scale*y_scale; + int block_count = luma_count + channels - 1; + int Y_step = x_scale*8; + const int UV_step = 16; + int u_plane_ofs = step*height; + int v_plane_ofs = u_plane_ofs + step*height; + + if( quality < 1 ) quality = 1; + if( quality > max_quality ) quality = max_quality; + + double inv_quality = 1./quality; + + // Encode header + strm.putBytes( (const uchar*)jpegHeader, sizeof(jpegHeader) - 1 ); + + // Encode quantization tables + for( i = 0; i < (channels > 1 ? 2 : 1); i++ ) + { + const uchar* qtable = i == 0 ? jpegTableK1_T : jpegTableK2_T; + int chroma_scale = i > 0 ? luma_count : 1; + + strm.jputShort( 0xffdb ); // DQT marker + strm.jputShort( 2 + 65*1 ); // put single qtable + strm.putByte( 0*16 + i ); // 8-bit table + + // put coefficients + for( j = 0; j < 64; j++ ) + { + int idx = zigzag[j]; + int qval = cvRound(qtable[idx]*inv_quality); + if( qval < 1 ) + qval = 1; + if( qval > 255 ) + qval = 255; + fdct_qtab[i][(idx/8) + (idx%8)*8] = (cvRound((1 << (postshift + 11)))/ + (qval*chroma_scale*idct_prescale[idx])); + strm.putByte( qval ); + } + } + + // Encode huffman tables + for( i = 0; i < (channels > 1 ? 4 : 2); i++ ) + { + const uchar* htable = i == 0 ? jpegTableK3 : i == 1 ? jpegTableK5 : + i == 2 ? jpegTableK4 : jpegTableK6; + int is_ac_tab = i & 1; + int idx = i >= 2; + int tableSize = 16 + (is_ac_tab ? 162 : 12); + + strm.jputShort( 0xFFC4 ); // DHT marker + strm.jputShort( 3 + tableSize ); // define one huffman table + strm.putByte( is_ac_tab*16 + idx ); // put DC/AC flag and table index + strm.putBytes( htable, tableSize ); // put table + + BitStream::createEncodeHuffmanTable( BitStream::createSourceHuffmanTable( + htable, hbuffer, 16, 9 ), is_ac_tab ? huff_ac_tab[idx] : + huff_dc_tab[idx], is_ac_tab ? 256 : 16 ); + } + + // put frame header + strm.jputShort( 0xFFC0 ); // SOF0 marker + strm.jputShort( 8 + 3*channels ); // length of frame header + strm.putByte( 8 ); // sample precision + strm.jputShort( height ); + strm.jputShort( width ); + strm.putByte( channels ); // number of components + + for( i = 0; i < channels; i++ ) + { + strm.putByte( i + 1 ); // (i+1)-th component id (Y,U or V) + if( i == 0 ) + strm.putByte(x_scale*16 + y_scale); // chroma scale factors + else + strm.putByte(1*16 + 1); + strm.putByte( i > 0 ); // quantization table idx + } + + // put scan header + strm.jputShort( 0xFFDA ); // SOS marker + strm.jputShort( 6 + 2*channels ); // length of scan header + strm.putByte( channels ); // number of components in the scan + + for( i = 0; i < channels; i++ ) + { + strm.putByte( i+1 ); // component id + strm.putByte( (i>0)*16 + (i>0) );// selection of DC & AC tables + } + + strm.jputShort(0*256 + 63); // start and end of spectral selection - for + // sequental DCT start is 0 and end is 63 + + strm.putByte( 0 ); // successive approximation bit position + // high & low - (0,0) for sequental DCT + unsigned currval = 0, code = 0, tempval = 0; + int bit_idx = 32; + +#define JPUT_BITS(val, bits) \ + bit_idx -= (bits); \ + tempval = (val) & bit_mask[(bits)]; \ + if( bit_idx <= 0 ) \ + { \ + strm.jput(currval | ((unsigned)tempval >> -bit_idx)); \ + bit_idx += 32; \ + currval = bit_idx < 32 ? (tempval << bit_idx) : 0; \ + } \ + else \ + currval |= (tempval << bit_idx) + +#define JPUT_HUFF(val, table) \ + code = table[(val) + 2]; \ + JPUT_BITS(code >> 8, (int)(code & 255)) + + // encode data + for( y = 0; y < height; y += y_step, data += y_step*step ) + { + for( x = 0; x < width; x += x_step ) + { + int x_limit = x_step; + int y_limit = y_step; + const uchar* pix_data = data + x*input_channels; + short* Y_data = block[0]; + + if( x + x_limit > width ) x_limit = width - x; + if( y + y_limit > height ) y_limit = height - y; + + memset( block, 0, block_count*64*sizeof(block[0][0])); + + if( channels > 1 ) + { + short* UV_data = block[luma_count]; + // double t = (double)cv::getTickCount(); + + if( colorspace == COLORSPACE_YUV444P && y_limit == 16 && x_limit == 16 ) + { + for( i = 0; i < y_limit; i += 2, pix_data += step*2, Y_data += Y_step*2, UV_data += UV_step ) + { +#ifdef WITH_NEON + { + uint16x8_t masklo = vdupq_n_u16(255); + uint16x8_t lane = vld1q_u16((unsigned short*)(pix_data+v_plane_ofs)); + uint16x8_t t1 = vaddq_u16(vshrq_n_u16(lane, 8), vandq_u16(lane, masklo)); + lane = vld1q_u16((unsigned short*)(pix_data + v_plane_ofs + step)); + uint16x8_t t2 = vaddq_u16(vshrq_n_u16(lane, 8), vandq_u16(lane, masklo)); + t1 = vaddq_u16(t1, t2); + vst1q_s16(UV_data, vsubq_s16(vreinterpretq_s16_u16(t1), vdupq_n_s16(128*4))); + + lane = vld1q_u16((unsigned short*)(pix_data+u_plane_ofs)); + t1 = vaddq_u16(vshrq_n_u16(lane, 8), vandq_u16(lane, masklo)); + lane = vld1q_u16((unsigned short*)(pix_data + u_plane_ofs + step)); + t2 = vaddq_u16(vshrq_n_u16(lane, 8), vandq_u16(lane, masklo)); + t1 = vaddq_u16(t1, t2); + vst1q_s16(UV_data + 8, vsubq_s16(vreinterpretq_s16_u16(t1), vdupq_n_s16(128*4))); + } + + { + int16x8_t lane = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(pix_data))); + int16x8_t delta = vdupq_n_s16(128); + lane = vsubq_s16(lane, delta); + vst1q_s16(Y_data, lane); + + lane = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(pix_data+8))); + lane = vsubq_s16(lane, delta); + vst1q_s16(Y_data + 8, lane); + + lane = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(pix_data+step))); + lane = vsubq_s16(lane, delta); + vst1q_s16(Y_data+Y_step, lane); + + lane = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(pix_data + step + 8))); + lane = vsubq_s16(lane, delta); + vst1q_s16(Y_data+Y_step + 8, lane); + } +#else + for( j = 0; j < x_limit; j += 2, pix_data += 2 ) + { + Y_data[j] = pix_data[0] - 128; + Y_data[j+1] = pix_data[1] - 128; + Y_data[j+Y_step] = pix_data[step] - 128; + Y_data[j+Y_step+1] = pix_data[step+1] - 128; + + UV_data[j>>1] = pix_data[v_plane_ofs] + pix_data[v_plane_ofs+1] + + pix_data[v_plane_ofs+step] + pix_data[v_plane_ofs+step+1] - 128*4; + UV_data[(j>>1)+8] = pix_data[u_plane_ofs] + pix_data[u_plane_ofs+1] + + pix_data[u_plane_ofs+step] + pix_data[u_plane_ofs+step+1] - 128*4; + + } + + pix_data -= x_limit*input_channels; +#endif + } + } + else + { + for( i = 0; i < y_limit; i++, pix_data += step, Y_data += Y_step ) + { + for( j = 0; j < x_limit; j++, pix_data += input_channels ) + { + int Y, U, V; + + if( colorspace == COLORSPACE_BGR ) + { + int r = pix_data[2]; + int g = pix_data[1]; + int b = pix_data[0]; + + Y = DCT_DESCALE( r*y_r + g*y_g + b*y_b, fixc) - 128; + U = DCT_DESCALE( r*cb_r + g*cb_g + b*cb_b, fixc ); + V = DCT_DESCALE( r*cr_r + g*cr_g + b*cr_b, fixc ); + } + else if( colorspace == COLORSPACE_RGBA ) + { + int r = pix_data[0]; + int g = pix_data[1]; + int b = pix_data[2]; + + Y = DCT_DESCALE( r*y_r + g*y_g + b*y_b, fixc) - 128; + U = DCT_DESCALE( r*cb_r + g*cb_g + b*cb_b, fixc ); + V = DCT_DESCALE( r*cr_r + g*cr_g + b*cr_b, fixc ); + } + else + { + Y = pix_data[0] - 128; + U = pix_data[v_plane_ofs] - 128; + V = pix_data[u_plane_ofs] - 128; + } + + int j2 = j >> (x_scale - 1); + Y_data[j] = (short)Y; + UV_data[j2] = (short)(UV_data[j2] + U); + UV_data[j2 + 8] = (short)(UV_data[j2 + 8] + V); + } + + pix_data -= x_limit*input_channels; + if( ((i+1) & (y_scale - 1)) == 0 ) + { + UV_data += UV_step; + } + } + } + + // total_cvt += (double)cv::getTickCount() - t; + } + else + { + for( i = 0; i < y_limit; i++, pix_data += step, Y_data += Y_step ) + { + for( j = 0; j < x_limit; j++ ) + Y_data[j] = (short)(pix_data[j]*4 - 128*4); + } + } + + for( i = 0; i < block_count; i++ ) + { + int is_chroma = i >= luma_count; + int src_step = x_scale * 8; + int run = 0, val; + const short* src_ptr = block[i & -2] + (i & 1)*8; + const unsigned* htable = huff_ac_tab[is_chroma]; + + //double t = (double)cv::getTickCount(); + aan_fdct8x8( src_ptr, buffer, src_step, fdct_qtab[is_chroma] ); + //total_dct += (double)cv::getTickCount() - t; + + j = is_chroma + (i > luma_count); + val = buffer[0] - dc_pred[j]; + dc_pred[j] = buffer[0]; + + { + int cat = cat_table[val + CAT_TAB_SIZE]; + + //CV_Assert( cat <= 11 ); + JPUT_HUFF( cat, huff_dc_tab[is_chroma] ); + JPUT_BITS( val - (val < 0 ? 1 : 0), cat ); + } + + for( j = 1; j < 64; j++ ) + { + val = buffer[zigzag[j]]; + + if( val == 0 ) + { + run++; + } + else + { + while( run >= 16 ) + { + JPUT_HUFF( 0xF0, htable ); // encode 16 zeros + run -= 16; + } + + { + int cat = cat_table[val + CAT_TAB_SIZE]; + //CV_Assert( cat <= 10 ); + JPUT_HUFF( cat + run*16, htable ); + JPUT_BITS( val - (val < 0 ? 1 : 0), cat ); + } + + run = 0; + } + } + + if( run ) + { + JPUT_HUFF( 0x00, htable ); // encode EOB + } + } + } + } + + // Flush + JPUT_BITS((unsigned)-1, bit_idx & 31); + strm.jputShort( 0xFFD9 ); // EOI marker + /*printf("total dct = %.1fms, total cvt = %.1fms\n", + total_dct*1000./cv::getTickFrequency(), + total_cvt*1000./cv::getTickFrequency());*/ + size_t pos = strm.getPos(); + size_t pos1 = (pos + 3) & ~3; + for( ; pos < pos1; pos++ ) + strm.putByte(0); +} + +} + +Ptr createMotionJpegWriter( const String& filename, double fps, Size frameSize, bool iscolor ) +{ + Ptr iwriter = makePtr(filename, fps, frameSize, iscolor); + if( !iwriter->isOpened() ) + iwriter.release(); + return iwriter; +} + +} diff --git a/modules/videoio/src/precomp.hpp b/modules/videoio/src/precomp.hpp index c399d72b1..38623c7cf 100644 --- a/modules/videoio/src/precomp.hpp +++ b/modules/videoio/src/precomp.hpp @@ -168,9 +168,24 @@ namespace cv virtual double getProperty(int) const { return 0; } virtual bool setProperty(int, double) { return 0; } virtual bool grabFrame() = 0; - virtual bool retrieveFrame(int, cv::OutputArray) = 0; + virtual bool retrieveFrame(int, OutputArray) = 0; + virtual bool isOpened() const = 0; virtual int getCaptureDomain() { return CAP_ANY; } // Return the type of the capture object: CAP_VFW, etc... }; + + class IVideoWriter + { + public: + virtual ~IVideoWriter() {} + virtual double getProperty(int) const { return 0; } + virtual bool setProperty(int, double) { return 0; } + + virtual bool isOpened() const = 0; + virtual void write(InputArray) = 0; + }; + + Ptr createMotionJpegCapture(const String& filename); + Ptr createMotionJpegWriter( const String& filename, double fps, Size frameSize, bool iscolor ); }; #endif /* __VIDEOIO_H_ */ From 82adf7955fc5250cbd95612aaf3abaae24d0e836 Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Thu, 26 Mar 2015 11:22:52 +0300 Subject: [PATCH 140/171] Fix python submodules inheritance detection --- modules/python/src2/gen2.py | 36 ++++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/modules/python/src2/gen2.py b/modules/python/src2/gen2.py index 55a79484c..43adfbfc4 100755 --- a/modules/python/src2/gen2.py +++ b/modules/python/src2/gen2.py @@ -255,20 +255,24 @@ class ClassInfo(object): self.methods = {} self.props = [] self.consts = {} + self.base = None customname = False if decl: - self.bases = decl[1].split()[1:] - if len(self.bases) > 1: + bases = decl[1].split()[1:] + if len(bases) > 1: print("Note: Class %s has more than 1 base class (not supported by Python C extensions)" % (self.name,)) - print(" Bases: ", " ".join(self.bases)) + print(" Bases: ", " ".join(bases)) print(" Only the first base class will be used") - self.bases = [self.bases[0].strip(",")] #return sys.exit(-1) - if self.bases and self.bases[0].startswith("cv::"): - self.bases[0] = self.bases[0][4:] - if self.bases and self.bases[0] == "Algorithm": - self.isalgorithm = True + elif len(bases) == 1: + self.base = bases[0].strip(",") + if self.base.startswith("cv::"): + self.base = self.base[4:] + if self.base == "Algorithm": + self.isalgorithm = True + self.base = self.base.replace("::", "_") + for m in decl[2]: if m.startswith("="): self.wname = m[1:] @@ -285,8 +289,8 @@ class ClassInfo(object): def gen_map_code(self, all_classes): code = "static bool pyopencv_to(PyObject* src, %s& dst, const char* name)\n{\n PyObject* tmp;\n bool ok;\n" % (self.cname) code += "".join([gen_template_set_prop_from_map.substitute(propname=p.name,proptype=p.tp) for p in self.props]) - if self.bases: - code += "\n return pyopencv_to(src, (%s&)dst, name);\n}\n" % all_classes[self.bases[0].replace("::", "_")].cname + if self.base: + code += "\n return pyopencv_to(src, (%s&)dst, name);\n}\n" % all_classes[self.base].cname else: code += "\n return true;\n}\n" return code @@ -330,8 +334,8 @@ class ClassInfo(object): methods_inits.write(m.get_tab_entry()) baseptr = "NULL" - if self.bases and self.bases[0] in all_classes: - baseptr = "&pyopencv_" + all_classes[self.bases[0]].name + "_Type" + if self.base and self.base in all_classes: + baseptr = "&pyopencv_" + all_classes[self.base].name + "_Type" code = gen_template_type_impl.substitute(name=self.name, wname=self.wname, cname=self.cname, getset_code=getset_code.getvalue(), getset_inits=getset_inits.getvalue(), @@ -753,17 +757,17 @@ class PythonWrapperGenerator(object): sys.exit(-1) self.classes[classinfo.name] = classinfo - if classinfo.bases: - chunks = classinfo.bases[0].split('::') + if classinfo.base: + chunks = classinfo.base.split('_') base = '_'.join(chunks) while base not in self.classes and len(chunks)>1: del chunks[-2] base = '_'.join(chunks) if base not in self.classes: print("Generator error: unable to resolve base %s for %s" - % (classinfo.bases[0], classinfo.name)) + % (classinfo.base, classinfo.name)) sys.exit(-1) - classinfo.bases[0] = "::".join(chunks) + classinfo.base = base classinfo.isalgorithm |= self.classes[base].isalgorithm def split_decl_name(self, name): From e94dfcee0d9826f614606f88791f46cd416ccd4f Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Thu, 26 Mar 2015 11:43:49 +0300 Subject: [PATCH 141/171] Add python test for inheritance structure generation --- modules/python/test/test.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/modules/python/test/test.py b/modules/python/test/test.py index a0f0daa56..093979aba 100644 --- a/modules/python/test/test.py +++ b/modules/python/test/test.py @@ -145,6 +145,16 @@ class Hackathon244Tests(NewOpenCVTests): self.check_close_pairs(mc, mc0, 5) self.assertLessEqual(abs(mr - mr0), 5) + def test_inheritance(self): + bm = cv2.StereoBM_create() + bm.getPreFilterCap() # from StereoBM + bm.getBlockSize() # from SteroMatcher + + boost = cv2.ml.Boost_create() + boost.getBoostType() # from ml::Boost + boost.getMaxDepth() # from ml::DTrees + boost.isClassifier() # from ml::StatModel + if __name__ == '__main__': parser = argparse.ArgumentParser(description='run OpenCV python tests') parser.add_argument('--repo', help='use sample image files from local git repository (path to folder), ' From 8d1232cc7adb122e4c98b8c3460daef09c7e2af3 Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Thu, 26 Mar 2015 15:59:49 +0300 Subject: [PATCH 142/171] fixed warnings; transpose qtab to make it compatible with neon branch --- modules/videoio/src/cap_mjpeg_decoder.cpp | 2 +- modules/videoio/src/cap_mjpeg_encoder.cpp | 34 ++++++++++++----------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/modules/videoio/src/cap_mjpeg_decoder.cpp b/modules/videoio/src/cap_mjpeg_decoder.cpp index ac5247e6a..11a86b2ab 100644 --- a/modules/videoio/src/cap_mjpeg_decoder.cpp +++ b/modules/videoio/src/cap_mjpeg_decoder.cpp @@ -44,7 +44,7 @@ namespace cv { -Ptr createMotionJpegCapture(const String& filename) +Ptr createMotionJpegCapture(const String&) { return Ptr(); } diff --git a/modules/videoio/src/cap_mjpeg_encoder.cpp b/modules/videoio/src/cap_mjpeg_encoder.cpp index eafd91388..601aeb644 100644 --- a/modules/videoio/src/cap_mjpeg_encoder.cpp +++ b/modules/videoio/src/cap_mjpeg_encoder.cpp @@ -216,8 +216,8 @@ public: } else { - size_t fpos = ftell(m_f); - fseek(m_f, pos, SEEK_SET); + long fpos = ftell(m_f); + fseek(m_f, (long)pos, SEEK_SET); uchar buf[] = { (uchar)val, (uchar)(val >> 8), (uchar)(val >> 16), (uchar)(val >> 24) }; fwrite(buf, 1, 4, m_f); fseek(m_f, fpos, SEEK_SET); @@ -390,6 +390,7 @@ public: startWriteAVI(); writeStreamHeader(); } + //printf("motion jpeg stream %s has been successfully opened\n", filename.c_str()); return true; } @@ -966,7 +967,7 @@ vst1q_s16((addr), reg); static void aan_fdct8x8( const short *src, short *dst, int step, const short *postscale ) { - short workspace[64], *work = workspace; + int workspace[64], *work = workspace; int i; // Pass 1: process rows @@ -1017,7 +1018,7 @@ static void aan_fdct8x8( const short *src, short *dst, work = workspace; // pass 2: process columns - for( i = 8; i > 0; i--, work++, postscale ++, dst += 8 ) + for( i = 8; i > 0; i--, work++, postscale += 8, dst += 8 ) { int x0 = work[8*0], x1 = work[8*7]; int x2 = work[8*3], x3 = work[8*4]; @@ -1038,14 +1039,14 @@ static void aan_fdct8x8( const short *src, short *dst, x3 = x0 + x1; x0 -= x1; x1 = x2 + x3; x2 -= x3; - dst[0] = DCT_DESCALE(x1*postscale[0*8], postshift); - dst[4] = DCT_DESCALE(x2*postscale[4*8], postshift); + dst[0] = (short)DCT_DESCALE(x1*postscale[0], postshift); + dst[4] = (short)DCT_DESCALE(x2*postscale[4], postshift); x0 = DCT_DESCALE((x0 - x4)*C0_707, fixb); x1 = x4 + x0; x4 -= x0; - dst[2] = DCT_DESCALE(x4*postscale[2*8], postshift); - dst[6] = DCT_DESCALE(x1*postscale[6*8], postshift); + dst[2] = (short)DCT_DESCALE(x4*postscale[2], postshift); + dst[6] = (short)DCT_DESCALE(x1*postscale[6], postshift); x0 = work[8*0]; x1 = work[8*3]; x2 = work[8*4]; x3 = work[8*7]; @@ -1061,10 +1062,10 @@ static void aan_fdct8x8( const short *src, short *dst, x1 = x0 + x3; x3 -= x0; x0 = x4 + x2; x4 -= x2; - dst[5] = DCT_DESCALE(x1*postscale[5*8], postshift); - dst[1] = DCT_DESCALE(x0*postscale[1*8], postshift); - dst[7] = DCT_DESCALE(x4*postscale[7*8], postshift); - dst[3] = DCT_DESCALE(x3*postscale[3*8], postshift); + dst[5] = (short)DCT_DESCALE(x1*postscale[5], postshift); + dst[1] = (short)DCT_DESCALE(x0*postscale[1], postshift); + dst[7] = (short)DCT_DESCALE(x4*postscale[7], postshift); + dst[3] = (short)DCT_DESCALE(x3*postscale[3], postshift); } } #endif @@ -1079,8 +1080,9 @@ void MotionJpegWriter::writeFrameData( const uchar* data, int step, int colorspa { for( int i = -CAT_TAB_SIZE; i <= CAT_TAB_SIZE; i++ ) { - float a = (float)i; - cat_table[i+CAT_TAB_SIZE] = (((int&)a >> 23) & 255) - (126 & (i ? -1 : 0)); + Cv32suf a; + a.f = (float)i; + cat_table[i+CAT_TAB_SIZE] = ((a.i >> 23) & 255) - (126 & (i ? -1 : 0)); } init_cat_table = true; } @@ -1142,8 +1144,8 @@ void MotionJpegWriter::writeFrameData( const uchar* data, int step, int colorspa qval = 1; if( qval > 255 ) qval = 255; - fdct_qtab[i][(idx/8) + (idx%8)*8] = (cvRound((1 << (postshift + 11)))/ - (qval*chroma_scale*idct_prescale[idx])); + fdct_qtab[i][idx] = (short)(cvRound((1 << (postshift + 11)))/ + (qval*chroma_scale*idct_prescale[idx])); strm.putByte( qval ); } } From b91313a84efe1879186d128ab2825883c10f91a0 Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Thu, 26 Mar 2015 16:26:41 +0300 Subject: [PATCH 143/171] initialize the output flow in EMD (thanks to Daniel DeMenthon for the bug report) --- modules/imgproc/src/emd.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/imgproc/src/emd.cpp b/modules/imgproc/src/emd.cpp index 69006f3c4..22468da6d 100644 --- a/modules/imgproc/src/emd.cpp +++ b/modules/imgproc/src/emd.cpp @@ -1152,6 +1152,7 @@ float cv::EMD( InputArray _signature1, InputArray _signature2, { _flow.create(signature1.rows, signature2.rows, CV_32F); flow = _flow.getMat(); + flow = Scalar::all(0); _cflow = flow; } From 5e354fa844d21c8694dd60f44ce4f73dac122238 Mon Sep 17 00:00:00 2001 From: Kevin Squire Date: Thu, 15 Jan 2015 18:13:06 -0800 Subject: [PATCH 144/171] Allow PYTHON_LIBRARY and PYTHON_INCLUDE_DIR to actually be overridden --- cmake/OpenCVDetectPython.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/OpenCVDetectPython.cmake b/cmake/OpenCVDetectPython.cmake index 8ed425fb5..ed7569821 100644 --- a/cmake/OpenCVDetectPython.cmake +++ b/cmake/OpenCVDetectPython.cmake @@ -75,10 +75,10 @@ function(find_python preferred_version min_version library_env include_dir_env if(NOT ANDROID AND NOT IOS) ocv_check_environment_variables(${library_env} ${include_dir_env}) - if(${${library_env}}) + if(NOT ${${library_env}} EQUAL "") set(PYTHON_LIBRARY "${${library_env}}") endif() - if(${${include_dir_env}}) + if(NOT ${${include_dir_env}} EQUAL "") set(PYTHON_INCLUDE_DIR "${${include_dir_env}}") endif() From fab2a947caaca0a6e05af7bc50f9cf141f3ec371 Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Fri, 27 Mar 2015 12:15:26 +0300 Subject: [PATCH 145/171] Fix build for VC10 --- modules/calib3d/src/rho.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index 81d641c3f..bed37a63e 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -1631,7 +1631,7 @@ static inline void sacInitNonRand(double beta, for(; n < N; n++){ double mu = n * beta; - double sigma = sqrt(n)* beta_beta1_sq_chi; + double sigma = sqrt((double)n)* beta_beta1_sq_chi; unsigned i_min = (unsigned)ceil(SMPL_SIZE + mu + sigma); nonRandMinInl[n] = i_min; From 56b02331f706dc1a15e283d4cffe8da47e87a193 Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Fri, 27 Mar 2015 14:36:27 +0300 Subject: [PATCH 146/171] Commented unused constants in the RHO algorithm --- modules/calib3d/src/rho.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/calib3d/src/rho.cpp b/modules/calib3d/src/rho.cpp index bed37a63e..2f27728a2 100644 --- a/modules/calib3d/src/rho.cpp +++ b/modules/calib3d/src/rho.cpp @@ -69,10 +69,10 @@ namespace cv{/* For C support, replace with extern "C" { */ const int MEM_ALIGN = 32; const size_t HSIZE = (3*3*sizeof(float)); const double MIN_DELTA_CHNG = 0.1; -const double CHI_STAT = 2.706; +// const double CHI_STAT = 2.706; const double CHI_SQ = 1.645; -const double RLO = 0.25; -const double RHI = 0.75; +// const double RLO = 0.25; +// const double RHI = 0.75; const int MAXLEVMARQITERS = 100; const int SMPL_SIZE = 4; /* 4 points required per model */ const int SPRT_T_M = 25; /* Guessing 25 match evlauations / 1 model generation */ From 602d2c33c0098ce7553eb7b283d7cedb84425b1f Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Fri, 27 Mar 2015 15:15:59 +0300 Subject: [PATCH 147/171] added video writer quality, which user may change dynamically in the case of motion jpeg; enabled NEON intrinsics in the encoder --- modules/videoio/include/opencv2/videoio.hpp | 22 +++++++++++++ modules/videoio/src/cap.cpp | 15 +++++++++ modules/videoio/src/cap_mjpeg_encoder.cpp | 35 ++++++++++++++++----- modules/videoio/src/precomp.hpp | 4 +-- 4 files changed, 66 insertions(+), 10 deletions(-) diff --git a/modules/videoio/include/opencv2/videoio.hpp b/modules/videoio/include/opencv2/videoio.hpp index 85021163a..ae958bcee 100644 --- a/modules/videoio/include/opencv2/videoio.hpp +++ b/modules/videoio/include/opencv2/videoio.hpp @@ -376,6 +376,9 @@ enum { CAP_INTELPERC_DEPTH_MAP = 0, // Each pixel is a 16-bit integ CAP_INTELPERC_IMAGE = 3 }; +enum { VIDEOWRITER_PROP_QUALITY = 1, // Quality (0..100%) of the videostream encoded + VIDEOWRITER_PROP_FRAMEBYTES = 2, // (Read-only): Size of just encoded video frame + }; class IVideoCapture; @@ -642,6 +645,25 @@ public: */ CV_WRAP virtual void write(const Mat& image); + /** @brief Sets a property in the VideoWriter. + + @param propId Property identifier. It can be one of the following: + - **VIDEOWRITER_PROP_QUALITY** Quality (0..100%) of the videostream encoded. Can be adjusted dynamically in some codecs. + @param value Value of the property. + */ + CV_WRAP virtual bool set(int propId, double value); + + /** @brief Returns the specified VideoWriter property + + @param propId Property identifier. It can be one of the following: + - **VIDEOWRITER_PROP_QUALITY** Current quality of the encoded videostream. + - **VIDEOWRITER_PROP_FRAMEBYTES** (Read-only) Size of just encoded video frame; note that the encoding order may be different from representation order. + + **Note**: When querying a property that is not supported by the backend used by the VideoWriter + class, value 0 is returned. + */ + CV_WRAP virtual double get(int propId) const; + /** @brief Concatenates 4 chars to a fourcc code This static method constructs the fourcc code of the codec to be used in the constructor diff --git a/modules/videoio/src/cap.cpp b/modules/videoio/src/cap.cpp index 28470c6a5..b5a44da3f 100644 --- a/modules/videoio/src/cap.cpp +++ b/modules/videoio/src/cap.cpp @@ -705,6 +705,21 @@ bool VideoWriter::isOpened() const return !iwriter.empty() || !writer.empty(); } + +bool VideoWriter::set(int propId, double value) +{ + if (!iwriter.empty()) + return iwriter->setProperty(propId, value); + return false; +} + +double VideoWriter::get(int propId) const +{ + if (!iwriter.empty()) + return iwriter->getProperty(propId); + return 0.; +} + void VideoWriter::write(const Mat& image) { if( iwriter ) diff --git a/modules/videoio/src/cap_mjpeg_encoder.cpp b/modules/videoio/src/cap_mjpeg_encoder.cpp index 601aeb644..7856fd416 100644 --- a/modules/videoio/src/cap_mjpeg_encoder.cpp +++ b/modules/videoio/src/cap_mjpeg_encoder.cpp @@ -42,9 +42,8 @@ #include "precomp.hpp" #include -//#define WITH_NEON -#ifdef WITH_NEON -#include "arm_neon.h" +#if CV_NEON +#define WITH_NEON #endif namespace cv @@ -381,7 +380,7 @@ public: outfps = cvRound(fps); width = size.width; height = size.height; - quality = 8; + quality = 75; rawstream = false; channels = iscolor ? 3 : 1; @@ -592,12 +591,31 @@ public: } } + double getProperty(int propId) const + { + if( propId == VIDEOWRITER_PROP_QUALITY ) + return quality; + if( propId == VIDEOWRITER_PROP_FRAMEBYTES ) + return frameSize.empty() ? 0. : (double)frameSize.back(); + return 0.; + } + + bool setProperty(int propId, double value) + { + if( propId == VIDEOWRITER_PROP_QUALITY ) + { + quality = value; + return true; + } + return false; + } + void writeFrameData( const uchar* data, int step, int colorspace, int input_channels ); protected: int outfps; int width, height, channels; - int quality; + double quality; size_t moviPointer; std::vector frameOffset, frameSize, AVIChunkSizeIndex, frameNumIndexes; bool rawstream; @@ -1116,11 +1134,12 @@ void MotionJpegWriter::writeFrameData( const uchar* data, int step, int colorspa const int UV_step = 16; int u_plane_ofs = step*height; int v_plane_ofs = u_plane_ofs + step*height; + double _quality = quality*0.01*max_quality; - if( quality < 1 ) quality = 1; - if( quality > max_quality ) quality = max_quality; + if( _quality < 1. ) _quality = 1.; + if( _quality > max_quality ) _quality = max_quality; - double inv_quality = 1./quality; + double inv_quality = 1./_quality; // Encode header strm.putBytes( (const uchar*)jpegHeader, sizeof(jpegHeader) - 1 ); diff --git a/modules/videoio/src/precomp.hpp b/modules/videoio/src/precomp.hpp index 38623c7cf..c4662cccc 100644 --- a/modules/videoio/src/precomp.hpp +++ b/modules/videoio/src/precomp.hpp @@ -166,7 +166,7 @@ namespace cv public: virtual ~IVideoCapture() {} virtual double getProperty(int) const { return 0; } - virtual bool setProperty(int, double) { return 0; } + virtual bool setProperty(int, double) { return false; } virtual bool grabFrame() = 0; virtual bool retrieveFrame(int, OutputArray) = 0; virtual bool isOpened() const = 0; @@ -178,7 +178,7 @@ namespace cv public: virtual ~IVideoWriter() {} virtual double getProperty(int) const { return 0; } - virtual bool setProperty(int, double) { return 0; } + virtual bool setProperty(int, double) { return false; } virtual bool isOpened() const = 0; virtual void write(InputArray) = 0; From d326c7700746de78faa4ec5f538dd8d489abcd06 Mon Sep 17 00:00:00 2001 From: Alexander Alekhin Date: Mon, 30 Mar 2015 16:28:53 +0300 Subject: [PATCH 148/171] fix build (related to PR #3814) --- modules/photo/src/fast_nlmeans_denoising_invoker.hpp | 4 ++-- modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp index 6e74acf03..cfa9826ae 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker.hpp @@ -224,9 +224,9 @@ void FastNlMeansDenoisingInvoker::operator() (const Range& ra // calc weights IT estimation[pixelInfo::channels], weights_sum[pixelInfo::channels]; - for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) + for (int channel_num = 0; channel_num < pixelInfo::channels; channel_num++) estimation[channel_num] = 0; - for (size_t channel_num = 0; channel_num < pixelInfo::channels; channel_num++) + for (int channel_num = 0; channel_num < pixelInfo::channels; channel_num++) weights_sum[channel_num] = 0; for (int y = 0; y < search_window_size_; y++) diff --git a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp index 2322919ef..d8eb34417 100644 --- a/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp +++ b/modules/photo/src/fast_nlmeans_denoising_invoker_commons.hpp @@ -66,7 +66,7 @@ template struct pixelInfo_ > template struct pixelInfo: public pixelInfo_ { - using typename pixelInfo_::sampleType; + typedef typename pixelInfo_::sampleType sampleType; static inline sampleType sampleMax() { From 2e86cedef276d0955fff614f76f4970e2ad96a77 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Mon, 23 Feb 2015 15:01:00 +0100 Subject: [PATCH 149/171] Fix MinGW detection on x86 MinGW-w64 always uses "w64" as vendor key which the previously used check for "64" anywhere in the target triplet matched. This would lead to MinGW-w64 setups always being treated as x64. Turns out we do not even need this additional check since the architecture has been correctly determined earlier in this file. No need to do it again. --- cmake/OpenCVDetectCXXCompiler.cmake | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/cmake/OpenCVDetectCXXCompiler.cmake b/cmake/OpenCVDetectCXXCompiler.cmake index 72d939917..361178be0 100644 --- a/cmake/OpenCVDetectCXXCompiler.cmake +++ b/cmake/OpenCVDetectCXXCompiler.cmake @@ -147,11 +147,7 @@ if(MSVC) elseif(MINGW) set(OpenCV_RUNTIME mingw) - execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine - OUTPUT_VARIABLE OPENCV_GCC_TARGET_MACHINE - OUTPUT_STRIP_TRAILING_WHITESPACE) - if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "64") - set(MINGW64 1) + if(MINGW64) set(OpenCV_ARCH x64) else() set(OpenCV_ARCH x86) From 8f84a73b821d3edad445d29478d2ab31b03d0381 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Sat, 7 Mar 2015 15:37:21 +0100 Subject: [PATCH 150/171] Fix MinGW architecture detection Fix typo that would always lead to detection of x86 for MinGW builds in the OpenCVConfig.cmake file. --- cmake/OpenCVConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/OpenCVConfig.cmake b/cmake/OpenCVConfig.cmake index dfd7e8f26..b69b60ff5 100644 --- a/cmake/OpenCVConfig.cmake +++ b/cmake/OpenCVConfig.cmake @@ -87,7 +87,7 @@ elseif(MINGW) set(OpenCV_RUNTIME mingw) execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine - OUTPUT_VARIABLE OPENCV_GCC_TARGET_MACHINE + OUTPUT_VARIABLE CMAKE_OPENCV_GCC_TARGET_MACHINE OUTPUT_STRIP_TRAILING_WHITESPACE) if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "64") set(MINGW64 1) From 3fff0e5b3c771c3b8f6b78ca9ccb803b3b16ed6b Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Sat, 7 Mar 2015 15:49:48 +0100 Subject: [PATCH 151/171] Fix MinGW detection on x86 Make detection of x64 using the gcc's target triplet identical to the one used in cmake/OpenCVDetectCXXCompiler.cmake. Otherwise, MinGW-w64 setups will always be treated as x64 since they contain "w64" as vendor key. --- cmake/OpenCVConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/OpenCVConfig.cmake b/cmake/OpenCVConfig.cmake index b69b60ff5..cf45eb6ba 100644 --- a/cmake/OpenCVConfig.cmake +++ b/cmake/OpenCVConfig.cmake @@ -89,7 +89,7 @@ elseif(MINGW) execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine OUTPUT_VARIABLE CMAKE_OPENCV_GCC_TARGET_MACHINE OUTPUT_STRIP_TRAILING_WHITESPACE) - if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "64") + if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "amd64|x86_64|AMD64") set(MINGW64 1) set(OpenCV_ARCH x64) else() From 66e653d24d4f2c773f57d37d0705b86720cfe6d4 Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Tue, 17 Mar 2015 13:52:59 +0100 Subject: [PATCH 152/171] Remove local variables from CMAKE namespace --- cmake/OpenCVConfig.cmake | 4 ++-- cmake/OpenCVDetectCXXCompiler.cmake | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cmake/OpenCVConfig.cmake b/cmake/OpenCVConfig.cmake index cf45eb6ba..4f0e39858 100644 --- a/cmake/OpenCVConfig.cmake +++ b/cmake/OpenCVConfig.cmake @@ -87,9 +87,9 @@ elseif(MINGW) set(OpenCV_RUNTIME mingw) execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine - OUTPUT_VARIABLE CMAKE_OPENCV_GCC_TARGET_MACHINE + OUTPUT_VARIABLE OPENCV_GCC_TARGET_MACHINE OUTPUT_STRIP_TRAILING_WHITESPACE) - if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "amd64|x86_64|AMD64") + if(OPENCV_GCC_TARGET_MACHINE MATCHES "amd64|x86_64|AMD64") set(MINGW64 1) set(OpenCV_ARCH x64) else() diff --git a/cmake/OpenCVDetectCXXCompiler.cmake b/cmake/OpenCVDetectCXXCompiler.cmake index 361178be0..871331883 100644 --- a/cmake/OpenCVDetectCXXCompiler.cmake +++ b/cmake/OpenCVDetectCXXCompiler.cmake @@ -91,9 +91,9 @@ elseif(CMAKE_COMPILER_IS_GNUCXX) if(WIN32) execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine - OUTPUT_VARIABLE CMAKE_OPENCV_GCC_TARGET_MACHINE + OUTPUT_VARIABLE OPENCV_GCC_TARGET_MACHINE OUTPUT_STRIP_TRAILING_WHITESPACE) - if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "amd64|x86_64|AMD64") + if(OPENCV_GCC_TARGET_MACHINE MATCHES "amd64|x86_64|AMD64") set(MINGW64 1) endif() endif() From afd9de6f1bb51094c73ee8dfb1a3592243f9c4df Mon Sep 17 00:00:00 2001 From: Alexander Nitsch Date: Mon, 30 Mar 2015 23:38:43 +0200 Subject: [PATCH 153/171] Fix static/shared lib detection The check for BUILD_SHARED_LIBS had its logic backwards. If this variable is not defined we must assume a build of static libs. --- cmake/OpenCVConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/OpenCVConfig.cmake b/cmake/OpenCVConfig.cmake index 4f0e39858..09174b02f 100644 --- a/cmake/OpenCVConfig.cmake +++ b/cmake/OpenCVConfig.cmake @@ -47,7 +47,7 @@ endif() if(NOT DEFINED OpenCV_STATIC) # look for global setting - if(NOT DEFINED BUILD_SHARED_LIBS OR BUILD_SHARED_LIBS) + if(BUILD_SHARED_LIBS) set(OpenCV_STATIC OFF) else() set(OpenCV_STATIC ON) From 149c1c16ba1a0e76df88084caa99833a2ea75b99 Mon Sep 17 00:00:00 2001 From: Roman Donchenko Date: Wed, 9 Jul 2014 16:55:40 +0400 Subject: [PATCH 154/171] cap_libv4l.cpp depends on both libv4l 1 and 2, so search for both How this worked before, I do not know. --- CMakeLists.txt | 5 +++-- cmake/OpenCVFindLibsVideo.cmake | 8 +++++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d06ef1acc..746faac20 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -983,8 +983,9 @@ if(DEFINED WITH_V4L) else() set(HAVE_CAMV4L2_STR "NO") endif() - status(" V4L/V4L2:" HAVE_LIBV4L THEN "Using libv4l (ver ${ALIASOF_libv4l1_VERSION})" - ELSE "${HAVE_CAMV4L_STR}/${HAVE_CAMV4L2_STR}") + status(" V4L/V4L2:" HAVE_LIBV4L + THEN "Using libv4l1 (ver ${ALIASOF_libv4l1_VERSION}) / libv4l2 (ver ${ALIASOF_libv4l2_VERSION})" + ELSE "${HAVE_CAMV4L_STR}/${HAVE_CAMV4L2_STR}") endif(DEFINED WITH_V4L) if(DEFINED WITH_DSHOW) diff --git a/cmake/OpenCVFindLibsVideo.cmake b/cmake/OpenCVFindLibsVideo.cmake index 60135945d..1443c62ca 100644 --- a/cmake/OpenCVFindLibsVideo.cmake +++ b/cmake/OpenCVFindLibsVideo.cmake @@ -153,7 +153,13 @@ endif(WITH_XINE) ocv_clear_vars(HAVE_LIBV4L HAVE_CAMV4L HAVE_CAMV4L2 HAVE_VIDEOIO) if(WITH_V4L) if(WITH_LIBV4L) - CHECK_MODULE(libv4l1 HAVE_LIBV4L) + CHECK_MODULE(libv4l1 HAVE_LIBV4L1) + CHECK_MODULE(libv4l2 HAVE_LIBV4L2) + if(HAVE_LIBV4L1 AND HAVE_LIBV4L2) + set(HAVE_LIBV4L YES) + else() + set(HAVE_LIBV4L NO) + endif() endif() CHECK_INCLUDE_FILE(linux/videodev.h HAVE_CAMV4L) CHECK_INCLUDE_FILE(linux/videodev2.h HAVE_CAMV4L2) From 412a2aa4ab8d4e12378fbfeaea0e08d38c433fec Mon Sep 17 00:00:00 2001 From: Maxim Kostin Date: Tue, 31 Mar 2015 16:31:37 +0300 Subject: [PATCH 155/171] Added support for 'imgcodecs' module: - Resolved GET_ENV and input type incompatibility - Made libjpeg compile for WINRT. So does imgcodecs module. - Updated .gitignore (log, tlog) --- .gitignore | 7 ++++++- 3rdparty/libjpeg/CMakeLists.txt | 7 +++++++ 3rdparty/libtiff/CMakeLists.txt | 4 ++-- modules/imgcodecs/CMakeLists.txt | 4 ---- 4 files changed, 15 insertions(+), 7 deletions(-) diff --git a/.gitignore b/.gitignore index 228365d9e..039d2400a 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ Thumbs.db tags tegra/ bin/ -CMakeFiles/ *.sdf *.opensdf *.obj @@ -17,3 +16,9 @@ CMakeFiles/ *.depend *.rule *.tmp +*/debug +*/CMakeFiles +CMakeCache.txt +*.suo +*.log +*.tlog \ No newline at end of file diff --git a/3rdparty/libjpeg/CMakeLists.txt b/3rdparty/libjpeg/CMakeLists.txt index 65a9d1c8a..d79f00ada 100644 --- a/3rdparty/libjpeg/CMakeLists.txt +++ b/3rdparty/libjpeg/CMakeLists.txt @@ -15,6 +15,13 @@ else() ocv_list_filterout(lib_srcs jmemnobs.c) endif() +if(WINRT) + add_definitions(-DNO_GETENV) + get_directory_property( DirDefs COMPILE_DEFINITIONS ) + message(STATUS "Adding NO_GETENV to compiler definitions for WINRT:") + message(STATUS " COMPILE_DEFINITIONS = ${DirDefs}") +endif() + # ---------------------------------------------------------------------------------- # Define the library target: # ---------------------------------------------------------------------------------- diff --git a/3rdparty/libtiff/CMakeLists.txt b/3rdparty/libtiff/CMakeLists.txt index ad8a46618..b7739e0e4 100644 --- a/3rdparty/libtiff/CMakeLists.txt +++ b/3rdparty/libtiff/CMakeLists.txt @@ -17,7 +17,7 @@ check_include_file(string.h HAVE_STRING_H) check_include_file(sys/types.h HAVE_SYS_TYPES_H) check_include_file(unistd.h HAVE_UNISTD_H) -if(WIN32) +if(WIN32 AND NOT WINRT) set(USE_WIN32_FILEIO 1) endif() @@ -79,7 +79,7 @@ set(lib_srcs "${CMAKE_CURRENT_BINARY_DIR}/tif_config.h" ) -if(WIN32) +if(WIN32 AND NOT WINRT) list(APPEND lib_srcs tif_win32.c) else() list(APPEND lib_srcs tif_unix.c) diff --git a/modules/imgcodecs/CMakeLists.txt b/modules/imgcodecs/CMakeLists.txt index 50e2d5da6..6d565217a 100644 --- a/modules/imgcodecs/CMakeLists.txt +++ b/modules/imgcodecs/CMakeLists.txt @@ -1,7 +1,3 @@ -if(WINRT) - ocv_module_disable(imgcodecs) -endif() - set(the_description "Image codecs") ocv_add_module(imgcodecs opencv_imgproc WRAP java python) From 7c5084e385f1d7046a21a89cd6751c28d9785ba9 Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Wed, 25 Mar 2015 16:11:53 +0100 Subject: [PATCH 156/171] - update documentation * correct the part about fixed aspect * improve formatting * add snippet markers to source * replace inline code by @snippet - do not run calibration twice when using a imageList - make output consistent in itself (CamelCase vs no_camel_case) as well as with old camera calibrate sample - respect write extrinsic/ points flags - set the aspectRatio value when specified - fix writing of calibration settings. also update grammar. - fix intendation and remove some size_t -> int casts by using size_t --- .../camera_calibration.markdown | 266 ++---------------- .../camera_calibration/camera_calibration.cpp | 234 ++++++++------- 2 files changed, 159 insertions(+), 341 deletions(-) diff --git a/doc/tutorials/calib3d/camera_calibration/camera_calibration.markdown b/doc/tutorials/calib3d/camera_calibration/camera_calibration.markdown index 0b2364396..a7bd1f059 100644 --- a/doc/tutorials/calib3d/camera_calibration/camera_calibration.markdown +++ b/doc/tutorials/calib3d/camera_calibration/camera_calibration.markdown @@ -30,7 +30,7 @@ y_{corrected} = y + [ p_1(r^2+ 2y^2)+ 2p_2xy]\f] So we have five distortion parameters which in OpenCV are presented as one row matrix with 5 columns: -\f[Distortion_{coefficients}=(k_1 \hspace{10pt} k_2 \hspace{10pt} p_1 \hspace{10pt} p_2 \hspace{10pt} k_3)\f] +\f[distortion\_coefficients=(k_1 \hspace{10pt} k_2 \hspace{10pt} p_1 \hspace{10pt} p_2 \hspace{10pt} k_3)\f] Now for the unit conversion we use the following formula: @@ -96,83 +96,30 @@ on how to do this you can find in the @ref tutorial_file_input_output_with_xml_y Explanation ----------- --# **Read the settings.** - @code{.cpp} - Settings s; - const string inputSettingsFile = argc > 1 ? argv[1] : "default.xml"; - FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings - if (!fs.isOpened()) - { - cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl; - return -1; - } - fs["Settings"] >> s; - fs.release(); // close Settings file +-# **Read the settings** + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp file_read - if (!s.goodInput) - { - cout << "Invalid input detected. Application stopping. " << endl; - return -1; - } - @endcode For this I've used simple OpenCV class input operation. After reading the file I've an additional post-processing function that checks validity of the input. Only if all inputs are good then *goodInput* variable will be true. --# **Get next input, if it fails or we have enough of them - calibrate**. After this we have a big +-# **Get next input, if it fails or we have enough of them - calibrate** + + After this we have a big loop where we do the following operations: get the next image from the image list, camera or video file. If this fails or we have enough images then we run the calibration process. In case of image we step out of the loop and otherwise the remaining frames will be undistorted (if the option is set) via changing from *DETECTION* mode to the *CALIBRATED* one. - @code{.cpp} - for(int i = 0;;++i) - { - Mat view; - bool blinkOutput = false; - - view = s.nextImage(); - - //----- If no more image, or got enough, then stop calibration and show result ------------- - if( mode == CAPTURING && imagePoints.size() >= (unsigned)s.nrFrames ) - { - if( runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints)) - mode = CALIBRATED; - else - mode = DETECTION; - } - if(view.empty()) // If no more images then run calibration, save and stop loop. - { - if( imagePoints.size() > 0 ) - runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints); - break; - imageSize = view.size(); // Format input image. - if( s.flipVertical ) flip( view, view, 0 ); - } - @endcode + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp get_input For some cameras we may need to flip the input image. Here we do this too. --# **Find the pattern in the current input**. The formation of the equations I mentioned above aims +-# **Find the pattern in the current input** + + The formation of the equations I mentioned above aims to finding major patterns in the input: in case of the chessboard this are corners of the squares and for the circles, well, the circles themselves. The position of these will form the result which will be written into the *pointBuf* vector. - @code{.cpp} - vector pointBuf; - - bool found; - switch( s.calibrationPattern ) // Find feature points on the input format - { - case Settings::CHESSBOARD: - found = findChessboardCorners( view, s.boardSize, pointBuf, - CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE); - break; - case Settings::CIRCLES_GRID: - found = findCirclesGrid( view, s.boardSize, pointBuf ); - break; - case Settings::ASYMMETRIC_CIRCLES_GRID: - found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID ); - break; - } - @endcode + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp find_pattern Depending on the type of the input pattern you use either the @ref cv::findChessboardCorners or the @ref cv::findCirclesGrid function. For both of them you pass the current image and the size of the board and you'll get the positions of the patterns. Furthermore, they return a boolean @@ -188,109 +135,27 @@ Explanation *imagePoints* vector to collect all of the equations into a single container. Finally, for visualization feedback purposes we will draw the found points on the input image using @ref cv::findChessboardCorners function. - @code{.cpp} - if ( found) // If done with success, - { - // improve the found corners' coordinate accuracy for chessboard - if( s.calibrationPattern == Settings::CHESSBOARD) - { - Mat viewGray; - cvtColor(view, viewGray, COLOR_BGR2GRAY); - cornerSubPix( viewGray, pointBuf, Size(11,11), - Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::MAX_ITER, 30, 0.1 )); - } + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp pattern_found +-# **Show state and result to the user, plus command line control of the application** - if( mode == CAPTURING && // For camera only take new samples after delay time - (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) ) - { - imagePoints.push_back(pointBuf); - prevTimestamp = clock(); - blinkOutput = s.inputCapture.isOpened(); - } - - // Draw the corners. - drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found ); - } - @endcode --# **Show state and result to the user, plus command line control of the application**. This part - shows text output on the image. - @code{.cpp} - //----------------------------- Output Text ------------------------------------------------ - string msg = (mode == CAPTURING) ? "100/100" : - mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; - int baseLine = 0; - Size textSize = getTextSize(msg, 1, 1, 1, &baseLine); - Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10); - - if( mode == CAPTURING ) - { - if(s.showUndistorsed) - msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames ); - else - msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames ); - } - - putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED); - - if( blinkOutput ) - bitwise_not(view, view); - @endcode + This part shows text output on the image. + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp output_text If we ran calibration and got camera's matrix with the distortion coefficients we may want to correct the image using @ref cv::undistort function: - @code{.cpp} - //------------------------- Video capture output undistorted ------------------------------ - if( mode == CALIBRATED && s.showUndistorsed ) - { - Mat temp = view.clone(); - undistort(temp, view, cameraMatrix, distCoeffs); - } - //------------------------------ Show image and check for input commands ------------------- - imshow("Image View", view); - @endcode - Then we wait for an input key and if this is *u* we toggle the distortion removal, if it is *g* - we start again the detection process, and finally for the *ESC* key we quit the application: - @code{.cpp} - char key = waitKey(s.inputCapture.isOpened() ? 50 : s.delay); - if( key == ESC_KEY ) - break; + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp output_undistorted + Then we show the image and wait for an input key and if this is *u* we toggle the distortion removal, + if it is *g* we start again the detection process, and finally for the *ESC* key we quit the application: + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp await_input +-# **Show the distortion removal for the images too** - if( key == 'u' && mode == CALIBRATED ) - s.showUndistorsed = !s.showUndistorsed; - - if( s.inputCapture.isOpened() && key == 'g' ) - { - mode = CAPTURING; - imagePoints.clear(); - } - @endcode --# **Show the distortion removal for the images too**. When you work with an image list it is not + When you work with an image list it is not possible to remove the distortion inside the loop. Therefore, you must do this after the loop. Taking advantage of this now I'll expand the @ref cv::undistort function, which is in fact first calls @ref cv::initUndistortRectifyMap to find transformation matrices and then performs transformation using @ref cv::remap function. Because, after successful calibration map calculation needs to be done only once, by using this expanded form you may speed up your application: - @code{.cpp} - if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed ) - { - Mat view, rview, map1, map2; - initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), - getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), - imageSize, CV_16SC2, map1, map2); - - for(int i = 0; i < (int)s.imageList.size(); i++ ) - { - view = imread(s.imageList[i], 1); - if(view.empty()) - continue; - remap(view, rview, map1, map2, INTER_LINEAR); - imshow("Image View", rview); - char c = waitKey(); - if( c == ESC_KEY || c == 'q' || c == 'Q' ) - break; - } - } - @endcode + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp show_results The calibration and save ------------------------ @@ -304,24 +169,7 @@ Therefore in the first function we just split up these two processes. Because we of the calibration variables we'll create these variables here and pass on both of them to the calibration and saving function. Again, I'll not show the saving part as that has little in common with the calibration. Explore the source file in order to find out how and what: -@code{.cpp} -bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,vector > imagePoints ) -{ - vector rvecs, tvecs; - vector reprojErrs; - double totalAvgErr = 0; - - bool ok = runCalibration(s,imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, - reprojErrs, totalAvgErr); - cout << (ok ? "Calibration succeeded" : "Calibration failed") - << ". avg re projection error = " << totalAvgErr ; - - if( ok ) // save only if the calibration was done with success - saveCameraParams( s, imageSize, cameraMatrix, distCoeffs, rvecs ,tvecs, reprojErrs, - imagePoints, totalAvgErr); - return ok; -} -@endcode +@snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp run_and_save We do the calibration with the help of the @ref cv::calibrateCamera function. It has the following parameters: @@ -331,29 +179,7 @@ parameters: present. Because, we use a single pattern for all the input images we can calculate this just once and multiply it for all the other input views. We calculate the corner points with the *calcBoardCornerPositions* function as: - @code{.cpp} - void calcBoardCornerPositions(Size boardSize, float squareSize, vector& corners, - Settings::Pattern patternType /*= Settings::CHESSBOARD*/) - { - corners.clear(); - - switch(patternType) - { - case Settings::CHESSBOARD: - case Settings::CIRCLES_GRID: - for( int i = 0; i < boardSize.height; ++i ) - for( int j = 0; j < boardSize.width; ++j ) - corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0)); - break; - - case Settings::ASYMMETRIC_CIRCLES_GRID: - for( int i = 0; i < boardSize.height; i++ ) - for( int j = 0; j < boardSize.width; j++ ) - corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0)); - break; - } - } - @endcode + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp board_corners And then multiply it as: @code{.cpp} vector > objectPoints(1); @@ -365,12 +191,8 @@ parameters: circle pattern). We have already collected this from @ref cv::findChessboardCorners or @ref cv::findCirclesGrid function. We just need to pass it on. - The size of the image acquired from the camera, video file or the images. -- The camera matrix. If we used the fixed aspect ratio option we need to set the \f$f_x\f$ to zero: - @code{.cpp} - cameraMatrix = Mat::eye(3, 3, CV_64F); - if( s.flag & CALIB_FIX_ASPECT_RATIO ) - cameraMatrix.at(0,0) = 1.0; - @endcode +- The camera matrix. If we used the fixed aspect ratio option we need to set \f$f_x\f$: + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp fixed_aspect - The distortion coefficient matrix. Initialize with zero. @code{.cpp} distCoeffs = Mat::zeros(8, 1, CV_64F); @@ -393,33 +215,7 @@ double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, calculate the absolute norm between what we got with our transformation and the corner/circle finding algorithm. To find the average error we calculate the arithmetical mean of the errors calculated for all the calibration images. - @code{.cpp} - double computeReprojectionErrors( const vector >& objectPoints, - const vector >& imagePoints, - const vector& rvecs, const vector& tvecs, - const Mat& cameraMatrix , const Mat& distCoeffs, - vector& perViewErrors) - { - vector imagePoints2; - int i, totalPoints = 0; - double totalErr = 0, err; - perViewErrors.resize(objectPoints.size()); - - for( i = 0; i < (int)objectPoints.size(); ++i ) - { - projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix, // project - distCoeffs, imagePoints2); - err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2); // difference - - int n = (int)objectPoints[i].size(); - perViewErrors[i] = (float) std::sqrt(err*err/n); // save for this view - totalErr += err*err; // sum it up - totalPoints += n; - } - - return std::sqrt(totalErr/totalPoints); // calculate the arithmetical mean - } - @endcode + @snippet samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp compute_errors Results ------- @@ -461,20 +257,20 @@ the input. Here's, how a detected pattern should look: In both cases in the specified output XML/YAML file you'll find the camera and distortion coefficients matrices: @code{.xml} - + 3 3
d
6.5746697944293521e+002 0. 3.1950000000000000e+002 0. - 6.5746697944293521e+002 2.3950000000000000e+002 0. 0. 1.
- + 6.5746697944293521e+002 2.3950000000000000e+002 0. 0. 1.
+ 5 1
d
-4.1802327176423804e-001 5.0715244063187526e-001 0. 0. - -5.7843597214487474e-001
+ -5.7843597214487474e-001 @endcode Add these values as constants to your program, call the @ref cv::initUndistortRectifyMap and the @ref cv::remap function to remove distortion and enjoy distortion free inputs for cheap and low diff --git a/samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp b/samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp index 34e2504c6..8059a4aec 100644 --- a/samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp +++ b/samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp @@ -34,7 +34,8 @@ public: void write(FileStorage& fs) const //Write serialization for this class { - fs << "{" << "BoardSize_Width" << boardSize.width + fs << "{" + << "BoardSize_Width" << boardSize.width << "BoardSize_Height" << boardSize.height << "Square_Size" << squareSize << "Calibrate_Pattern" << patternToUse @@ -43,8 +44,8 @@ public: << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint - << "Write_DetectedFeaturePoints" << bwritePoints - << "Write_extrinsicParameters" << bwriteExtrinsics + << "Write_DetectedFeaturePoints" << writePoints + << "Write_extrinsicParameters" << writeExtrinsics << "Write_outputFileName" << outputFileName << "Show_UndistortedImage" << showUndistorsed @@ -62,8 +63,8 @@ public: node["Square_Size"] >> squareSize; node["Calibrate_NrOfFrameToUse"] >> nrFrames; node["Calibrate_FixAspectRatio"] >> aspectRatio; - node["Write_DetectedFeaturePoints"] >> bwritePoints; - node["Write_extrinsicParameters"] >> bwriteExtrinsics; + node["Write_DetectedFeaturePoints"] >> writePoints; + node["Write_extrinsicParameters"] >> writeExtrinsics; node["Write_outputFileName"] >> outputFileName; node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist; node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint; @@ -71,9 +72,9 @@ public: node["Show_UndistortedImage"] >> showUndistorsed; node["Input"] >> input; node["Input_Delay"] >> delay; - interprate(); + validate(); } - void interprate() + void validate() { goodInput = true; if (boardSize.width <= 0 || boardSize.height <= 0) @@ -105,10 +106,10 @@ public: else { if (readStringList(input, imageList)) - { - inputType = IMAGE_LIST; - nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size(); - } + { + inputType = IMAGE_LIST; + nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size(); + } else inputType = VIDEO_FILE; } @@ -121,7 +122,7 @@ public: } if (inputType == INVALID) { - cerr << " Inexistent input: " << input; + cerr << " Input does not exist: " << input; goodInput = false; } @@ -136,10 +137,10 @@ public: if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID; if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID; if (calibrationPattern == NOT_EXISTING) - { - cerr << " Inexistent camera calibration mode: " << patternToUse << endl; - goodInput = false; - } + { + cerr << " Camera calibration mode does not exist: " << patternToUse << endl; + goodInput = false; + } atImageList = 0; } @@ -152,7 +153,7 @@ public: inputCapture >> view0; view0.copyTo(result); } - else if( atImageList < (int)imageList.size() ) + else if( atImageList < imageList.size() ) result = imread(imageList[atImageList++], IMREAD_COLOR); return result; @@ -173,26 +174,24 @@ public: return true; } public: - Size boardSize; // The size of the board -> Number of items by width and height - Pattern calibrationPattern;// One of the Chessboard, circles, or asymmetric circle pattern - float squareSize; // The size of a square in your defined unit (point, millimeter,etc). - int nrFrames; // The number of frames to use from the input for calibration - float aspectRatio; // The aspect ratio - int delay; // In case of a video input - bool bwritePoints; // Write detected feature points - bool bwriteExtrinsics; // Write extrinsic parameters - bool calibZeroTangentDist; // Assume zero tangential distortion - bool calibFixPrincipalPoint;// Fix the principal point at the center - bool flipVertical; // Flip the captured images around the horizontal axis - string outputFileName; // The name of the file where to write - bool showUndistorsed; // Show undistorted images after calibration - string input; // The input -> - - + Size boardSize; // The size of the board -> Number of items by width and height + Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern + float squareSize; // The size of a square in your defined unit (point, millimeter,etc). + int nrFrames; // The number of frames to use from the input for calibration + float aspectRatio; // The aspect ratio + int delay; // In case of a video input + bool writePoints; // Write detected feature points + bool writeExtrinsics; // Write extrinsic parameters + bool calibZeroTangentDist; // Assume zero tangential distortion + bool calibFixPrincipalPoint; // Fix the principal point at the center + bool flipVertical; // Flip the captured images around the horizontal axis + string outputFileName; // The name of the file where to write + bool showUndistorsed; // Show undistorted images after calibration + string input; // The input -> int cameraID; vector imageList; - int atImageList; + size_t atImageList; VideoCapture inputCapture; InputType inputType; bool goodInput; @@ -204,7 +203,7 @@ private: }; -static void read(const FileNode& node, Settings& x, const Settings& default_value = Settings()) +static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings()) { if(node.empty()) x = default_value; @@ -212,6 +211,11 @@ static void read(const FileNode& node, Settings& x, const Settings& default_valu x.read(node); } +static inline void write(FileStorage& fs, const String&, const Settings& s ) +{ + s.write(fs); +} + enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, @@ -220,6 +224,8 @@ bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& int main(int argc, char* argv[]) { help(); + + //! [file_read] Settings s; const string inputSettingsFile = argc > 1 ? argv[1] : "default.xml"; FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings @@ -230,6 +236,10 @@ int main(int argc, char* argv[]) } fs["Settings"] >> s; fs.release(); // close Settings file + //! [file_read] + + //FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML + //fout << "Settings" << s; if (!s.goodInput) { @@ -245,32 +255,35 @@ int main(int argc, char* argv[]) const Scalar RED(0,0,255), GREEN(0,255,0); const char ESC_KEY = 27; - for(int i = 0;;++i) + //! [get_input] + for(;;) { - Mat view; - bool blinkOutput = false; + Mat view; + bool blinkOutput = false; - view = s.nextImage(); + view = s.nextImage(); - //----- If no more image, or got enough, then stop calibration and show result ------------- - if( mode == CAPTURING && imagePoints.size() >= (unsigned)s.nrFrames ) - { + //----- If no more image, or got enough, then stop calibration and show result ------------- + if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames ) + { if( runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints)) mode = CALIBRATED; else mode = DETECTION; - } - if(view.empty()) // If no more images then run calibration, save and stop loop. - { - if( imagePoints.size() > 0 ) + } + if(view.empty()) // If there are no more images stop the loop + { + // if calibration threshold was not reached yet, calibrate now + if( mode != CALIBRATED && !imagePoints.empty() ) runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints); break; - } - + } + //! [get_input] imageSize = view.size(); // Format input image. if( s.flipVertical ) flip( view, view, 0 ); + //! [find_pattern] vector pointBuf; bool found; @@ -290,7 +303,8 @@ int main(int argc, char* argv[]) found = false; break; } - + //! [find_pattern] + //! [pattern_found] if ( found) // If done with success, { // improve the found corners' coordinate accuracy for chessboard @@ -313,8 +327,9 @@ int main(int argc, char* argv[]) // Draw the corners. drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found ); } - + //! [pattern_found] //----------------------------- Output Text ------------------------------------------------ + //! [output_text] string msg = (mode == CAPTURING) ? "100/100" : mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; int baseLine = 0; @@ -333,15 +348,17 @@ int main(int argc, char* argv[]) if( blinkOutput ) bitwise_not(view, view); - + //! [output_text] //------------------------- Video capture output undistorted ------------------------------ + //! [output_undistorted] if( mode == CALIBRATED && s.showUndistorsed ) { Mat temp = view.clone(); undistort(temp, view, cameraMatrix, distCoeffs); } - + //! [output_undistorted] //------------------------------ Show image and check for input commands ------------------- + //! [await_input] imshow("Image View", view); char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay); @@ -356,9 +373,11 @@ int main(int argc, char* argv[]) mode = CAPTURING; imagePoints.clear(); } + //! [await_input] } // -----------------------Show the undistorted image for the image list ------------------------ + //! [show_results] if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed ) { Mat view, rview, map1, map2; @@ -366,7 +385,7 @@ int main(int argc, char* argv[]) getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize, CV_16SC2, map1, map2); - for(int i = 0; i < (int)s.imageList.size(); i++ ) + for(size_t i = 0; i < s.imageList.size(); i++ ) { view = imread(s.imageList[i], 1); if(view.empty()) @@ -378,11 +397,12 @@ int main(int argc, char* argv[]) break; } } - + //! [show_results] return 0; } +//! [compute_errors] static double computeReprojectionErrors( const vector >& objectPoints, const vector >& imagePoints, const vector& rvecs, const vector& tvecs, @@ -390,17 +410,16 @@ static double computeReprojectionErrors( const vector >& objectP vector& perViewErrors) { vector imagePoints2; - int i, totalPoints = 0; + size_t totalPoints = 0; double totalErr = 0, err; perViewErrors.resize(objectPoints.size()); - for( i = 0; i < (int)objectPoints.size(); ++i ) + for(size_t i = 0; i < objectPoints.size(); ++i ) { - projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix, - distCoeffs, imagePoints2); - err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2); + projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2); + err = norm(imagePoints[i], imagePoints2, NORM_L2); - int n = (int)objectPoints[i].size(); + size_t n = objectPoints[i].size(); perViewErrors[i] = (float) std::sqrt(err*err/n); totalErr += err*err; totalPoints += n; @@ -408,7 +427,8 @@ static double computeReprojectionErrors( const vector >& objectP return std::sqrt(totalErr/totalPoints); } - +//! [compute_errors] +//! [board_corners] static void calcBoardCornerPositions(Size boardSize, float squareSize, vector& corners, Settings::Pattern patternType /*= Settings::CHESSBOARD*/) { @@ -420,28 +440,28 @@ static void calcBoardCornerPositions(Size boardSize, float squareSize, vector > imagePoints, vector& rvecs, vector& tvecs, vector& reprojErrs, double& totalAvgErr) { - + //! [fixed_aspect] cameraMatrix = Mat::eye(3, 3, CV_64F); if( s.flag & CALIB_FIX_ASPECT_RATIO ) - cameraMatrix.at(0,0) = 1.0; - + cameraMatrix.at(0,0) = s.aspectRatio; + //! [fixed_aspect] distCoeffs = Mat::zeros(8, 1, CV_64F); vector > objectPoints(1); @@ -475,49 +495,48 @@ static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, M time( &tm ); struct tm *t2 = localtime( &tm ); char buf[1024]; - strftime( buf, sizeof(buf)-1, "%c", t2 ); + strftime( buf, sizeof(buf), "%c", t2 ); - fs << "calibration_Time" << buf; + fs << "calibration_time" << buf; if( !rvecs.empty() || !reprojErrs.empty() ) - fs << "nrOfFrames" << (int)std::max(rvecs.size(), reprojErrs.size()); - fs << "image_Width" << imageSize.width; - fs << "image_Height" << imageSize.height; - fs << "board_Width" << s.boardSize.width; - fs << "board_Height" << s.boardSize.height; - fs << "square_Size" << s.squareSize; + fs << "nr_of_frames" << (int)std::max(rvecs.size(), reprojErrs.size()); + fs << "image_width" << imageSize.width; + fs << "image_height" << imageSize.height; + fs << "board_width" << s.boardSize.width; + fs << "board_height" << s.boardSize.height; + fs << "square_size" << s.squareSize; if( s.flag & CALIB_FIX_ASPECT_RATIO ) - fs << "FixAspectRatio" << s.aspectRatio; + fs << "fix_aspect_ratio" << s.aspectRatio; - if( s.flag ) + if (s.flag) { - sprintf( buf, "flags: %s%s%s%s", - s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "", - s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "", - s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "", - s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "" ); - //cvWriteComment( *fs, buf, 0 ); - + sprintf(buf, "flags: %s%s%s%s", + s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "", + s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspect_ratio" : "", + s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "", + s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : ""); + cvWriteComment(*fs, buf, 0); } - fs << "flagValue" << s.flag; + fs << "flags" << s.flag; - fs << "Camera_Matrix" << cameraMatrix; - fs << "Distortion_Coefficients" << distCoeffs; + fs << "camera_matrix" << cameraMatrix; + fs << "distortion_coefficients" << distCoeffs; - fs << "Avg_Reprojection_Error" << totalAvgErr; - if( !reprojErrs.empty() ) - fs << "Per_View_Reprojection_Errors" << Mat(reprojErrs); + fs << "avg_reprojection_error" << totalAvgErr; + if (s.writeExtrinsics && !reprojErrs.empty()) + fs << "per_view_reprojection_errors" << Mat(reprojErrs); - if( !rvecs.empty() && !tvecs.empty() ) + if(s.writeExtrinsics && !rvecs.empty() && !tvecs.empty() ) { CV_Assert(rvecs[0].type() == tvecs[0].type()); Mat bigmat((int)rvecs.size(), 6, rvecs[0].type()); - for( int i = 0; i < (int)rvecs.size(); i++ ) + for( size_t i = 0; i < rvecs.size(); i++ ) { - Mat r = bigmat(Range(i, i+1), Range(0,3)); - Mat t = bigmat(Range(i, i+1), Range(3,6)); + Mat r = bigmat(Range(int(i), int(i+1)), Range(0,3)); + Mat t = bigmat(Range(int(i), int(i+1)), Range(3,6)); CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1); CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1); @@ -526,35 +545,38 @@ static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, M t = tvecs[i].t(); } //cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 ); - fs << "Extrinsic_Parameters" << bigmat; + fs << "extrinsic_parameters" << bigmat; } - if( !imagePoints.empty() ) + if(s.writePoints && !imagePoints.empty() ) { Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2); - for( int i = 0; i < (int)imagePoints.size(); i++ ) + for( size_t i = 0; i < imagePoints.size(); i++ ) { - Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols); + Mat r = imagePtMat.row(int(i)).reshape(2, imagePtMat.cols); Mat imgpti(imagePoints[i]); imgpti.copyTo(r); } - fs << "Image_points" << imagePtMat; + fs << "image_points" << imagePtMat; } } -bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,vector > imagePoints ) +//! [run_and_save] +bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, + vector > imagePoints) { vector rvecs, tvecs; vector reprojErrs; double totalAvgErr = 0; - bool ok = runCalibration(s,imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, - reprojErrs, totalAvgErr); + bool ok = runCalibration(s, imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs, + totalAvgErr); cout << (ok ? "Calibration succeeded" : "Calibration failed") - << ". avg re projection error = " << totalAvgErr ; + << ". avg re projection error = " << totalAvgErr << endl; - if( ok ) - saveCameraParams( s, imageSize, cameraMatrix, distCoeffs, rvecs ,tvecs, reprojErrs, - imagePoints, totalAvgErr); + if (ok) + saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, imagePoints, + totalAvgErr); return ok; } +//! [run_and_save] From 75850a13512efb8b1b5fd2ecc94d011e9e255b1e Mon Sep 17 00:00:00 2001 From: Maxim Kostin Date: Tue, 31 Mar 2015 17:09:09 +0300 Subject: [PATCH 157/171] Added sample imgcodecs logic to winrt sample app - Made use imread() in OcvImageProcessing sample. - Added test/sample logic to verify file creation via imgcodecs. Signed-off-by: Maxim Kostin --- .../OcvImageProcessing/MainPage.xaml.cpp | 84 ++++++++++++++++++- .../OcvImageProcessing/MainPage.xaml.h | 5 ++ .../OcvImageProcessing/opencv.props | 5 +- 3 files changed, 90 insertions(+), 4 deletions(-) diff --git a/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.cpp b/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.cpp index 2e91eb156..fc7440fb2 100644 --- a/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.cpp +++ b/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.cpp @@ -10,6 +10,10 @@ #include #include #include +#include +#include + +#include using namespace OcvImageProcessing; @@ -18,6 +22,7 @@ using namespace concurrency; using namespace Platform; using namespace Windows::Foundation; using namespace Windows::Storage::Streams; +using namespace Windows::Storage; using namespace Windows::UI::Xaml::Media::Imaging; using namespace Windows::Graphics::Imaging; using namespace Windows::Foundation::Collections; @@ -37,6 +42,17 @@ MainPage::MainPage() { InitializeComponent(); +#ifdef __OPENCV_IMGCODECS_HPP__ + + // Image loading OpenCV way ... way more simple + cv::Mat image = cv::imread("Assets/Lena.png"); + Lena = cv::Mat(image.rows, image.cols, CV_8UC4); + cvtColor(image, Lena, CV_BGR2BGRA); + UpdateImage(Lena); + +#else + + // Image loading WinRT way RandomAccessStreamReference^ streamRef = RandomAccessStreamReference::CreateFromUri(InputImageUri); task (streamRef->OpenReadAsync()). @@ -68,6 +84,67 @@ MainPage::MainPage() memcpy(Lena.data, srcPixels->Data, 4*frameWidth*frameHeight); UpdateImage(Lena); }); + +#endif +} + +/// +/// Temporary file creation example. Will be created in WinRT application temporary directory +/// which usually is "C:\Users\{username}\AppData\Local\Packages\{package_id}\TempState\{random_name}.{suffix}" +/// +/// Temporary file suffix, e.g. "tmp" +std::string OcvImageProcessing::MainPage::CreateTempFile(const std::string &suffix) { + return cv::tempfile(suffix.c_str()); +} + +/// +/// Creating/writing a file in the application local directory +/// +/// Image to save +bool OcvImageProcessing::MainPage::SaveImage(cv::Mat image) { + StorageFolder^ localFolderRT = ApplicationData::Current->LocalFolder; + cv::String localFile = ConvertPath(ApplicationData::Current->LocalFolder->Path) + "\\Lena.png"; + + return cv::imwrite(localFile, image); +} + +/// +/// Getting std::string from managed string via std::wstring. +/// Provides an example of three ways to do it. +/// Can't use this one: https://msdn.microsoft.com/en-us/library/bb384865.aspx, not available on WinRT. +/// +/// Path to be converted +cv::String OcvImageProcessing::MainPage::ConvertPath(Platform::String^ path) { + std::wstring localPathW(path->Begin()); + + // Opt #1 + //std::string localPath(localPathW.begin(), localPathW.end()); + + // Opt #2 + //std::string localPath(StrToWStr(localPathW)); + + // Opt #3 + size_t outSize = localPathW.length() + 1; + char* localPathC = new char[outSize]; + size_t charsConverted = 0; + wcstombs_s(&charsConverted, localPathC, outSize, localPathW.c_str(), localPathW.length()); + cv::String localPath(localPathC); + + // Implicit conversion from std::string to cv::String + return localPath; +} + +std::string OcvImageProcessing::MainPage::StrToWStr(const std::wstring &input) { + if (input.empty()) { + return std::string(); + } + + int size = WideCharToMultiByte(CP_UTF8, 0, &input[0], (int)input.size(), NULL, 0, NULL, NULL); + std::string result(size, 0); + + WideCharToMultiByte(CP_UTF8, 0, &input[0], (int)input.size(), &result[0], size, NULL, NULL); + + return result; } /// @@ -91,15 +168,16 @@ void OcvImageProcessing::MainPage::UpdateImage(const cv::Mat& image) // Obtain IBufferByteAccess ComPtr pBufferByteAccess; - ComPtr pBuffer((IUnknown*)buffer); + ComPtr pBuffer((IInspectable*)buffer); pBuffer.As(&pBufferByteAccess); // Get pointer to pixel bytes pBufferByteAccess->Buffer(&dstPixels); - memcpy(dstPixels, image.data, 4*image.cols*image.rows); + memcpy(dstPixels, image.data, image.step.buf[1]*image.cols*image.rows); // Set the bitmap to the Image element - PreviewWidget->Source = bitmap;} + PreviewWidget->Source = bitmap; +} cv::Mat OcvImageProcessing::MainPage::ApplyGrayFilter(const cv::Mat& image) diff --git a/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.h b/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.h index 79c1ac74c..bb7c4c33d 100644 --- a/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.h +++ b/samples/winrt/OcvImageProcessing/OcvImageProcessing/MainPage.xaml.h @@ -39,6 +39,11 @@ namespace OcvImageProcessing cv::Mat ApplySepiaFilter(const cv::Mat& image); void UpdateImage(const cv::Mat& image); + std::string CreateTempFile(const std::string &suffix); + bool SaveImage(cv::Mat image); + + std::string StrToWStr(const std::wstring &wstr); + cv::String ConvertPath(Platform::String^ path); cv::Mat Lena; unsigned int frameWidth, frameHeight; diff --git a/samples/winrt/OcvImageProcessing/OcvImageProcessing/opencv.props b/samples/winrt/OcvImageProcessing/OcvImageProcessing/opencv.props index 40eaffd1f..64b0ac98a 100644 --- a/samples/winrt/OcvImageProcessing/OcvImageProcessing/opencv.props +++ b/samples/winrt/OcvImageProcessing/OcvImageProcessing/opencv.props @@ -17,6 +17,9 @@ true + + true + true @@ -33,7 +36,7 @@ - opencv_core300$(DebugSuffix).lib;opencv_imgproc300$(DebugSuffix).lib;opencv_features2d300$(DebugSuffix).lib;opencv_flann300$(DebugSuffix).lib;opencv_ml300$(DebugSuffix).lib;%(AdditionalDependencies) + opencv_core300$(DebugSuffix).lib;opencv_imgproc300$(DebugSuffix).lib;opencv_features2d300$(DebugSuffix).lib;opencv_flann300$(DebugSuffix).lib;opencv_ml300$(DebugSuffix).lib;opencv_imgcodecs300$(DebugSuffix).lib;%(AdditionalDependencies) $(OpenCV_Lib);%(AdditionalLibraryDirectories); From 1f04f9d63be678e37c9123cd70c4c75a0b133956 Mon Sep 17 00:00:00 2001 From: Evgeny Agafonchikov Date: Tue, 24 Mar 2015 15:52:49 +0300 Subject: [PATCH 158/171] Fixing help messages --help message and starting run.py w/o parameters generate different messages E. g. w/ and w/o build_path in the end build_path is required, removing square brackets --- modules/ts/misc/run.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/ts/misc/run.py b/modules/ts/misc/run.py index 19ab2ab7f..5b4d362c3 100755 --- a/modules/ts/misc/run.py +++ b/modules/ts/misc/run.py @@ -854,7 +854,7 @@ if __name__ == "__main__": test_args = [a for a in sys.argv if a.startswith("--perf_") or a.startswith("--gtest_")] argv = [a for a in sys.argv if not(a.startswith("--perf_") or a.startswith("--gtest_"))] - parser = OptionParser() + parser = OptionParser(usage="run.py [options] build_path") parser.add_option("-t", "--tests", dest="tests", help="comma-separated list of modules to test", metavar="SUITS", default="") parser.add_option("-w", "--cwd", dest="cwd", help="working directory for tests", metavar="PATH", default=".") parser.add_option("-a", "--accuracy", dest="accuracy", help="look for accuracy tests instead of performance tests", action="store_true", default=False) @@ -879,7 +879,7 @@ if __name__ == "__main__": run_args = getRunArgs(args[1:] or ['.']) if len(run_args) == 0: - print >> sys.stderr, "Usage:", os.path.basename(sys.argv[0]), "[options] [build_path]" + print >> sys.stderr, "Usage:", os.path.basename(sys.argv[0]), "[options] build_path" exit(1) options.android_env = {} From ae090fe10d0f668bf574e67e8b32a1b1bfd76099 Mon Sep 17 00:00:00 2001 From: Evgeny Agafonchikov Date: Fri, 27 Mar 2015 16:50:18 +0300 Subject: [PATCH 159/171] Functionality for relocation of tests This allows to start run.py with --move_tests parameter to place tests to another location and keep them runnable from there --- modules/ts/misc/run.py | 48 ++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 46 insertions(+), 2 deletions(-) diff --git a/modules/ts/misc/run.py b/modules/ts/misc/run.py index 5b4d362c3..f25922d94 100755 --- a/modules/ts/misc/run.py +++ b/modules/ts/misc/run.py @@ -850,12 +850,51 @@ def getRunArgs(args): path = npath return run_args +if hostos == "nt": + def moveTests(instance, destination): + src = os.path.dirname(instance.tests_dir) + # new binaries path + newBinPath = os.path.join(destination, "bin") + + try: + # copy binaries and CMakeCache.txt to the specified destination + shutil.copytree(src, newBinPath) + shutil.copy(os.path.join(instance.path, "CMakeCache.txt"), os.path.join(destination, "CMakeCache.txt")) + except Exception, e: + print "Copying error occurred:", str(e) + exit(e.errno) + + # pattern of CMakeCache.txt string to be replaced + replacePattern = re.compile("EXECUTABLE_OUTPUT_PATH:PATH=(.+)") + + with open(os.path.join(destination, "CMakeCache.txt"), "r") as cachefile: + try: + cachedata = cachefile.read() + if hostos == 'nt': + # fix path slashes on nt systems + newBinPath = re.sub(r"\\", r"/", newBinPath) + # replace old binaries path in CMakeCache.txt + cachedata = re.sub(re.search(replacePattern, cachedata).group(1), newBinPath, cachedata) + except Exception, e: + print "Reading error occurred:", str(e) + exit(e.errno) + + with open(os.path.join(destination, "CMakeCache.txt"), "w") as cachefile: + try: + cachefile.write(cachedata) + except Exception, e: + print "Writing error occurred:", str(e) + exit(e.errno) + exit() + if __name__ == "__main__": test_args = [a for a in sys.argv if a.startswith("--perf_") or a.startswith("--gtest_")] argv = [a for a in sys.argv if not(a.startswith("--perf_") or a.startswith("--gtest_"))] - parser = OptionParser(usage="run.py [options] build_path") + parser = OptionParser(usage="run.py [options] [build_path]", description="Note: build_path is required if running not from CMake build directory") parser.add_option("-t", "--tests", dest="tests", help="comma-separated list of modules to test", metavar="SUITS", default="") + if hostos == "nt": + parser.add_option("-m", "--move_tests", dest="move", help="location to move current tests build", metavar="PATH", default="") parser.add_option("-w", "--cwd", dest="cwd", help="working directory for tests", metavar="PATH", default=".") parser.add_option("-a", "--accuracy", dest="accuracy", help="look for accuracy tests instead of performance tests", action="store_true", default=False) parser.add_option("-l", "--longname", dest="useLongNames", action="store_true", help="generate log files with long names", default=False) @@ -879,7 +918,8 @@ if __name__ == "__main__": run_args = getRunArgs(args[1:] or ['.']) if len(run_args) == 0: - print >> sys.stderr, "Usage:", os.path.basename(sys.argv[0]), "[options] build_path" + print >> sys.stderr, "Usage:", os.path.basename(sys.argv[0]), "[options] [build_path]" + print >> sys.stderr, "Please specify build_path or run script from CMake build directory" exit(1) options.android_env = {} @@ -906,6 +946,10 @@ if __name__ == "__main__": test_list = [] for path in run_args: suite = TestSuite(options, path) + + if hostos == "nt": + if(options.move): + moveTests(suite, options.move) #print vars(suite),"\n" if options.list: test_list.extend(suite.tests) From 952f9dbe629e8e9f310c70c78ed46d484439c1c9 Mon Sep 17 00:00:00 2001 From: Dikay900 Date: Wed, 1 Apr 2015 20:22:27 +0200 Subject: [PATCH 160/171] initialize tiltedOffset variable at correct location do not use this variable if it is not an tilted feature --- modules/objdetect/src/cascadedetect.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/modules/objdetect/src/cascadedetect.cpp b/modules/objdetect/src/cascadedetect.cpp index 46a1d60f2..bc1fedc80 100644 --- a/modules/objdetect/src/cascadedetect.cpp +++ b/modules/objdetect/src/cascadedetect.cpp @@ -618,8 +618,7 @@ Ptr HaarEvaluator::clone() const void HaarEvaluator::computeChannels(int scaleIdx, InputArray img) { const ScaleData& s = scaleData->at(scaleIdx); - tofs = (int)sbufSize.area(); - sqofs = hasTiltedFeatures ? tofs*2 : tofs; + sqofs = hasTiltedFeatures ? sbufSize.area() * 2 : sbufSize.area(); if (img.isUMat()) { @@ -660,6 +659,9 @@ void HaarEvaluator::computeChannels(int scaleIdx, InputArray img) void HaarEvaluator::computeOptFeatures() { + if (hasTiltedFeatures) + tofs = sbufSize.area(); + int sstep = sbufSize.width; CV_SUM_OFS( nofs[0], nofs[1], nofs[2], nofs[3], 0, normrect, sstep ); From 6593422c05e85f41063f32b7799d6412caa17ed7 Mon Sep 17 00:00:00 2001 From: Sancho McCann Date: Wed, 1 Apr 2015 19:00:39 +0000 Subject: [PATCH 161/171] Bugfix: #4030 SVM auto-training. --- modules/ml/src/svm.cpp | 6 +- modules/ml/test/test_svmtrainauto.cpp | 89 +++++++++++++++++++++++++++ 2 files changed, 92 insertions(+), 3 deletions(-) create mode 100644 modules/ml/test/test_svmtrainauto.cpp diff --git a/modules/ml/src/svm.cpp b/modules/ml/src/svm.cpp index 8bed11763..449eb8dcd 100644 --- a/modules/ml/src/svm.cpp +++ b/modules/ml/src/svm.cpp @@ -1669,13 +1669,13 @@ public: Mat samples = data->getTrainSamples(); Mat responses; bool is_classification = false; - Mat class_labels0 = class_labels; int class_count = (int)class_labels.total(); if( svmType == C_SVC || svmType == NU_SVC ) { responses = data->getTrainNormCatResponses(); class_labels = data->getClassLabels(); + class_count = (int)class_labels.total(); is_classification = true; vector temp_class_labels; @@ -1755,8 +1755,9 @@ public: Mat temp_train_responses(train_sample_count, 1, rtype); Mat temp_test_responses; + // If grid.minVal == grid.maxVal, this will allow one and only one pass through the loop with params.var = grid.minVal. #define FOR_IN_GRID(var, grid) \ - for( params.var = grid.minVal; params.var == grid.minVal || params.var < grid.maxVal; params.var *= grid.logStep ) + for( params.var = grid.minVal; params.var == grid.minVal || params.var < grid.maxVal; params.var = (grid.minVal == grid.maxVal) ? grid.maxVal + 1 : params.var * grid.logStep ) FOR_IN_GRID(C, C_grid) FOR_IN_GRID(gamma, gamma_grid) @@ -1814,7 +1815,6 @@ public: } params = best_params; - class_labels = class_labels0; return do_train( samples, responses ); } diff --git a/modules/ml/test/test_svmtrainauto.cpp b/modules/ml/test/test_svmtrainauto.cpp new file mode 100644 index 000000000..918d2b711 --- /dev/null +++ b/modules/ml/test/test_svmtrainauto.cpp @@ -0,0 +1,89 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// 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. +// +// +// Intel License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// 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 Intel Corporation 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. +// +//M*/ + +#include "test_precomp.hpp" + +using namespace cv; +using namespace std; +using cv::ml::SVM; +using cv::ml::TrainData; + +//-------------------------------------------------------------------------------------------- +class CV_SVMTrainAutoTest : public cvtest::BaseTest { +public: + CV_SVMTrainAutoTest() {} +protected: + virtual void run( int start_from ); +}; + +void CV_SVMTrainAutoTest::run( int /*start_from*/ ) +{ + int datasize = 100; + cv::Mat samples = cv::Mat::zeros( datasize, 2, CV_32FC1 ); + cv::Mat responses = cv::Mat::zeros( datasize, 1, CV_32S ); + + RNG rng(0); + for (int i = 0; i < datasize; ++i) + { + int response = rng.uniform(0, 2); // Random from {0, 1}. + samples.at( i, 0 ) = rng.uniform(0.f, 0.5f) + response * 0.5f; + samples.at( i, 1 ) = rng.uniform(0.f, 0.5f) + response * 0.5f; + responses.at( i, 0 ) = response; + } + + cv::Ptr data = TrainData::create( samples, cv::ml::ROW_SAMPLE, responses ); + cv::Ptr svm = SVM::create(); + svm->trainAuto( data, 10 ); // 2-fold cross validation. + + float test_data0[2] = {0.25f, 0.25f}; + cv::Mat test_point0 = cv::Mat( 1, 2, CV_32FC1, test_data0 ); + float result0 = svm->predict( test_point0 ); + float test_data1[2] = {0.75f, 0.75f}; + cv::Mat test_point1 = cv::Mat( 1, 2, CV_32FC1, test_data1 ); + float result1 = svm->predict( test_point1 ); + + if ( fabs( result0 - 0 ) > 0.001 || fabs( result1 - 1 ) > 0.001 ) + { + ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY ); + } +} + +TEST(ML_SVM, trainauto) { CV_SVMTrainAutoTest test; test.safe_run(); } From 0df9dc8fb9aa5bda198fac4e747b1a8ea769e005 Mon Sep 17 00:00:00 2001 From: Simon Heinen Date: Thu, 2 Apr 2015 08:42:19 +0200 Subject: [PATCH 162/171] Update android+AsyncServiceHelper.java changed tabs in last commit to spaces --- .../generator/src/java/android+AsyncServiceHelper.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/java/generator/src/java/android+AsyncServiceHelper.java b/modules/java/generator/src/java/android+AsyncServiceHelper.java index b7c57f28c..e1f884235 100644 --- a/modules/java/generator/src/java/android+AsyncServiceHelper.java +++ b/modules/java/generator/src/java/android+AsyncServiceHelper.java @@ -21,11 +21,11 @@ class AsyncServiceHelper final LoaderCallbackInterface Callback) { AsyncServiceHelper helper = new AsyncServiceHelper(Version, AppContext, Callback); - Intent intent = new Intent("org.opencv.engine.BIND"); - intent.setPackage("org.opencv.engine"); - if (AppContext.bindService(intent, helper.mServiceConnection, - Context.BIND_AUTO_CREATE)) { - return true; + Intent intent = new Intent("org.opencv.engine.BIND"); + intent.setPackage("org.opencv.engine"); + if (AppContext.bindService(intent, helper.mServiceConnection, Context.BIND_AUTO_CREATE)) + { + return true; } else { From d2dc7f4c27b745561d5483ee1f2ef3b7a12fa2a0 Mon Sep 17 00:00:00 2001 From: Simon Heinen Date: Thu, 2 Apr 2015 09:05:56 +0200 Subject: [PATCH 163/171] Update android+AsyncServiceHelper.java small formatting fixes --- .../java/generator/src/java/android+AsyncServiceHelper.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/java/generator/src/java/android+AsyncServiceHelper.java b/modules/java/generator/src/java/android+AsyncServiceHelper.java index e1f884235..e18d5a500 100644 --- a/modules/java/generator/src/java/android+AsyncServiceHelper.java +++ b/modules/java/generator/src/java/android+AsyncServiceHelper.java @@ -23,9 +23,9 @@ class AsyncServiceHelper AsyncServiceHelper helper = new AsyncServiceHelper(Version, AppContext, Callback); Intent intent = new Intent("org.opencv.engine.BIND"); intent.setPackage("org.opencv.engine"); - if (AppContext.bindService(intent, helper.mServiceConnection, Context.BIND_AUTO_CREATE)) + if (AppContext.bindService(intent, helper.mServiceConnection, Context.BIND_AUTO_CREATE)) { - return true; + return true; } else { From 7e35f76d06185d7ea53e1ff4d904b826c97c7407 Mon Sep 17 00:00:00 2001 From: StevenPuttemans Date: Fri, 6 Mar 2015 11:52:26 +0100 Subject: [PATCH 164/171] allowing people to manually define how sharp a cascade classifier model should be trained --- apps/traincascade/cascadeclassifier.cpp | 13 ++++++++++--- apps/traincascade/cascadeclassifier.h | 3 ++- apps/traincascade/traincascade.cpp | 9 ++++++++- doc/user_guide/ug_traincascade.markdown | 6 ++++++ 4 files changed, 26 insertions(+), 5 deletions(-) diff --git a/apps/traincascade/cascadeclassifier.cpp b/apps/traincascade/cascadeclassifier.cpp index c9b524f5e..a64566ee4 100644 --- a/apps/traincascade/cascadeclassifier.cpp +++ b/apps/traincascade/cascadeclassifier.cpp @@ -135,7 +135,8 @@ bool CvCascadeClassifier::train( const string _cascadeDirName, const CvCascadeParams& _cascadeParams, const CvFeatureParams& _featureParams, const CvCascadeBoostParams& _stageParams, - bool baseFormatSave ) + bool baseFormatSave, + double acceptanceRatioBreakValue ) { // Start recording clock ticks for training time output const clock_t begin_time = clock(); @@ -185,6 +186,7 @@ bool CvCascadeClassifier::train( const string _cascadeDirName, cout << "numStages: " << numStages << endl; cout << "precalcValBufSize[Mb] : " << _precalcValBufSize << endl; cout << "precalcIdxBufSize[Mb] : " << _precalcIdxBufSize << endl; + cout << "acceptanceRatioBreakValue : " << acceptanceRatioBreakValue << endl; cascadeParams.printAttrs(); stageParams->printAttrs(); featureParams->printAttrs(); @@ -207,13 +209,18 @@ bool CvCascadeClassifier::train( const string _cascadeDirName, if ( !updateTrainingSet( tempLeafFARate ) ) { cout << "Train dataset for temp stage can not be filled. " - "Branch training terminated." << endl; + "Branch training terminated." << endl; break; } if( tempLeafFARate <= requiredLeafFARate ) { cout << "Required leaf false alarm rate achieved. " - "Branch training terminated." << endl; + "Branch training terminated." << endl; + break; + } + if( (tempLeafFARate <= acceptanceRatioBreakValue) && (acceptanceRatioBreakValue < 0) ){ + cout << "The required acceptanceRatio for the model has been reached to avoid overfitting of trainingdata. " + "Branch training terminated." << endl; break; } diff --git a/apps/traincascade/cascadeclassifier.h b/apps/traincascade/cascadeclassifier.h index 6d6cb5b3f..d8e044828 100644 --- a/apps/traincascade/cascadeclassifier.h +++ b/apps/traincascade/cascadeclassifier.h @@ -94,7 +94,8 @@ public: const CvCascadeParams& _cascadeParams, const CvFeatureParams& _featureParams, const CvCascadeBoostParams& _stageParams, - bool baseFormatSave = false ); + bool baseFormatSave = false, + double acceptanceRatioBreakValue = -1.0 ); private: int predict( int sampleIdx ); void save( const std::string cascadeDirName, bool baseFormat = false ); diff --git a/apps/traincascade/traincascade.cpp b/apps/traincascade/traincascade.cpp index d1c3e4e87..ba8afb27c 100644 --- a/apps/traincascade/traincascade.cpp +++ b/apps/traincascade/traincascade.cpp @@ -15,6 +15,7 @@ int main( int argc, char* argv[] ) int precalcValBufSize = 256, precalcIdxBufSize = 256; bool baseFormatSave = false; + double acceptanceRatioBreakValue = -1.0; CvCascadeParams cascadeParams; CvCascadeBoostParams stageParams; @@ -36,6 +37,7 @@ int main( int argc, char* argv[] ) cout << " [-precalcIdxBufSize ]" << endl; cout << " [-baseFormatSave]" << endl; cout << " [-numThreads ]" << endl; + cout << " [-acceptanceRatioBreakValue = " << acceptanceRatioBreakValue << ">]" << endl; cascadeParams.printDefaults(); stageParams.printDefaults(); for( int fi = 0; fi < fc; fi++ ) @@ -86,6 +88,10 @@ int main( int argc, char* argv[] ) { numThreads = atoi(argv[++i]); } + else if( !strcmp( argv[i], "-acceptanceRatioBreakValue" ) ) + { + acceptanceRatioBreakValue = atof(argv[++i]); + } else if ( cascadeParams.scanAttr( argv[i], argv[i+1] ) ) { i++; } else if ( stageParams.scanAttr( argv[i], argv[i+1] ) ) { i++; } else if ( !set ) @@ -112,6 +118,7 @@ int main( int argc, char* argv[] ) cascadeParams, *featureParams[cascadeParams.featureType], stageParams, - baseFormatSave ); + baseFormatSave, + acceptanceRatioBreakValue ); return 0; } diff --git a/doc/user_guide/ug_traincascade.markdown b/doc/user_guide/ug_traincascade.markdown index 059d25e8a..ccceb5051 100644 --- a/doc/user_guide/ug_traincascade.markdown +++ b/doc/user_guide/ug_traincascade.markdown @@ -256,6 +256,12 @@ Command line arguments of opencv_traincascade application grouped by purposes: Maximum number of threads to use during training. Notice that the actual number of used threads may be lower, depending on your machine and compilation options. + - -acceptanceRatioBreakValue \ + + This argument is used to determine how precise your model should keep learning and when to stop. + A good guideline is to train not further than 10e-5, to ensure the model does not overtrain on your training data. + By default this value is set to -1 to disable this feature. + -# Cascade parameters: - -stageType \ From 796c15d3e663332d01dd3ef20f56ac5497f1cc4a Mon Sep 17 00:00:00 2001 From: StevenPuttemans Date: Fri, 3 Apr 2015 12:54:05 +0200 Subject: [PATCH 165/171] fixing wrong equation in accuracy break rule --- apps/traincascade/cascadeclassifier.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/traincascade/cascadeclassifier.cpp b/apps/traincascade/cascadeclassifier.cpp index a64566ee4..8b3eb57ac 100644 --- a/apps/traincascade/cascadeclassifier.cpp +++ b/apps/traincascade/cascadeclassifier.cpp @@ -218,7 +218,7 @@ bool CvCascadeClassifier::train( const string _cascadeDirName, "Branch training terminated." << endl; break; } - if( (tempLeafFARate <= acceptanceRatioBreakValue) && (acceptanceRatioBreakValue < 0) ){ + if( (tempLeafFARate <= acceptanceRatioBreakValue) && (acceptanceRatioBreakValue >= 0) ){ cout << "The required acceptanceRatio for the model has been reached to avoid overfitting of trainingdata. " "Branch training terminated." << endl; break; From ed9f933d41bc6fd9fa40b42d1ca370f1b75d3e96 Mon Sep 17 00:00:00 2001 From: Andrey Pavlenko Date: Fri, 3 Apr 2015 14:03:09 +0300 Subject: [PATCH 166/171] fixing confusing variable naming in a sample code --- modules/imgcodecs/include/opencv2/imgcodecs.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/modules/imgcodecs/include/opencv2/imgcodecs.hpp b/modules/imgcodecs/include/opencv2/imgcodecs.hpp index b0c942172..30846efea 100644 --- a/modules/imgcodecs/include/opencv2/imgcodecs.hpp +++ b/modules/imgcodecs/include/opencv2/imgcodecs.hpp @@ -185,13 +185,14 @@ compression parameters : void createAlphaMat(Mat &mat) { + CV_Assert(mat.channels() == 4); for (int i = 0; i < mat.rows; ++i) { for (int j = 0; j < mat.cols; ++j) { - Vec4b& rgba = mat.at(i, j); - rgba[0] = UCHAR_MAX; - rgba[1] = saturate_cast((float (mat.cols - j)) / ((float)mat.cols) * UCHAR_MAX); - rgba[2] = saturate_cast((float (mat.rows - i)) / ((float)mat.rows) * UCHAR_MAX); - rgba[3] = saturate_cast(0.5 * (rgba[1] + rgba[2])); + Vec4b& bgra = mat.at(i, j); + bgra[0] = UCHAR_MAX; // Blue + bgra[1] = saturate_cast((float (mat.cols - j)) / ((float)mat.cols) * UCHAR_MAX); // Green + bgra[2] = saturate_cast((float (mat.rows - i)) / ((float)mat.rows) * UCHAR_MAX); // Red + bgra[3] = saturate_cast(0.5 * (bgra[1] + bgra[2])); // Alpha } } } From 58f20a3b7f2bc2aa3043c4ddbafe8548602d8ac0 Mon Sep 17 00:00:00 2001 From: Andrew Senin Date: Fri, 3 Apr 2015 22:49:50 +0300 Subject: [PATCH 167/171] Ximea camera fix (see issue #4235) --- modules/videoio/src/cap_ximea.cpp | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/modules/videoio/src/cap_ximea.cpp b/modules/videoio/src/cap_ximea.cpp index 8356b4d92..ccf49e45b 100644 --- a/modules/videoio/src/cap_ximea.cpp +++ b/modules/videoio/src/cap_ximea.cpp @@ -52,7 +52,15 @@ CvCapture* cvCreateCameraCapture_XIMEA( int index ) // Enumerate connected devices void CvCaptureCAM_XIMEA::init() { +#if defined WIN32 || defined _WIN32 xiGetNumberDevices( &numDevices); +#else + // try second re-enumeration if first one fails + if (xiGetNumberDevices( &numDevices) != XI_OK) + { + xiGetNumberDevices( &numDevices); + } +#endif hmv = NULL; frame = NULL; timeout = 0; @@ -73,8 +81,17 @@ bool CvCaptureCAM_XIMEA::open( int wIndex ) if((mvret = xiOpenDevice( wIndex, &hmv)) != XI_OK) { +#if defined WIN32 || defined _WIN32 errMsg("Open XI_DEVICE failed", mvret); return false; +#else + // try opening second time if first fails + if((mvret = xiOpenDevice( wIndex, &hmv)) != XI_OK) + { + errMsg("Open XI_DEVICE failed", mvret); + return false; + } +#endif } int width = 0; @@ -260,7 +277,7 @@ double CvCaptureCAM_XIMEA::getProperty( int property_id ) const case CV_CAP_PROP_XI_AUTO_WB : xiGetParamInt( hmv, XI_PRM_AUTO_WB, &ival); return ival; case CV_CAP_PROP_XI_AEAG : xiGetParamInt( hmv, XI_PRM_AEAG, &ival); return ival; case CV_CAP_PROP_XI_EXP_PRIORITY : xiGetParamFloat( hmv, XI_PRM_EXP_PRIORITY, &fval); return fval; - case CV_CAP_PROP_XI_AE_MAX_LIMIT : xiGetParamInt( hmv, XI_PRM_AE_MAX_LIMIT, &ival); return ival; + case CV_CAP_PROP_XI_AE_MAX_LIMIT : xiGetParamInt( hmv, XI_PRM_EXP_PRIORITY, &ival); return ival; case CV_CAP_PROP_XI_AG_MAX_LIMIT : xiGetParamFloat( hmv, XI_PRM_AG_MAX_LIMIT, &fval); return fval; case CV_CAP_PROP_XI_AEAG_LEVEL : xiGetParamInt( hmv, XI_PRM_AEAG_LEVEL, &ival); return ival; case CV_CAP_PROP_XI_TIMEOUT : return timeout; @@ -293,7 +310,7 @@ bool CvCaptureCAM_XIMEA::setProperty( int property_id, double value ) case CV_CAP_PROP_XI_OFFSET_Y : mvret = xiSetParamInt( hmv, XI_PRM_OFFSET_Y, ival); break; case CV_CAP_PROP_XI_TRG_SOURCE : mvret = xiSetParamInt( hmv, XI_PRM_TRG_SOURCE, ival); break; case CV_CAP_PROP_XI_GPI_SELECTOR : mvret = xiSetParamInt( hmv, XI_PRM_GPI_SELECTOR, ival); break; - case CV_CAP_PROP_XI_TRG_SOFTWARE : mvret = xiSetParamInt( hmv, XI_PRM_TRG_SOFTWARE, 1); break; + case CV_CAP_PROP_XI_TRG_SOFTWARE : mvret = xiSetParamInt( hmv, XI_PRM_TRG_SOURCE, 1); break; case CV_CAP_PROP_XI_GPI_MODE : mvret = xiSetParamInt( hmv, XI_PRM_GPI_MODE, ival); break; case CV_CAP_PROP_XI_GPI_LEVEL : mvret = xiSetParamInt( hmv, XI_PRM_GPI_LEVEL, ival); break; case CV_CAP_PROP_XI_GPO_SELECTOR : mvret = xiSetParamInt( hmv, XI_PRM_GPO_SELECTOR, ival); break; @@ -301,10 +318,10 @@ bool CvCaptureCAM_XIMEA::setProperty( int property_id, double value ) case CV_CAP_PROP_XI_LED_SELECTOR : mvret = xiSetParamInt( hmv, XI_PRM_LED_SELECTOR, ival); break; case CV_CAP_PROP_XI_LED_MODE : mvret = xiSetParamInt( hmv, XI_PRM_LED_MODE, ival); break; case CV_CAP_PROP_XI_AUTO_WB : mvret = xiSetParamInt( hmv, XI_PRM_AUTO_WB, ival); break; - case CV_CAP_PROP_XI_MANUAL_WB : mvret = xiSetParamInt( hmv, XI_PRM_MANUAL_WB, ival); break; + case CV_CAP_PROP_XI_MANUAL_WB : mvret = xiSetParamInt( hmv, XI_PRM_LED_MODE, ival); break; case CV_CAP_PROP_XI_AEAG : mvret = xiSetParamInt( hmv, XI_PRM_AEAG, ival); break; case CV_CAP_PROP_XI_EXP_PRIORITY : mvret = xiSetParamFloat( hmv, XI_PRM_EXP_PRIORITY, fval); break; - case CV_CAP_PROP_XI_AE_MAX_LIMIT : mvret = xiSetParamInt( hmv, XI_PRM_AE_MAX_LIMIT, ival); break; + case CV_CAP_PROP_XI_AE_MAX_LIMIT : mvret = xiSetParamInt( hmv, XI_PRM_EXP_PRIORITY, ival); break; case CV_CAP_PROP_XI_AG_MAX_LIMIT : mvret = xiSetParamFloat( hmv, XI_PRM_AG_MAX_LIMIT, fval); break; case CV_CAP_PROP_XI_AEAG_LEVEL : mvret = xiSetParamInt( hmv, XI_PRM_AEAG_LEVEL, ival); break; case CV_CAP_PROP_XI_TIMEOUT : timeout = ival; break; From 03ea24f298f90934d71fcbabe4d134286ff34d6e Mon Sep 17 00:00:00 2001 From: Ashod Nakashian Date: Wed, 11 Feb 2015 17:49:19 -0500 Subject: [PATCH 168/171] Fix for decoding large Jp2 images on Windows. On Windows, the tmpnam function returns a temp filename in the current directory, which has a prepended backslash '\\'. This subsequently fails the open function. This patch creates a proper temp filename in the temp folder and makes unlike work by opening the file as short-lived. --- 3rdparty/libjasper/jas_stream.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/3rdparty/libjasper/jas_stream.c b/3rdparty/libjasper/jas_stream.c index ca1239c7d..4c5db5871 100644 --- a/3rdparty/libjasper/jas_stream.c +++ b/3rdparty/libjasper/jas_stream.c @@ -365,10 +365,14 @@ jas_stream_t *jas_stream_tmpfile() #ifdef _WIN32 /* Choose a file name. */ - tmpnam(obj->pathname); + char lpTempPathBuffer[MAX_PATH]; + const DWORD dwRetVal = GetTempPath(MAX_PATH, lpTempPathBuffer); /* Open the underlying file. */ - if ((obj->fd = open(obj->pathname, O_CREAT | O_EXCL | O_RDWR | O_TRUNC | O_BINARY, + if (dwRetVal >= MAX_PATH || dwRetVal == 0 || + sprintf_s(obj->pathname, MAX_PATH, "%s\\tmp.XXXXXXXXXX", lpTempPathBuffer) <= 0 || + _mktemp_s(obj->pathname, MAX_PATH) || + (obj->fd = open(obj->pathname, O_CREAT | O_EXCL | O_RDWR | O_TRUNC | O_BINARY | O_TEMPORARY | _O_SHORT_LIVED, JAS_STREAM_PERMS)) < 0) { jas_stream_destroy(stream); return 0; From 6fd2fdc2e6fe7670b39c2728e06b57db230a441d Mon Sep 17 00:00:00 2001 From: Ashod Nakashian Date: Wed, 11 Feb 2015 21:28:44 -0500 Subject: [PATCH 169/171] Jasper unit-tests and removed a superfluous assertion. --- 3rdparty/libjasper/jas_cm.c | 1 - modules/imgcodecs/test/test_grfmt.cpp | 13 +++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/3rdparty/libjasper/jas_cm.c b/3rdparty/libjasper/jas_cm.c index dc23ead89..16d4a502d 100644 --- a/3rdparty/libjasper/jas_cm.c +++ b/3rdparty/libjasper/jas_cm.c @@ -842,7 +842,6 @@ static int jas_cmshapmat_apply(jas_cmpxform_t *pxform, jas_cmreal_t *in, *dst++ = a2; } } else { -assert(0); while (--cnt >= 0) { a0 = *src++; src++; diff --git a/modules/imgcodecs/test/test_grfmt.cpp b/modules/imgcodecs/test/test_grfmt.cpp index 423d030a0..5ef0164fe 100644 --- a/modules/imgcodecs/test/test_grfmt.cpp +++ b/modules/imgcodecs/test/test_grfmt.cpp @@ -87,6 +87,9 @@ TEST(Imgcodecs_imread, regression) { const char* const filenames[] = { +#ifdef HAVE_JASPER + "Rome.jp2", +#endif "color_palette_alpha.png", "multipage.tif", "rle.hdr", @@ -109,6 +112,16 @@ TEST(Imgcodecs_imread, regression) } } +#ifdef HAVE_JASPER +TEST(Imgcodecs_jasper, regression) +{ + const string folder = string(cvtest::TS::ptr()->get_data_path()) + "/readwrite/"; + + ASSERT_TRUE(imread_compare(folder + "Bretagne2.jp2", IMREAD_UNCHANGED)); + ASSERT_TRUE(imread_compare(folder + "Bretagne2.jp2", IMREAD_GRAYSCALE)); +} +#endif + class CV_GrfmtWriteBigImageTest : public cvtest::BaseTest { public: From 54ab3137d5c417a6060abf1ae85a31abaf23fe32 Mon Sep 17 00:00:00 2001 From: Ashod Nakashian Date: Sun, 29 Mar 2015 16:49:47 -0400 Subject: [PATCH 170/171] Simplified temp filename generation. --- 3rdparty/libjasper/jas_stream.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/3rdparty/libjasper/jas_stream.c b/3rdparty/libjasper/jas_stream.c index 4c5db5871..3ba7a837d 100644 --- a/3rdparty/libjasper/jas_stream.c +++ b/3rdparty/libjasper/jas_stream.c @@ -345,6 +345,7 @@ jas_stream_t *jas_stream_tmpfile() { jas_stream_t *stream; jas_stream_fileobj_t *obj; + char *tmpname; if (!(stream = jas_stream_create())) { return 0; @@ -365,14 +366,12 @@ jas_stream_t *jas_stream_tmpfile() #ifdef _WIN32 /* Choose a file name. */ - char lpTempPathBuffer[MAX_PATH]; - const DWORD dwRetVal = GetTempPath(MAX_PATH, lpTempPathBuffer); + tmpname = tempnam(NULL, NULL); + strcpy(obj->pathname, tmpname); + free(tmpname); /* Open the underlying file. */ - if (dwRetVal >= MAX_PATH || dwRetVal == 0 || - sprintf_s(obj->pathname, MAX_PATH, "%s\\tmp.XXXXXXXXXX", lpTempPathBuffer) <= 0 || - _mktemp_s(obj->pathname, MAX_PATH) || - (obj->fd = open(obj->pathname, O_CREAT | O_EXCL | O_RDWR | O_TRUNC | O_BINARY | O_TEMPORARY | _O_SHORT_LIVED, + if ((obj->fd = open(obj->pathname, O_CREAT | O_EXCL | O_RDWR | O_TRUNC | O_BINARY | O_TEMPORARY | _O_SHORT_LIVED, JAS_STREAM_PERMS)) < 0) { jas_stream_destroy(stream); return 0; From f75f2ffd48a4256dcb347cd79e241c07d9e3b419 Mon Sep 17 00:00:00 2001 From: Ashod Nakashian Date: Fri, 3 Apr 2015 20:59:13 -0400 Subject: [PATCH 171/171] Jpeg2k color to greyscale conversion on non-Windows is done post decoding because system libjasper segfaults when decoding color images as greyscale. --- modules/imgcodecs/src/grfmt_jpeg2000.cpp | 25 +++++++++++++++++++++++- modules/imgcodecs/src/loadsave.cpp | 9 +-------- modules/imgcodecs/test/test_grfmt.cpp | 22 +++++++++++++-------- 3 files changed, 39 insertions(+), 17 deletions(-) diff --git a/modules/imgcodecs/src/grfmt_jpeg2000.cpp b/modules/imgcodecs/src/grfmt_jpeg2000.cpp index 83fd55a59..e499c58b8 100644 --- a/modules/imgcodecs/src/grfmt_jpeg2000.cpp +++ b/modules/imgcodecs/src/grfmt_jpeg2000.cpp @@ -45,6 +45,7 @@ #ifdef HAVE_JASPER #include "grfmt_jpeg2000.hpp" +#include "opencv2/imgproc.hpp" #ifdef WIN32 #define JAS_WIN_MSVC_BUILD 1 @@ -159,6 +160,21 @@ bool Jpeg2KDecoder::readData( Mat& img ) jas_stream_t* stream = (jas_stream_t*)m_stream; jas_image_t* image = (jas_image_t*)m_image; +#ifndef WIN32 + // At least on some Linux instances the + // system libjasper segfaults when + // converting color to grey. + // We do this conversion manually at the end. + Mat clr; + if (CV_MAT_CN(img.type()) < CV_MAT_CN(this->type())) + { + clr.create(img.size().height, img.size().width, this->type()); + color = true; + data = clr.ptr(); + step = (int)clr.step; + } +#endif + if( stream && image ) { bool convert; @@ -171,7 +187,7 @@ bool Jpeg2KDecoder::readData( Mat& img ) else { convert = (jas_clrspc_fam( jas_image_clrspc( image ) ) != JAS_CLRSPC_FAM_GRAY); - colorspace = JAS_CLRSPC_SGRAY; // TODO GENGRAY or SGRAY? + colorspace = JAS_CLRSPC_SGRAY; // TODO GENGRAY or SGRAY? (GENGRAY fails on Win.) } // convert to the desired colorspace @@ -256,6 +272,13 @@ bool Jpeg2KDecoder::readData( Mat& img ) close(); +#ifndef WIN32 + if (!clr.empty()) + { + cv::cvtColor(clr, img, COLOR_BGR2GRAY); + } +#endif + return result; } diff --git a/modules/imgcodecs/src/loadsave.cpp b/modules/imgcodecs/src/loadsave.cpp index 8526a4a3f..383c25a2b 100644 --- a/modules/imgcodecs/src/loadsave.cpp +++ b/modules/imgcodecs/src/loadsave.cpp @@ -374,15 +374,8 @@ imreadmulti_(const String& filename, int flags, std::vector& mats) type = CV_MAKETYPE(CV_MAT_DEPTH(type), 1); } - // established the required input image size. - CvSize size; - size.width = decoder->width(); - size.height = decoder->height(); - - Mat mat; - mat.create(size.height, size.width, type); - // read the image data + Mat mat(decoder->height(), decoder->width(), type); if (!decoder->readData(mat)) { break; diff --git a/modules/imgcodecs/test/test_grfmt.cpp b/modules/imgcodecs/test/test_grfmt.cpp index 5ef0164fe..92238a95f 100644 --- a/modules/imgcodecs/test/test_grfmt.cpp +++ b/modules/imgcodecs/test/test_grfmt.cpp @@ -102,13 +102,17 @@ TEST(Imgcodecs_imread, regression) for (size_t i = 0; i < sizeof(filenames) / sizeof(filenames[0]); ++i) { - ASSERT_TRUE(imread_compare(folder + string(filenames[i]), IMREAD_UNCHANGED)); - ASSERT_TRUE(imread_compare(folder + string(filenames[i]), IMREAD_GRAYSCALE)); - ASSERT_TRUE(imread_compare(folder + string(filenames[i]), IMREAD_COLOR)); - ASSERT_TRUE(imread_compare(folder + string(filenames[i]), IMREAD_ANYDEPTH)); - ASSERT_TRUE(imread_compare(folder + string(filenames[i]), IMREAD_ANYCOLOR)); - if (i != 2) // GDAL does not support hdr - ASSERT_TRUE(imread_compare(folder + string(filenames[i]), IMREAD_LOAD_GDAL)); + const string path = folder + string(filenames[i]); + ASSERT_TRUE(imread_compare(path, IMREAD_UNCHANGED)); + ASSERT_TRUE(imread_compare(path, IMREAD_GRAYSCALE)); + ASSERT_TRUE(imread_compare(path, IMREAD_COLOR)); + ASSERT_TRUE(imread_compare(path, IMREAD_ANYDEPTH)); + ASSERT_TRUE(imread_compare(path, IMREAD_ANYCOLOR)); + if (path.substr(path.length() - 3) != "hdr") + { + // GDAL does not support hdr + ASSERT_TRUE(imread_compare(path, IMREAD_LOAD_GDAL)); + } } } @@ -117,8 +121,10 @@ TEST(Imgcodecs_jasper, regression) { const string folder = string(cvtest::TS::ptr()->get_data_path()) + "/readwrite/"; - ASSERT_TRUE(imread_compare(folder + "Bretagne2.jp2", IMREAD_UNCHANGED)); + ASSERT_TRUE(imread_compare(folder + "Bretagne2.jp2", IMREAD_COLOR)); ASSERT_TRUE(imread_compare(folder + "Bretagne2.jp2", IMREAD_GRAYSCALE)); + ASSERT_TRUE(imread_compare(folder + "Grey.jp2", IMREAD_COLOR)); + ASSERT_TRUE(imread_compare(folder + "Grey.jp2", IMREAD_GRAYSCALE)); } #endif