public static string[] CalibOutputToString(CalibOutput output)
{
var c1 = matrixToString("IntrinsicCam1IntrinsicMatrix", output.IntrinsicCam1IntrinsicMatrix);
var c1d = matrixToString("IntrinsicCam1DistortionCoeffs", output.IntrinsicCam1DistortionCoeffs);
var c2 = matrixToString("IntrinsicCam2IntrinsicMatrix", output.IntrinsicCam2IntrinsicMatrix);
var c2d = matrixToString("IntrinsicCam2DistortionCoeffs", output.IntrinsicCam2DistortionCoeffs);
var t = matrixToString("EX_ParamTranslationVector", output.EX_ParamTranslationVector);
var r = matrixToString("EX_ParamRotationVector", output.EX_ParamRotationVector);
var f = matrixToString("fundamental", output.fundamental);
var e = matrixToString("essential", output.essential);
var res = new List <string>();
res.Add($"{output.size.Width},{output.size.Height}");
res.AddRange(c1);
res.AddRange(c1d);
res.AddRange(c2);
res.AddRange(c2d);
res.AddRange(t);
res.AddRange(r);
res.AddRange(f);
res.AddRange(e);
return(res.ToArray());
}
public static CalibOutput stringToCalibOutput(string[] lines)
{
CalibOutput output = new CalibOutput();
var sizeStr = lines[0].Split(',');
output.size = new Size(Int32.Parse(sizeStr[0]), Int32.Parse(sizeStr[1]));
int pos = 1;
output.IntrinsicCam1IntrinsicMatrix = stringToMatrix(lines, ref pos);
output.IntrinsicCam1DistortionCoeffs = stringToMatrix(lines, ref pos);
output.IntrinsicCam2IntrinsicMatrix = stringToMatrix(lines, ref pos);
output.IntrinsicCam2DistortionCoeffs = stringToMatrix(lines, ref pos);
output.EX_ParamTranslationVector = stringToMatrix(lines, ref pos);
var rot = stringToMatrix(lines, ref pos);
var rotData = new double[3];
for (int i = 0; i < 3; i++)
{
rotData[i] = rot.Data[i, 0];
}
output.EX_ParamRotationVector = new RotationVector3D(rotData);
output.fundamental = stringToMatrix(lines, ref pos);
output.essential = stringToMatrix(lines, ref pos);
return(output);
}
private static void SaveOutput(CalibOutput output)
{
var lines = CalibOutputToString(output);
File.WriteAllLines(saveFileName_mat, lines);
//Toolbox.XmlSerialize(IntrinsicCam1IntrinsicMatrix).Save(saveFilePath + "IntrinsicMatrix1.xml");
//Toolbox.XmlSerialize(IntrinsicCam1DistortionCoeffs).Save(saveFilePath + "DistortionCoeffs1.xml");
//Toolbox.XmlSerialize(IntrinsicCam2IntrinsicMatrix).Save(saveFilePath + "IntrinsicMatrix2.xml");
//Toolbox.XmlSerialize(IntrinsicCam2DistortionCoeffs).Save(saveFilePath + "DistortionCoeffs2.xml");
//Toolbox.XmlSerialize(EX_ParamTranslationVector).Save(saveFilePath + "TranslationVector.xml");
//Toolbox.XmlSerialize(EX_ParamRotationVector).Save(saveFilePath + "RotationVector.xml");
//Toolbox.XmlSerialize(fundamental).Save(saveFilePath + "fundamental.xml");
//Toolbox.XmlSerialize(essential).Save(saveFilePath + "essential.xml");
}
public static CalibOutput Caluculating_Stereo_Intrinsics(PointF[][] corners_points_Left, PointF[][] corners_points_Right, Size size)
{
if (File.Exists(saveFileName_mat))
{
return(stringToCalibOutput(File.ReadAllLines(saveFileName_mat)));
}
var objLen = corners_points_Left.GetLength(0); //buffer_length
MCvPoint3D32f[][] corners_object_Points = new MCvPoint3D32f[objLen][]; //stores the calculated size for the chessboard
for (int k = 0; k < corners_points_Left.Length; k++)
{
//Fill our objects list with the real world mesurments for the intrinsic calculations
List <MCvPoint3D32f> object_list = new List <MCvPoint3D32f>();
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
object_list.Add(new MCvPoint3D32f(j * 20.0F, i * 20.0F, 0.0F));
}
}
corners_object_Points[k] = object_list.ToArray();
}
CalibOutput output = new CalibOutput();
output.size = size;
CvInvoke.StereoCalibrate(corners_object_Points, corners_points_Left, corners_points_Right,
output.IntrinsicCam1IntrinsicMatrix,
output.IntrinsicCam1DistortionCoeffs,
output.IntrinsicCam2IntrinsicMatrix,
output.IntrinsicCam2DistortionCoeffs, size,
output.EX_ParamRotationVector,
output.EX_ParamTranslationVector,
output.essential, output.fundamental, Emgu.CV.CvEnum.CalibType.Default,
new MCvTermCriteria(0.1e5)
);
SaveOutput(output);
return(output);
}
public static void Rectify(CalibOutput co)
{
CvInvoke.StereoRectify(co.IntrinsicCam1IntrinsicMatrix,
co.IntrinsicCam1DistortionCoeffs,
co.IntrinsicCam2IntrinsicMatrix,
co.IntrinsicCam2DistortionCoeffs,
co.size,
co.EX_ParamRotationVector.RotationMatrix, co.EX_ParamTranslationVector,
co.R1, co.R2, co.P1, co.P2, co.Q,
Emgu.CV.CvEnum.StereoRectifyType.Default, 0,
co.size, ref co.Rec1, ref co.Rec2);
co.rmap00 = new Matrix <short>(co.size);
co.rmap01 = new Matrix <short>(co.size);
co.rmap10 = new Matrix <short>(1, 1);
co.rmap11 = new Matrix <short>(1, 1);
var CV_16SC2 = (Emgu.CV.CvEnum.DepthType.Cv16S + 8);
//var CV_16SC2 = (Emgu.CV.CvEnum.DepthType.Default );
CvInvoke.InitUndistortRectifyMap(co.IntrinsicCam1IntrinsicMatrix, co.IntrinsicCam1DistortionCoeffs, co.R1, co.P1, co.size, CV_16SC2, co.rmap00, co.rmap01);
CvInvoke.InitUndistortRectifyMap(co.IntrinsicCam2IntrinsicMatrix, co.IntrinsicCam2DistortionCoeffs, co.R2, co.P2, co.size, CV_16SC2, co.rmap10, co.rmap11);
co.rectified = true;
}
