From: Olexa Bilaniuk Date: Thu, 15 Jan 2015 09:21:16 +0000 (-0500) Subject: Further LevMarq improvements. X-Git-Tag: accepted/tizen/6.0/unified/20201030.111113~2570^2~45 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=02124f19e6a445b0712495f69f829afb849214fe;p=platform%2Fupstream%2Fopencv.git 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. --- diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index 79beb3e..1fafc14 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; } /**