private void ImportAgisoft()
{
OpenFileDialog openFileDialog = new OpenFileDialog();
openFileDialog.Title = ScreenManagerLang.dlgCameraCalibration_OpenDialogTitle;
openFileDialog.Filter = ScreenManagerLang.FileFilter_XML;
openFileDialog.FilterIndex = 1;
openFileDialog.InitialDirectory = Software.CameraCalibrationDirectory;
if (openFileDialog.ShowDialog() != DialogResult.OK || string.IsNullOrEmpty(openFileDialog.FileName))
{
return;
}
DistortionParameters dp = DistortionImporterAgisoft.Import(openFileDialog.FileName, calibrationHelper.ImageSize);
if (dp != null)
{
distortionParameters = dp;
distorter.Initialize(distortionParameters, calibrationHelper.ImageSize);
AfterImport();
PopulatePhysicalParameters();
PopulateValues();
UpdateDistortionGrid();
}
}
public void Initialize(DistortionParameters parameters, Size imageSize)
{
this.parameters = parameters;
this.imageSize = imageSize;
icp = parameters.IntrinsicCameraParameters;
initialized = true;
}
public FormCalibrateDistortion(Bitmap currentImage, List <List <PointF> > points, CalibrationHelper calibrationHelper)
{
this.bmpCurrentImage = currentImage;
this.calibrationHelper = calibrationHelper;
if (calibrationHelper.DistortionHelper == null || !calibrationHelper.DistortionHelper.Initialized)
{
distortionParameters = new DistortionParameters(calibrationHelper.ImageSize);
}
else
{
distortionParameters = calibrationHelper.DistortionHelper.Parameters;
}
distorter.Initialize(distortionParameters, calibrationHelper.ImageSize);
calibrator = new CameraCalibrator(points, calibrationHelper.ImageSize);
InitializeComponent();
LocalizeForm();
SetupStyle();
PopulateStyleElements();
mnuOpen.Click += (s, e) => Open();
mnuSave.Click += (s, e) => Save();
mnuImportAgisoft.Click += (s, e) => ImportAgisoft();
mnuDefault.Click += (s, e) => RestoreDefaults();
mnuQuit.Click += (s, e) => Close();
btnCalibrate.Enabled = calibrator.Valid;
AfterImport();
PopulatePhysicalParameters();
PopulateValues();
UpdateDistortionGrid();
}
public static void Serialize(XmlWriter w, DistortionParameters p, bool saveSize, Size imageSize)
{
if (p == null)
{
return;
}
w.WriteStartElement("CameraCalibration");
Action <string, double> write = (element, value) => w.WriteElementString(element, string.Format(CultureInfo.InvariantCulture, "{0}", value));
if (saveSize)
{
w.WriteElementString("ImageSize", string.Format("{0};{1}", imageSize.Width, imageSize.Height));
}
write("Fx", p.Fx);
write("Fy", p.Fy);
write("Cx", p.Cx);
write("Cy", p.Cy);
write("K1", p.K1);
write("K2", p.K2);
write("K3", p.K3);
write("P1", p.P1);
write("P2", p.P2);
w.WriteEndElement();
}
private void RestoreDefaults()
{
DistortionParameters parameters = DistortionParameters.Default;
distorter.Initialize(parameters, calibrationHelper.ImageSize);
Populate();
}
public static DistortionParameters Import(string path, Size imageSize)
{
if (!File.Exists(path))
{
return(null);
}
DistortionParameters parameters = null;
try
{
XmlReaderSettings settings = new XmlReaderSettings();
settings.IgnoreComments = true;
settings.IgnoreProcessingInstructions = true;
settings.IgnoreWhitespace = true;
settings.CloseInput = true;
using (XmlReader r = XmlReader.Create(path, settings))
{
r.MoveToContent();
parameters = DistortionSerializer.Deserialize(r, imageSize);
}
}
catch
{
log.ErrorFormat("Import of lens distortion parameters failed.");
}
return(parameters);
}
private void Populate()
{
DistortionParameters p = distorter.Parameters;
lblK1.Text = string.Format("k1 : {0:0.000}", p.K1);
lblK2.Text = string.Format("k2 : {0:0.000}", p.K2);
lblK3.Text = string.Format("k3 : {0:0.000}", p.K3);
lblP1.Text = string.Format("p1 : {0:0.000}", p.P1);
lblP2.Text = string.Format("p2 : {0:0.000}", p.P2);
lblFx.Text = string.Format("fx : {0:0.000}", p.Fx);
lblFy.Text = string.Format("fy : {0:0.000}", p.Fy);
lblCx.Text = string.Format("cx : {0:0.000}", p.Cx);
lblCy.Text = string.Format("cy : {0:0.000}", p.Cy);
Color background = Color.FromArgb(255, 42, 42, 42);
Color foreground = Color.White;
int steps = 20;
Bitmap bmpDistortionGrid = distorter.GetDistortionGrid(background, foreground, steps);
RatioStretch(bmpDistortionGrid, pbDistortion);
bmpUndistorted = distorter.GetUndistortedImage(currentImage);
UpdateImages();
}
private void RestoreDefaults()
{
distortionParameters = new DistortionParameters(calibrationHelper.ImageSize);
distorter.Initialize(distortionParameters, calibrationHelper.ImageSize);
AfterImport();
PopulatePhysicalParameters();
PopulateValues();
UpdateDistortionGrid();
}
public static void Export(string path, DistortionParameters parameters, Size imageSize)
{
XmlWriterSettings settings = new XmlWriterSettings();
settings.Indent = true;
settings.CloseOutput = true;
using (XmlWriter w = XmlWriter.Create(path, settings))
{
DistortionSerializer.Serialize(w, parameters, true, imageSize);
}
}
public void ReadXml(XmlReader r, PointF scale, Size imageSize)
{
r.ReadStartElement();
while (r.NodeType == XmlNodeType.Element)
{
switch (r.Name)
{
case "CalibrationPlane":
calibratorType = CalibratorType.Plane;
calibrator = calibrationPlane;
calibrationPlane.ReadPlaneXml(r, scale);
ComputeCoordinateSystemGrid();
break;
case "CalibrationLine":
calibratorType = CalibratorType.Line;
//calibrator = calibrationLine;
calibrator = calibrationPlane;
calibrationPlane.ReadLineXml(r, scale);
//calibrationLine.ReadXml(r, scale);
break;
case "CalibrationDrawingId":
Guid result;
bool parsed = Guid.TryParse(r.ReadElementContentAsString(), out result);
if (parsed)
{
calibrationDrawingId = result;
}
break;
case "Unit":
lengthUnit = (LengthUnit)Enum.Parse(typeof(LengthUnit), r.ReadElementContentAsString());
break;
case "CameraCalibration":
DistortionParameters parameters = DistortionSerializer.Deserialize(r, imageSize);
distortionHelper.Initialize(parameters, imageSize);
break;
default:
string unparsed = r.ReadOuterXml();
log.DebugFormat("Unparsed content in KVA XML: {0}", unparsed);
break;
}
}
r.ReadEndElement();
AfterCalibrationChanged();
}
private void btnCalibrate_Click(object sender, EventArgs e)
{
if (!calibrator.Valid)
{
return;
}
DistortionParameters parameters = calibrator.Calibrate();
distorter.Initialize(parameters, calibrationHelper.ImageSize);
Populate();
}
public void Initialize(DistortionParameters parameters, Size imageSize)
{
if (parameters.Cx == 0 && parameters.Cy == 0)
{
this.parameters = new DistortionParameters(imageSize);
}
else
{
this.parameters = parameters;
}
this.imageSize = imageSize;
icp = this.parameters.IntrinsicCameraParameters;
initialized = true;
}
private void btnCalibrate_Click(object sender, EventArgs e)
{
if (!calibrator.Valid)
{
return;
}
distortionParameters = calibrator.Calibrate();
distorter.Initialize(distortionParameters, calibrationHelper.ImageSize);
AfterImport();
PopulatePhysicalParameters();
PopulateValues();
UpdateDistortionGrid();
}
public static DistortionParameters Import(string path, Size imageSize)
{
DistortionParameters parameters = null;
try
{
XmlDocument doc = new XmlDocument();
doc.Load(path);
int width = ReadInt(doc, "/calibration/width");
int height = ReadInt(doc, "/calibration/height");
double fx = ReadDouble(doc, "/calibration/fx");
double fy = ReadDouble(doc, "/calibration/fy");
double cx = ReadDouble(doc, "/calibration/cx");
double cy = ReadDouble(doc, "/calibration/cy");
double k1 = ReadDouble(doc, "/calibration/k1");
double k2 = ReadDouble(doc, "/calibration/k2");
double k3 = ReadDouble(doc, "/calibration/k3");
double p1 = ReadDouble(doc, "/calibration/p1");
double p2 = ReadDouble(doc, "/calibration/p2");
if (imageSize.Width != width || imageSize.Height != height)
{
double xFactor = (double)imageSize.Width / width;
double yFactor = (double)imageSize.Height / height;
fx *= xFactor;
fy *= yFactor;
cx *= xFactor;
cy *= yFactor;
}
double pixelsPerMillemeters = imageSize.Width / DistortionParameters.defaultSensorWidth;
parameters = new DistortionParameters(k1, k2, k3, p1, p2, fx, fy, cx, cy, pixelsPerMillemeters);
}
catch
{
log.ErrorFormat("Import of Agisoft Lens distortion parameters failed.");
}
return(parameters);
}
private void Open()
{
OpenFileDialog openFileDialog = new OpenFileDialog();
openFileDialog.Title = ScreenManagerLang.dlgCameraCalibration_OpenDialogTitle;
openFileDialog.Filter = ScreenManagerLang.FileFilter_XML;
openFileDialog.FilterIndex = 1;
openFileDialog.InitialDirectory = Software.CameraCalibrationDirectory;
if (openFileDialog.ShowDialog() != DialogResult.OK || string.IsNullOrEmpty(openFileDialog.FileName))
{
return;
}
DistortionParameters parameters = DistortionImporterKinovea.Import(openFileDialog.FileName, calibrationHelper.ImageSize);
if (parameters != null)
{
distorter.Initialize(parameters, calibrationHelper.ImageSize);
Populate();
}
}
public static DistortionParameters Deserialize(XmlReader r, Size inputSize)
{
r.ReadStartElement();
Size size = inputSize;
double k1 = 0;
double k2 = 0;
double k3 = 0;
double p1 = 0;
double p2 = 0;
double fx = 1;
double fy = 1;
double cx = 0;
double cy = 0;
while (r.NodeType == XmlNodeType.Element)
{
switch (r.Name)
{
case "ImageSize":
size = XmlHelper.ParseSize(r.ReadElementContentAsString());
break;
case "Fx":
fx = r.ReadElementContentAsDouble();
break;
case "Fy":
fy = r.ReadElementContentAsDouble();
break;
case "Cx":
cx = r.ReadElementContentAsDouble();
break;
case "Cy":
cy = r.ReadElementContentAsDouble();
break;
case "K1":
k1 = r.ReadElementContentAsDouble();
break;
case "K2":
k2 = r.ReadElementContentAsDouble();
break;
case "K3":
k3 = r.ReadElementContentAsDouble();
break;
case "P1":
p1 = r.ReadElementContentAsDouble();
break;
case "P2":
p2 = r.ReadElementContentAsDouble();
break;
default:
string unparsed = r.ReadOuterXml();
log.DebugFormat("Unparsed content in Camera calibration: {0}", unparsed);
break;
}
}
r.ReadEndElement();
if (inputSize.Width != size.Width || inputSize.Height != size.Height)
{
double xFactor = (double)inputSize.Width / size.Width;
double yFactor = (double)inputSize.Height / size.Height;
fx *= xFactor;
fy *= yFactor;
cx *= xFactor;
cy *= yFactor;
}
DistortionParameters parameters = new DistortionParameters(k1, k2, k3, p1, p2, fx, fy, cx, cy);
return(parameters);
}
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);
}
