RadialCorrection RadialCorrection::invertCorrection(int h, int w, int step)
{
LensDistortionModelParameters input = this->mParams;
LensDistortionModelParameters result;
/* make initial guess */
result.setPrincipalX(input.principalX());
result.setPrincipalY(input.principalY());
result.setNormalizingFocal(input.normalizingFocal());
result.setTangentialX(-input.tangentialX());
result.setTangentialY(-input.tangentialY());
result.setScale(1.0 / input.scale());
result.setAspect(1.0 / input.scale()); /*< bad guess I believe */
result.mKoeff.resize(RadialCorrectionInversionCostFunction::MODEL_POWER);
for (unsigned i = 0; i < RadialCorrectionInversionCostFunction::MODEL_POWER; i++)
{
if (i < input.mKoeff.size()) {
result.mKoeff[i] = -input.mKoeff[i];
} else {
result.mKoeff[i] = 0.0;
}
}
/* Pack the guess and launch optimization */
RadialCorrection guess(result);
RadialCorrectionInversionCostFunction cost(*this, guess, step, h, w);
LevenbergMarquardt lmFit;
lmFit.maxIterations = 101;
lmFit.maxLambda = 10e8;
lmFit.lambdaFactor = 8;
lmFit.f = &cost;
lmFit.traceCrucial = true;
lmFit.traceProgress = true;
lmFit.traceMatrix = true;
vector<double> initialGuess(cost.inputs);
RadialCorrectionInversionCostFunction::fillWithRadial(guess, &(initialGuess[0]));
cout << guess.mParams << endl;
EllipticalApproximation1d stats;
stats = cost.aggregatedCost(&(initialGuess[0]));
SYNC_PRINT(("Start Mean Error: %f px\n", stats.getRadiusAround0()));
SYNC_PRINT(("Start Max Error: %f px\n", stats.getMax()));
vector<double> target(cost.outputs, 0.0);
vector<double> optimal = lmFit.fit(initialGuess, target);
guess = RadialCorrectionInversionCostFunction::updateWithModel(guess, &(optimal[0]));
/* Cost */
cout << guess.mParams << endl;
stats = cost.aggregatedCost(&(optimal[0]));
SYNC_PRINT(("Final Mean Error: %f px\n", stats.getRadiusAround0()));
SYNC_PRINT(("Final Max Error: %f px\n", stats.getMax()));
return guess;
}
