Optimizing OpenCV's undistortPoints

I am working on improving the runtime of code that undistorts every pixel point in an image. Currently each point is undistorted via a call to cv::undistortPoints() as shown below:

cv::Mat src(1, 1, CV_32FC2);        // Create src and dst cv::Mat objects for cv::undistortPoints
cv::Mat dst(1, 1, CV_32FC2);

float* ptr;
ptr = src.ptr<float>(0);            // Fill src with correct pixel x and y values
ptr[0] = (float)x;
ptr[1] = (float)y;

cv::undistortPoints(src, dst, cam_intrinsic, distCoeffs);   //Undistort points

I've identified this particular function as the source of most of the performance issues, since it is called several million times for a single image, and wrote an alternative function in the hopes it would be faster. My function, while it works, runs much slower than cv::undistortPoints. I am not really sure why, since it basically mirrors cv::undistortPoints, minus all the extraneous functionality-- shouldn't it be at least as fast as cv:undistortPoints? What might be causing my code to run so slowly?

void fast_undistortPoints(const CvMat* src, CvMat* dst, CvMat* K,   CvMat* D)
{
    double A[3][3], RR[3][3], k[8] = { 0, 0, 0, 0, 0 }, fx, fy, ifx, ify, cx, cy;
    CvMat matA = cvMat(3, 3, CV_64F, A), _Dk;
    const CvPoint2D64f* srcd;
    CvPoint2D64f* dstd;
    int stype, dtype;   
    int j, iters = 1;

    cvConvert(K, &matA);
    _Dk = cvMat(D->rows, D->cols,
        CV_MAKETYPE(CV_64F, CV_MAT_CN(D->type)), k);

    cvConvert(D, &_Dk);

    fx = A[0][0];
    fy = A[1][1];
    ifx = 1. / fx;
    ify = 1. / fy;
    cx = A[0][2];
    cy = A[1][2];
    srcd = (const CvPoint2D64f*)src->data.ptr;
    dstd = (CvPoint2D64f*)dst->data.ptr;
    stype = CV_MAT_TYPE(src->type);
    dtype = CV_MAT_TYPE(dst->type);

    iters = 5;

    double x, y, x0, y0;
    x = srcd[0].x;
    y = srcd[0].y;


    x0 = x = (x - cx)*ifx;
    y0 = y = (y - cy)*ify;

    // compensate distortion iteratively
    for (j = 0; j < iters; j++)
    {
        double r2 = x*x + y*y;
        double icdist = 1 / (1 + ((k[4] * r2 + k[1])*r2 + k[0])*r2);
        double deltaX = 2 * k[2] * x*y + k[3] * (r2 + 2 * x*x);
        double deltaY = k[2] * (r2 + 2 * y*y) + 2 * k[3] * x*y;
        x = (x0 - deltaX)*icdist;
        y = (y0 - deltaY)*icdist;
    }


    dstd[0].x = x;
    dstd[0].y = y;

}