Revision history [back]

I'm assuming your x and y to distort are in the range 0->image dimensions, not the normalized version.

There are three steps. First, normalize the points to be independent of the camera matrix using undistortPoints with no distortion matrix. Second, convert them to 3d points using the convertPointsToHomogeneous. Thirdly, project them back to image space using the distortion matrix.

vector<Point2d> ptsOut;
vector<Point3d> ptsTemp;
Mat rtemp, ttemp;
rtemp.create( 3, 1, CV_32F );
rtemp.setTo( 0 );
rtemp.copyTo( ttemp );
undistortPoints( ptsOut, ptsOut, dist::cameraMatrix, noArray());
convertPointsToHomogeneous( ptsOut, ptsTemp );
projectPoints( ptsTemp, rtemp, ttemp, dist::cameraMatrix, dist::distortion, ptsOut );

I'm seeing a bit of a problem right at the very corner that I'm not sure why. Probably where the distortion goes iffy anyway.