/// <summary> /// Given coordinates in distorted space, returns the point in undistorted space. /// </summary> public PointF Undistort(PointF point) { if (!initialized) { return(point); } double x = 0; double y = 0; using (Matrix <float> src = EmguHelper.ToMatrix(point)) using (Matrix <float> dst = new Matrix <float>(1, 1, 2)) { CvInvoke.cvUndistortPoints( src.Ptr, dst.Ptr, icp.IntrinsicMatrix.Ptr, icp.DistortionCoeffs.Ptr, IntPtr.Zero, IntPtr.Zero ); x = dst.Data[0, 0] * parameters.Fx + parameters.Cx; y = dst.Data[0, 1] * parameters.Fy + parameters.Cy; } return(new PointF((float)x, (float)y)); }
public DistortionParameters Calibrate() { CALIB_TYPE flags = //CALIB_TYPE.CV_CALIB_FIX_ASPECT_RATIO | //CALIB_TYPE.CV_CALIB_FIX_FOCAL_LENGTH | //CALIB_TYPE.CV_CALIB_FIX_PRINCIPAL_POINT | //CALIB_TYPE.CV_CALIB_FIX_K3 | (CALIB_TYPE)16384; // CV_CALIB_RATIONAL_MODEL int imageCount = allImagePoints.Length; int[] pointCounts = new int[allObjectPoints.Length]; for (int i = 0; i < allObjectPoints.Length; i++) { // TODO: Check that both image and object have the same number of points for this image. pointCounts[i] = allObjectPoints[i].Length; } IntrinsicCameraParameters icp = new IntrinsicCameraParameters(); MCvTermCriteria termCriteria = new MCvTermCriteria(); termCriteria.type = TERMCRIT.CV_TERMCRIT_ITER | TERMCRIT.CV_TERMCRIT_EPS; termCriteria.max_iter = 30; termCriteria.epsilon = 0.001; using (Matrix <float> objectPointMatrix = EmguHelper.ToMatrix(allObjectPoints)) using (Matrix <float> imagePointMatrix = EmguHelper.ToMatrix(allImagePoints)) using (Matrix <int> pointCountsMatrix = new Matrix <int>(pointCounts)) using (Matrix <double> rotationVectors = new Matrix <double>(imageCount, 3)) using (Matrix <double> translationVectors = new Matrix <double>(imageCount, 3)) { CvInvoke.cvCalibrateCamera2( objectPointMatrix.Ptr, imagePointMatrix.Ptr, pointCountsMatrix.Ptr, imageSize, icp.IntrinsicMatrix, icp.DistortionCoeffs, rotationVectors, translationVectors, flags, termCriteria); } double k1 = icp.DistortionCoeffs[0, 0]; double k2 = icp.DistortionCoeffs[1, 0]; double k3 = icp.DistortionCoeffs[4, 0]; double p1 = icp.DistortionCoeffs[2, 0]; double p2 = icp.DistortionCoeffs[3, 0]; double fx = icp.IntrinsicMatrix[0, 0]; double fy = icp.IntrinsicMatrix[1, 1]; double cx = icp.IntrinsicMatrix[0, 2]; double cy = icp.IntrinsicMatrix[1, 2]; DistortionParameters parameters = new DistortionParameters(icp); log.DebugFormat("Distortion coefficients: k1:{0:0.000}, k2:{1:0.000}, k3:{2:0.000}, p1:{3:0.000}, p2:{4:0.000}.", k1, k2, k3, p1, p2); log.DebugFormat("Camera intrinsics: fx:{0:0.000}, fy:{1:0.000}, cx:{2:0.000}, cy:{3:0.000}", fx, fy, cx, cy); return(parameters); }