Reverse fisheye radial distortion using Field of View model

Let's suppose I have this distorted image taken from a fisheye camera with 185º FoV.

Image taken from Proc. IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP), pp. 1541-1545, Shanghai, China, March 2016.

I want to undistort it using the FoV model explained in Frederic Devernay, Olivier Faugeras. Straight lines have to be straight: automatic calibration and removal of distortion from scenes of structured enviroments. Machine Vision and Applications, Springer Verlag, 2001, 13 (1), pp.14-24, concretely in equations 13 and 14.

rd = 1 / ω * arctan (2 * ru * tan(ω / 2))   // Equation 13
ru = tan(rd * ω) / (2 * tan(ω / 2))         // Equation 14

I've implemented it in OpenCV and I can't achieve it to work. I interpret rd as the distorted distance of a point from the optical center, and ru as the new undistorted distance.

I let you a full minimal project.

#include <opencv2/opencv.hpp>

#define W (185*CV_PI/180)

cv::Mat undistortFishEye(const cv::Mat &distorted, const float w)
{
    cv::Mat map_x, map_y;
    map_x.create(distorted.size(), CV_32FC1);
    map_y.create(distorted.size(), CV_32FC1);

    int Cx = distorted.cols/2;
    int Cy = distorted.rows/2;

    for (int x = -Cx; x < Cx; ++x) {
        for (int y = -Cy; y < Cy; ++y) {
            double rd = sqrt(x*x+ y*y);
            double ru = tan(rd*w) / (2*tan(w/2));
            map_x.at<float>(y+Cy,x+Cx) = ru/rd * x + Cx;
            map_y.at<float>(y+Cy,x+Cx) = ru/rd * y + Cy;
        }
    }

    cv::Mat undistorted;
    remap(distorted, undistorted, map_x, map_y, CV_INTER_LINEAR);
    return undistorted;
}

int main(int argc, char **argv)
{
    cv::Mat im_d = cv::imread(<your_image_path>, CV_LOAD_IMAGE_GRAYSCALE);
    cv::imshow("Image distorted", im_d);

    cv::Mat im_u = undistortFishEye(im_d, W);
    cv::imshow("Image undistorted", im_u);

    cv::waitKey(0);
}