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

@@ -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;k<npoints;k++){
         tempMask.data[k] = !!tempMask.data[k];
     }
+    tempMask.copyTo(_tempMask);
 
     return result;
 }

modules/calib3d/src/rhorefc.cpp

@@ -72,6 +72,8 @@
 #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(). */
 
 
 
@@ -140,9 +142,11 @@ static inline void   sacCalcJacobianErrors(const float* restrict H,
                                            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 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]);
@@ -1571,27 +1575,15 @@ 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 (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;i<MAXLEVMARQITERS;i++){
@@ -1626,37 +1617,42 @@ static inline void   sacRefine(RHO_HEST_REFC* p){
          * 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);*/
+            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(L<FLT_EPSILON){
-                L = 1;
-            }
-        }else{
-            L *= 2.0;
-            if(L>1.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;
 }
 
 /**