/*
* The main process of levenberg-marquart optimization
*/
bool intraCamLMEpiProc(const double K[9],
const double invK[9],
const double R0[9],
const double t0[3],
int n3D2D,
const double Ms[],
const double ms[],
int n2D2D,
const double Rpre[],
const double tpre[],
const double umspre[],
const double ums[],
double R_opt[9],
double t_opt[3],
IntraCamPoseOption* opt) {
assert(opt);
double param[6];
//compute the reprojection error
opt->npts = n3D2D;
opt->lambda = opt->lambda0;
opt->err0 = reprojError2(K, R0, t0, n3D2D, Ms, ms) + _epiError2(invK, n2D2D, Rpre, tpre, umspre, R0, t0, ums);
opt->err = opt->err0;
if (opt->verboseLM > 0)
printf("[%d]err:%lf, lambda:%lf\n", -1, opt->err, opt->lambda);
double R[9], t[3];
doubleArrCopy(R, 0, R0, 9);
doubleArrCopy(t, 0, t0, 3);
opt->retTypeLM = 1;
int i = 0;
for (; i < opt->maxIterLM; i++) {
intraCamEpiLMStep(K, invK, R, t, n3D2D, Ms, ms, n2D2D, Rpre, tpre, umspre, ums, param, opt->lambda);
intraCamUpdatePose(R, t, param, R_opt, t_opt);
double err = reprojError2(K, R_opt, t_opt, n3D2D, Ms, ms) + _epiError2(invK, n2D2D, Rpre, tpre, umspre, R_opt,
t_opt, ums);
if (opt->verboseLM > 0)
printf("[%d]err:%lf, lambda:%lf\n", i, err, opt->lambda);
if (fabs(err - opt->err) < opt->epsErrorChangeLM) {
opt->retTypeLM = 0;
break;
}
if (err < opt->err) {
doubleArrCopy(R, 0, R_opt, 9);
doubleArrCopy(t, 0, t_opt, 3);
opt->err = err;
opt->lambda /= 10;
} else {
opt->lambda *= 10;
if (opt->lambda > 1e+4) {
//cannot find the optimum
opt->retTypeLM = -1;
break;
}
}
}
opt->nIterLM = i;
return opt->retTypeLM >= 0;
}
double reprojError(const double* R, double *t, int npts, const double * Ms,
const double* ms0) {
return sqrt(reprojError2(R, t, npts, Ms, ms0) / npts);
}
