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

}

• Part of the issue might be that you use the cv::Mat methods (.at<>, .ptr<>, etc.) whereas the OpenCV version uses them directly as multi-dimensional arrays. I would look into this first. The extra overhead could be what is slowing you down. I'm basing this off of this GitHub source that I found. – Der Kommissar Aug 5 '15 at 17:56
• hmm, I did try removing the built-in cv::Mat methods, but it still runs quite a bit slower than cv::undistortPoints. see edited post for updated code. – major mishap Aug 5 '15 at 19:07