Ректификация объектива «рыбий глаз» на плоскость

У меня есть линза типа «рыбий глаз», для которой я знаю главную точку C= (x_0,y_0) и отношение между r (искаженное радиальное расстояние) и Theta (угол между оптической осью и входящим лучом), которое соответствует эквидистантной модели r(Theta)= f*Theta

Я хотел бы использовать эти параметры для исправления этого изображения Изображение для исправления, для этого я выполните следующие действия, но я не уверен, что мой подход правильный, потому что в конце у меня остались отрицательные значения:

1- shift the origin to the principal point

2- append to each point in the image plane  1 for the z coordinate 
(which corresponds to a focal length equal to 1): {x,y}  ==> {x,y,1}

3- calculate the angle Thea between {x, y, 1} and the point {0,0,1}

4- calculate the angle Beta in the image plane Beta = ArcTan(y/x)

5- calculate the image rectified coordinates:

      x_rec = x_0 +[ Cos(Beta) * r(Theta)]

     y_rec = y_0 +[ Sin(Beta) * r(Theta)]