/// <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);
}
