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; }