void testRadialInversion(int scale)
{
RGB24Buffer *image = new RGB24Buffer(250 * scale, 400 * scale);
auto operation = [](int i, int j, RGBColor *pixel)
{
i = i / 100;
j = j / 200;
if ( (i % 2) && (j % 2)) *pixel = RGBColor::Green();
if (!(i % 2) && (j % 2)) *pixel = RGBColor::Yellow();
if ( (i % 2) && !(j % 2)) *pixel = RGBColor::Red();
if (!(i % 2) && !(j % 2)) *pixel = RGBColor::Blue();
};
touchOperationElementwize(image, operation);
#if 0
LensDistortionModelParameters deformator;
deformator.setPrincipalX(image->w / 2);
deformator.setPrincipalY(image->h / 2);
deformator.setNormalizingFocal(deformator.principalPoint().l2Metric());
deformator.setTangentialX(0.001);
deformator.setTangentialY(0.001);
deformator.setAspect(1.0);
deformator.setScale(1.0);
deformator.mKoeff.push_back( 0.1);
deformator.mKoeff.push_back(-0.2);
deformator.mKoeff.push_back( 0.3);
#else
LensDistortionModelParameters deformator;
deformator.setMapForward(false);
deformator.setPrincipalX(480);
deformator.setPrincipalY(360);
deformator.setNormalizingFocal(734.29999999999995);
deformator.setTangentialX(0.00);
deformator.setTangentialY(0.00);
deformator.setShiftX(0.00);
deformator.setShiftY(0.00);
deformator.setAspect(1.0);
deformator.setScale (1.0);
deformator.mKoeff.clear();
deformator.mKoeff.push_back( 0);
deformator.mKoeff.push_back( -0.65545);
deformator.mKoeff.push_back( 0);
deformator.mKoeff.push_back( 8.2439);
// deformator.mKoeff.push_back( 0);
// deformator.mKoeff.push_back( 8.01);
#endif
RadialCorrection T(deformator);
PreciseTimer timer;
cout << "Initial deformation... " << endl;
cout << T.mParams << flush;;
cout << "Starting deformation... " << flush;
timer = PreciseTimer::currentTime();
RGB24Buffer *deformed = image->doReverseDeformationBlTyped<RadialCorrection>(&T);
cout << "done in: " << timer.usecsToNow() << "us" << endl;
/* */
int inversionGridStep = 30;
cout << "Starting invertion... " << flush;
RadialCorrection invert = T.invertCorrection(image->h, image->w, inversionGridStep);
cout << "done" << endl;
cout << "Starting backprojection... " << flush;
timer = PreciseTimer::currentTime();
RGB24Buffer *backproject = deformed->doReverseDeformationBlTyped<RadialCorrection>(&invert);
cout << "done in: " << timer.usecsToNow() << "us" << endl;
cout << "done" << endl;
RGB24Buffer *debug = new RGB24Buffer(image->getSize());
/* Show visual */
double dh = (double)image->h / (inversionGridStep - 1);
double dw = (double)image->w / (inversionGridStep - 1);
for (int i = 0; i < inversionGridStep; i++)
{
for (int j = 0; j < inversionGridStep; j++)
{
Vector2dd point(dw * j, dh * i);
debug->drawCrosshare1(point, RGBColor::Yellow());
Vector2dd deformed = T.mapToUndistorted(point); /* this could be cached */
Vector2dd backproject = invert.mapToUndistorted(deformed);
debug->drawCrosshare1(backproject, RGBColor::Green());
}
}
BMPLoader().save("input.bmp" , image);
BMPLoader().save("debug.bmp" , debug);
BMPLoader().save("forward.bmp" , deformed);
BMPLoader().save("backproject.bmp", backproject);
delete_safe(image);
delete_safe(debug);
delete_safe(deformed);
delete_safe(backproject);
}
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;
}
