Skip to content

O
opencv
Commit ff91af82 authored by Olexa Bilaniuk's avatar Olexa Bilaniuk
Browse files
Options

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.
parent 8a518266
Hide whitespace changes
Inline Side-by-side
Showing with 73 additions and 42 deletions
modules/calib3d/src/rhorefc.cpp
View file @ ff91af82
...@@ -66,7 +66,7 @@ ...@@ -66,7 +66,7 @@
#define CHI_SQ 1.645 #define CHI_SQ 1.645
#define RLO 0.25 #define RLO 0.25
#define RHI 0.75 #define RHI 0.75
#define MAXLEVMARQITERS 50 #define MAXLEVMARQITERS 100
#define m 4 /* 4 points required per model */ #define m 4 /* 4 points required per model */
#define SPRT_T_M 25 /* Guessing 25 match evlauations / 1 model generation */ #define SPRT_T_M 25 /* Guessing 25 match evlauations / 1 model generation */
#define SPRT_M_S 1 /* 1 model per sample */ #define SPRT_M_S 1 /* 1 model per sample */
...@@ -1571,10 +1571,20 @@ static inline int sacCanRefine(RHO_HEST_REFC* p){ ...@@ -1571,10 +1571,20 @@ static inline int sacCanRefine(RHO_HEST_REFC* p){
return p->best.numInl > m; 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. * Refines the best-so-far homography (p->best.H).
*
* BUG: Totally broken for now. DO NOT ENABLE.
*/ */
static inline void sacRefine(RHO_HEST_REFC* p){ static inline void sacRefine(RHO_HEST_REFC* p){
...@@ -1591,41 +1601,62 @@ static inline void sacRefine(RHO_HEST_REFC* p){ ...@@ -1591,41 +1601,62 @@ static inline void sacRefine(RHO_HEST_REFC* p){
/* Find initial conditions */ /* Find initial conditions */
sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N,
p->lm.JtJ, p->lm.Jte, &S); p->lm.JtJ, p->lm.Jte, &S);
/*printf("Initial Error: %12.6f\n", S);*/
/*{ /*Levenberg-Marquardt Loop.*/
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;i<MAXLEVMARQITERS;i++){ for(i=0;i<MAXLEVMARQITERS;i++){
/* The code below becomes an infinite loop when L reeaches infinity. */ /**
while(sacChol8x8Damped(p->lm.JtJ, L, p->lm.tmp1)){ * Attempt a step given current state
L *= 2.0f; * - 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); sacTRInv8x8 (p->lm.tmp1, p->lm.tmp1);
sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH); sacTRISolve8x8(p->lm.tmp1, p->lm.Jte, dH);
sacSub8x1 (newH, p->best.H, dH); sacSub8x1 (newH, p->best.H, dH);
sacCalcJacobianErrors(newH, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, sacCalcJacobianErrors(newH, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N,
NULL, NULL, &newS); 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; * If the new Sum of Square Errors (newS) corresponding to newH is
}else if(R < 0.25f){ * lower than the previous one, save the current state and undamp
L *= 2.0f; * sligthly (decrease L). Else damp more (increase L).
} */
if(newS < S){ if(newS < S){
S = newS; S = newS;
memcpy(p->best.H, newH, sizeof(newH)); memcpy(p->best.H, newH, sizeof(newH));
sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N, sacCalcJacobianErrors(p->best.H, p->arg.src, p->arg.dst, p->arg.inl, p->arg.N,
p->lm.JtJ, p->lm.Jte, &S); 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;
}
} }
} }
} }
...@@ -1705,15 +1736,15 @@ static inline void sacCalcJacobianErrors(const float* restrict H, ...@@ -1705,15 +1736,15 @@ static inline void sacCalcJacobianErrors(const float* restrict H,
float dxh11 = x * iW; float dxh11 = x * iW;
float dxh12 = y * iW; float dxh12 = y * iW;
float dxh13 = iW; float dxh13 = iW;
float dxh21 = 0.0f; /*float dxh21 = 0.0f;*/
float dxh22 = 0.0f; /*float dxh22 = 0.0f;*/
float dxh23 = 0.0f; /*float dxh23 = 0.0f;*/
float dxh31 = -reprojX*x * iW; float dxh31 = -reprojX*x * iW;
float dxh32 = -reprojX*y * iW; float dxh32 = -reprojX*y * iW;
float dyh11 = 0.0f; /*float dyh11 = 0.0f;*/
float dyh12 = 0.0f; /*float dyh12 = 0.0f;*/
float dyh13 = 0.0f; /*float dyh13 = 0.0f;*/
float dyh21 = x * iW; float dyh21 = x * iW;
float dyh22 = y * iW; float dyh22 = y * iW;
float dyh23 = iW; float dyh23 = iW;
...@@ -1743,20 +1774,20 @@ static inline void sacCalcJacobianErrors(const float* restrict H, ...@@ -1743,20 +1774,20 @@ static inline void sacCalcJacobianErrors(const float* restrict H,
JtJ[2][1] += dxh12*dxh13 ;/* +0 */ JtJ[2][1] += dxh12*dxh13 ;/* +0 */
JtJ[2][2] += dxh13*dxh13 ;/* +0 */ JtJ[2][2] += dxh13*dxh13 ;/* +0 */
/*JtJ[3][0] += ;/* 0+0 */ /*JtJ[3][0] += ; 0+0 */
/*JtJ[3][1] += ;/* 0+0 */ /*JtJ[3][1] += ; 0+0 */
/*JtJ[3][2] += ;/* 0+0 */ /*JtJ[3][2] += ; 0+0 */
JtJ[3][3] += dyh21*dyh21;/* 0+ */ JtJ[3][3] += dyh21*dyh21;/* 0+ */
/*JtJ[4][0] += ;/* 0+0 */ /*JtJ[4][0] += ; 0+0 */
/*JtJ[4][1] += ;/* 0+0 */ /*JtJ[4][1] += ; 0+0 */
/*JtJ[4][2] += ;/* 0+0 */ /*JtJ[4][2] += ; 0+0 */
JtJ[4][3] += dyh21*dyh22;/* 0+ */ JtJ[4][3] += dyh21*dyh22;/* 0+ */
JtJ[4][4] += dyh22*dyh22;/* 0+ */ JtJ[4][4] += dyh22*dyh22;/* 0+ */
/*JtJ[5][0] += ;/* 0+0 */ /*JtJ[5][0] += ; 0+0 */
/*JtJ[5][1] += ;/* 0+0 */ /*JtJ[5][1] += ; 0+0 */
/*JtJ[5][2] += ;/* 0+0 */ /*JtJ[5][2] += ; 0+0 */
JtJ[5][3] += dyh21*dyh23;/* 0+ */ JtJ[5][3] += dyh21*dyh23;/* 0+ */
JtJ[5][4] += dyh22*dyh23;/* 0+ */ JtJ[5][4] += dyh22*dyh23;/* 0+ */
JtJ[5][5] += dyh23*dyh23;/* 0+ */ JtJ[5][5] += dyh23*dyh23;/* 0+ */
...@@ -1826,7 +1857,7 @@ static inline float sacDs(const float (*JtJ)[8], ...@@ -1826,7 +1857,7 @@ static inline float sacDs(const float (*JtJ)[8],
* *
* For damping, the diagonal elements are scaled by 1.0 + lambda. * 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# * Source: http://en.wikipedia.org/wiki/Cholesky_decomposition#
* The_Cholesky.E2.80.93Banachiewicz_and_Cholesky.E2.80.93Crout_algorithms * The_Cholesky.E2.80.93Banachiewicz_and_Cholesky.E2.80.93Crout_algorithms
...@@ -1857,13 +1888,13 @@ static inline int sacChol8x8Damped(const float (*A)[8], ...@@ -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 */ x -= (double)L[j][k] * L[j][k];/* - Sum_{k=0..j-1} Ljk^2 */
} }
if(x<0){ if(x<0){
return 1; return 0;
} }
L[j][j] = sqrt(x); /* Ljj = sqrt( ... ) */ L[j][j] = sqrt(x); /* Ljj = sqrt( ... ) */
} }
} }
return 0; return 1;
} }
/** /**
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment