Коррекция «рыбий глаз»

Я видел по этой ссылке http://www.tannerhelland.com/4743/simple-algorithm-correcting-lens-distortion/ алгоритм, чтобы отменить искажение объектива Fisheye, и я попытался реализовать его в C ++ с помощью opencv. Выходное изображение точно такое же, как входное изображение, когда сила параметра близка к нулю, и я получаю плохие результаты с более высокими значениями. Если кто-то знает, в чем может быть проблема (в моем коде или, в более общем смысле, в алгоритме), это будет очень полезно.
Большое спасибо.

#include "opencv2\core\core.hpp"#include "opencv2\highgui\highgui.hpp"#include "opencv2\calib3d\calib3d.hpp"#include <stdio.h>
#include <iostream>
#include <math.h>

using namespace std;
using namespace cv;

int main() {

cout << " Usage: display_image ImageToLoadAndDisplay" << endl;
Mat_<Vec3b> eiffel;
eiffel = imread("C:/Users/Administrator/Downloads/TestFisheye.jpg", CV_LOAD_IMAGE_COLOR);   // Read the file
if (!eiffel.data)                              // Check for invalid input
{
cout << "Could not open or find the image" << endl;
return -1;
}
cout << "Input image depth: " << eiffel.depth() << endl;

namedWindow("Display window", WINDOW_AUTOSIZE);// Create a window for display.
imshow("Display window", eiffel);                   // Show our image inside it.

//waitKey(0);                                          // Wait for a keystroke in the window

int halfWidth = eiffel.rows / 2;
int halfHeight = eiffel.cols / 2;
double strength = 0.0001;
double correctionRadius = sqrt(pow(eiffel.rows, 2) + pow(eiffel.cols, 2)) / strength;
Mat_<Vec3b> dstImage = eiffel;

int newX, newY;
double distance;
double theta;
int sourceX;
int sourceY;
double r;
for (int i = 0; i < dstImage.rows; ++i)
{
for (int j = 0; j < dstImage.cols; j++)
{
newX = i - halfWidth;
newY = j - halfHeight;
distance = sqrt(pow(newX, 2) + pow(newY, 2));
r = distance / correctionRadius;
if (r == 0.0)
theta = 1;
else
theta = atan(r) / r;

sourceX = round(halfWidth + theta*newX);
sourceY = round(halfHeight + theta * newY);

dstImage(i, j)[0] = eiffel(sourceX, sourceY)[0];
dstImage(i, j)[1] = eiffel(sourceX, sourceY)[1];
dstImage(i, j)[2] = eiffel(sourceX, sourceY)[2];
}
}

namedWindow("Display window 2", WINDOW_AUTOSIZE);
imshow("Display window 2", dstImage);                   // Show our image inside it.
waitKey(0);

return 0;
}

PS: я сейчас работаю над первым изображением, размещенным в ссылке.