public void loadCalibrationNow(string file = "cameraMat.txt")
{
FileStorage fs = new FileStorage(file, FileStorage.Mode.Read);
Mat cameraMat = new Mat();
fs.GetFirstTopLevelNode().ReadMat(cameraMat);
fs = new FileStorage("distort.txt", FileStorage.Mode.Read);
Mat distortMat = new Mat();
fs.GetFirstTopLevelNode().ReadMat(distortMat);
cameraPar = new float[9];
double[] cameraParDouble = new Double[9];
cameraMat.CopyTo(cameraParDouble);
for (int i = 0; i < 9; i++)
{
cameraPar[i] = (float)cameraParDouble[i];
}
const int width = 4; //5 //width of chessboard no. squares in width - 1
const int height = 4; //5 // heght of chess board no. squares in heigth - 1
Size patternSize = new Size(width, height); //size of chess board to be detected
MCvPoint3D32f[] corners_object_list = new MCvPoint3D32f[width * height];
PointF[] corners_points_list = new PointF[width * height];
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
//corners_object_list[width * i + j] = new MCvPoint3D32f((j) * 31, (height -1 - i) * 31, 0 );
corners_object_list[width * i + j] = new MCvPoint3D32f((j) * -31 - 289.916f + (float)Form1.currentX0.Value, (height - 1 - i) * -31 - 129.96f + (float)Form1.currentY0.Value, 0);
}
}
var output = new Emgu.CV.Util.VectorOfPointF();
Size smallerPicSize = new Size(800, 600);
render.currentBMP.Save("PicCalibrate.bmp");
Mat smallerPic = new Mat("PicCalibrate.bmp", LoadImageType.Unchanged);
bool found = CvInvoke.FindChessboardCorners(smallerPic, patternSize, output);//find chessboard
if (found == false)
{
MessageBox.Show("fail");
return;
}
Console.WriteLine("found:" + found);
corners_points_list = output.ToArray();
Mat rotationVec = new Mat();
Mat translationVec = new Mat();
bool solved = CvInvoke.SolvePnP(corners_object_list, corners_points_list, cameraMat, distortMat, rotationVec, translationVec);
//1 by 3 array of rotate Matrix
rotateArr = new float[9];
Mat rotationMatrix = new Mat();
CvInvoke.Rodrigues(rotationVec, rotationMatrix);
double[] rotateArrDouble = new double[9];
rotationMatrix.CopyTo(rotateArrDouble);
for (int i = 0; i < 9; i++)
{
rotateArr[i] = (float)rotateArrDouble[i];
}
//1 by 3 array of translate Matrix
translateArr = new float[3];
double[] translateArrDouble = new double[3];
translationVec.CopyTo(translateArrDouble);
for (int i = 0; i < 3; i++)
{
translateArr[i] = (float)translateArrDouble[i];
}
System.IO.StreamWriter swTranslate = new StreamWriter("transArr.txt");
for (int i = 0; i < 3; i++)
{
swTranslate.WriteLine(translateArr[i]);
}
swTranslate.Close();
System.IO.StreamWriter swRot = new StreamWriter("rotArr.txt");
for (int i = 0; i < 9; i++)
{
swRot.WriteLine(rotateArr[i]);
}
swRot.Close();
}
public void loadCalibration(string file = "cameraMat.txt")
{
FileStorage fs = new FileStorage(file, FileStorage.Mode.Read);
Mat cameraMat = new Mat();
fs.GetFirstTopLevelNode().ReadMat(cameraMat);
fs = new FileStorage("distort.txt", FileStorage.Mode.Read);
Mat distortMat = new Mat();
fs.GetFirstTopLevelNode().ReadMat(distortMat);
cameraPar = new float[9];
double[] cameraParDouble = new Double[9];
cameraMat.CopyTo(cameraParDouble);
for (int i = 0; i < 9; i++)
{
cameraPar[i] = (float)cameraParDouble[i];
}
const int width = 5; //5 //width of chessboard no. squares in width - 1
const int height = 5; //5 // heght of chess board no. squares in heigth - 1
Size patternSize = new Size(width, height); //size of chess board to be detected
MCvPoint3D32f[] corners_object_list = new MCvPoint3D32f[width * height];
PointF[] corners_points_list = new PointF[width * height];
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
corners_object_list[5 * i + j] = new MCvPoint3D32f((4 - i) * 29, (4 - j) * 29, 5);
}
}
var output = new Emgu.CV.Util.VectorOfPointF();
Size smallerPicSize = new Size(816, 612);
Mat imgCam = new Mat("1.jpg", LoadImageType.Unchanged);//load picture of chessboard
Mat smallerPic = new Mat();
Size PicSize = new Size(3264, 2448);
CvInvoke.Resize(imgCam, smallerPic, smallerPicSize);
bool found = CvInvoke.FindChessboardCorners(smallerPic, patternSize, output);//find chessboard
Console.WriteLine("found:" + found);
corners_points_list = output.ToArray();
Mat rotationVec = new Mat();
Mat translationVec = new Mat();
CvInvoke.SolvePnP(corners_object_list, corners_points_list, cameraMat, distortMat, rotationVec, translationVec);
//1 by 3 array of rotate Matrix
rotateArr = new float[9];
Mat rotationMatrix = new Mat();
CvInvoke.Rodrigues(rotationVec, rotationMatrix);
double[] rotateArrDouble = new double[9];
rotationMatrix.CopyTo(rotateArrDouble);
for (int i = 0; i < 9; i++)
{
rotateArr[i] = (float)rotateArrDouble[i];
}
//1 by 3 array of translate Matrix
translateArr = new float[3];
double[] translateArrDouble = new double[3];
translationVec.CopyTo(translateArrDouble);
for (int i = 0; i < 3; i++)
{
translateArr[i] = (float)translateArrDouble[i];
}
}
private void CalibrateCamera()
{
const int width = 5; //5 //width of chessboard no. squares in width - 1
const int height = 5; //5 // heght of chess board no. squares in heigth - 1
Size patternSize = new Size(width, height); //size of chess board to be detected
MCvPoint3D32f[][] corners_object_list = new MCvPoint3D32f[6][];
PointF[][] corners_points_list = new PointF[6][];
for (int k = 0; k < 6; k++)
{
corners_object_list[k] = new MCvPoint3D32f[width * height];
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
corners_object_list[k][5 * i + j] = new MCvPoint3D32f((4 - i) * 29, (4 - j) * 29, 8);
}
}
}
var output = new Emgu.CV.Util.VectorOfPointF();
Size smallerPicSize = new Size(816, 612);
for (int k = 1; k <= 6; k++)
{
Mat imgCam = new Mat(k + ".jpg", LoadImageType.Unchanged);//load picture of chessboard
Mat smallerPic = new Mat();
Size PicSize = new Size(3264, 2448);
CvInvoke.Resize(imgCam, smallerPic, smallerPicSize);
if (k == 1)
{
smallerPic.Save("small1.jpg");
}
//CvInvoke.Imshow("small", smallerPic);
bool found = CvInvoke.FindChessboardCorners(smallerPic, patternSize, output);//find chessboard
Console.WriteLine("found:" + found);
corners_points_list[k - 1] = output.ToArray();
}
for (int i = 0; i < output.Size; i++)
{
Console.WriteLine(corners_points_list[0].GetValue(i));
}
Mat cameraMat = new Mat();
Mat distorCoef = new Mat();
Mat[] rotationVec = new Mat[6];
Mat[] translationVec = new Mat[6];
for (int k = 0; k < 6; k++)
{
translationVec[k] = new Mat();
rotationVec[k] = new Mat();
}
MCvTermCriteria criteria = new MCvTermCriteria();
double rms = CvInvoke.CalibrateCamera(corners_object_list, corners_points_list, smallerPicSize, cameraMat, distorCoef, CalibType.RationalModel, criteria, out rotationVec, out translationVec);
cameraPar = new float[9];
double[] cameraParDouble = new Double[9];
cameraMat.CopyTo(cameraParDouble);
for (int i = 0; i < 9; i++)
{
cameraPar[i] = (float)cameraParDouble[i];
}
//1 by 14 array of distortion coeff, only first 8 important
double[] distortArr = new double[14];
distorCoef.CopyTo(distortArr);
//1 by 3 array of rotate Matrix
rotateArr = new float[9];
Mat rotationMatrix = new Mat();
//need to flip stuff
//double[] rv = new double[3];
//rotationVec[0].CopyTo(rv);
//rv[1] = -1.0f * rv[1]; rv[2] = -1.0f * rv[2];
//rotationVec[0].SetTo(rv);
CvInvoke.Rodrigues(rotationVec[0], rotationMatrix);
double[] rotateArrDouble = new double[9];
rotationMatrix.CopyTo(rotateArrDouble);
for (int i = 0; i < 9; i++)
{
rotateArr[i] = (float)rotateArrDouble[i];
}
//1 by 3 array of translate Matrix
translateArr = new float[3];
double[] translateArrDouble = new double[3];
translationVec[0].CopyTo(translateArrDouble);
for (int i = 0; i < 3; i++)
{
translateArr[i] = (float)translateArrDouble[i];
}
for (int i = 0; i < 3; i++)
{
Console.WriteLine(rotateArr[i]);
}
for (int i = 0; i < 3; i++)
{
Console.WriteLine(translateArr[i]);
}
//CvInvoke.Imshow("chessboard", imgCam);
Console.WriteLine(rms);
FileStorage fs = new FileStorage("cameraMat.txt", FileStorage.Mode.Write);
fs.Write(cameraMat);
fs.ReleaseAndGetString();
fs = new FileStorage("distort.txt", FileStorage.Mode.Write);
fs.Write(distorCoef);
fs.ReleaseAndGetString();
}
public static void DetectMarkers(string myFile)
{
var grayImage = new Image <Gray, byte>(myFile);
//CheckConnectedComponents(grayImage.Mat);
Mat imageCopy = Emgu.CV.CvInvoke.Imread(myFile, Emgu.CV.CvEnum.ImreadModes.Color);
byte[] grayImageBytes = new byte[grayImage.Data.Length];
Buffer.BlockCopy(grayImage.Data, 0, grayImageBytes, 0, grayImage.Data.Length);
myVideoWidth = grayImage.Width;
myVideoHeight = grayImage.Height;
//var thresh = grayImage.Clone();
//double otsuThreshold = CvInvoke.Threshold(grayImage, thresh, 128.0, 255.0, Emgu.CV.CvEnum.ThresholdType.Otsu);
//CvInvoke.Imwrite(Path.GetDirectoryName(myFile) + "\\" + Path.GetFileNameWithoutExtension(myFile) + "-threshold" + Path.GetExtension(myFile), thresh, new KeyValuePair<Emgu.CV.CvEnum.ImwriteFlags, int>(Emgu.CV.CvEnum.ImwriteFlags.PngCompression, 3));
//Detect the AR Marker first
// Initialise AR
string myCameraFile = "data\\calib.dat";
var arParams = LoadCameraFromFile(myCameraFile);
// string myVConf = "-module=Image -preset=photo -format=BGRA";
string myVConf = "-module=Image -width=" + myVideoWidth + " -height=" + myVideoHeight + " -format=MONO";
ARToolKitFunctions.Instance.arwInitialiseAR();
ARToolKitFunctions.Instance.arwInitARToolKit(myVConf, myCameraFile);
string artkVersion = ARToolKitFunctions.Instance.arwGetARToolKitVersion();
string pixelFormat = string.Empty;
ARToolKitFunctions.Instance.arwSetLogLevel(0);
myLogger = new Logger();
Mat cameraMatrix = new Mat(3, 3, Emgu.CV.CvEnum.DepthType.Cv64F, 1);
int nFactors = 8;
Mat distortionCoeffs = new Mat(nFactors, 1, Emgu.CV.CvEnum.DepthType.Cv64F, 1);
double[] cameraArray = new double[9];
for (int j = 0; j < 3; j++)
{
for (int i = 0; i < 3; i++)
{
cameraArray[j * 3 + i] = arParams.mat[j, i];
}
}
double[] distCoeffArray = new double[nFactors];
for (int i = 0; i < nFactors; i++)
{
distCoeffArray[i] = arParams.dist_factor[i];
}
Marshal.Copy(cameraArray, 0, cameraMatrix.DataPointer, 9);
Marshal.Copy(distCoeffArray, 0, distortionCoeffs.DataPointer, nFactors);
mdlRecognise.AddMarkersToARToolKit();
var cornersErr = new Emgu.CV.Util.VectorOfPointF();
var cornersErr2 = new Emgu.CV.Util.VectorOfPointF();
var retB = ARToolKitFunctions.Instance.arwUpdateARToolKit(grayImageBytes, false);
for (int markerID = 0; markerID < 102; markerID++)
{
double[] mv = new double[16] {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
double[] corners = new double[32] {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
retB = ARToolKitFunctions.Instance.arwQueryMarkerTransformation(markerID, mv, corners, out int numCorners);
if (!retB)
{
continue;
}
var trans = OpenGL2Trans(mv);
var pts2d = new List <clsPoint>();
var cornerPoints = new Emgu.CV.Util.VectorOfPointF();
pts2d.Add(new clsPoint(-40, -40));
pts2d.Add(new clsPoint(40, -40));
pts2d.Add(new clsPoint(40, 40));
pts2d.Add(new clsPoint(-40, 40));
if (markerID == myGFMarkerID)
{
pts2d.Add(new clsPoint(110 - 40, -40));
pts2d.Add(new clsPoint(110 + 40, -40));
pts2d.Add(new clsPoint(110 + 40, 40));
pts2d.Add(new clsPoint(110 - 40, 40));
pts2d.Add(new clsPoint(110 - 40, -40 - 190));
pts2d.Add(new clsPoint(110 + 40, -40 - 190));
pts2d.Add(new clsPoint(110 + 40, 40 - 190));
pts2d.Add(new clsPoint(110 - 40, 40 - 190));
pts2d.Add(new clsPoint(-40, -40 - 190));
pts2d.Add(new clsPoint(40, -40 - 190));
pts2d.Add(new clsPoint(40, 40 - 190));
pts2d.Add(new clsPoint(-40, 40 - 190));
}
else
{
pts2d.Add(new clsPoint(-40 - 85, -40));
pts2d.Add(new clsPoint(40 - 85, -40));
pts2d.Add(new clsPoint(40 - 85, 40));
pts2d.Add(new clsPoint(-40 - 85, 40));
}
//var objectPoints = new Emgu.CV.Util.VectorOfPoint3D32F(pts2d.Select(p => new MCvPoint3D32f((float)p.x, (float)p.y, 0)).ToArray());
//var reprojectPoints = new Emgu.CV.Util.VectorOfPointF();
//Mat rvec = new Mat(3, 3, Emgu.CV.CvEnum.DepthType.Cv64F, 1);
//double[] matrixArray = new double[9];
//for (int j = 0; j < 3; j++) {
// for (int k = 0; k < 3; k++) {
// matrixArray[j * 3 + k] = trans[j, k];
// }
//}
//Marshal.Copy(matrixArray, 0, rvec.DataPointer, 9);
//Mat tvec = new Mat(3, 1, Emgu.CV.CvEnum.DepthType.Cv64F, 1);
//double[] vectorArray = new double[3];
//for (int j = 0; j < 3; j++) {
// vectorArray[j] = trans[j, 3];
//}
//Marshal.Copy(vectorArray, 0, tvec.DataPointer, 3);
//CvInvoke.ProjectPoints(objectPoints, rvec, tvec, cameraMatrix, distortionCoeffs, reprojectPoints);
//cornerPoints.Push(reprojectPoints.ToArray());
//cornersErr.Push(reprojectPoints.ToArray());
for (int i = 0; i < pts2d.Count; i++)
{
var pt = ModelToImageSpace(arParams, trans, pts2d[i]);
cornerPoints.Push(new PointF[] { new PointF((float)pt.X, (float)pt.Y) });
}
cornersErr.Push(cornerPoints.ToArray());
//for (int i = 0; i < numCorners; i++) {
// //pts2d.Add(new clsPoint(0, 0));
// cornerPoints.Push(new PointF[] { new PointF((float)corners[i * 2], (float)corners[i * 2 + 1]) });
// //arParamIdeal2Observ(arParams.dist_factor, corners[i * 2], corners[i * 2 + 1], out double ox, out double oy, arParams.dist_function_version);
// //cornerPoints.Push(new PointF[] { new PointF((float)ox, (float)oy) });
//}
if (cornerPoints.Size == pts2d.Count)
{
//cornersErr.Push(cornerPoints.ToArray());
//var cornersCopy = new List<clsPoint>();
//var cornersCopy2 = new List<clsPoint>();
//foreach (var p in cornerPoints.ToArray()) cornersCopy.Add(new clsPoint(p.X, p.Y));
//CvInvoke.CornerSubPix(grayImage, cornerPoints, new Size(5, 5), new Size(-1, -1), new Emgu.CV.Structure.MCvTermCriteria(100));
//foreach (var p in cornerPoints.ToArray()) cornersCopy2.Add(new clsPoint(p.X, p.Y));
//for (int i = 0; i < cornersCopy.Count; i++) {
// cornersErr.Push(new PointF[] { new PointF((float)cornersCopy[i].x, (float)cornersCopy[i].y) });
// if (cornersCopy[i].Dist(cornersCopy2[i]) > 4.0) {
// cornersErr2.Push(new PointF[] { new PointF((float)cornersCopy2[i].x, (float)cornersCopy2[i].y) });
// }
//}
//Emgu.CV.Util.VectorOfPointF imagePoints = new Emgu.CV.Util.VectorOfPointF();
//for (int i = 0; i < centerPoints.Size; i++) {
// arParamObserv2Ideal(arParams.dist_factor, centerPoints[i].X, centerPoints[i].Y, out double ox, out double oy, arParams.dist_function_version);
// imagePoints.Push(new PointF[] { new PointF((float)ox, (float)oy) });
//}
//Mat rvec = new Mat();
//Mat tvec = new Mat();
//CvInvoke.SolvePnP(objectPoints, imagePoints, cameraMatrix, distortionCoeffs, rvec, tvec, false, Emgu.CV.CvEnum.SolvePnpMethod.IPPE);
//Mat rotationMatrix = new Mat();
//CvInvoke.Rodrigues(rvec, rotationMatrix);
//trans = new double[3, 4];
//double[] rotationMatrixArray = new double[12];
//Marshal.Copy(rotationMatrix.DataPointer, rotationMatrixArray, 0, 12);
//double[] translationMatrixArray = new double[3];
//Marshal.Copy(tvec.DataPointer, translationMatrixArray, 0, 3);
//for (int j = 0; j < 3; j++) {
// for (int i = 0; i < 3; i++) {
// trans[j, i] = rotationMatrixArray[3 * j + i];
// }
// trans[j, 3] = translationMatrixArray[j];
//}
//mv = Trans2OpenGL(trans);
}
}
DrawCornersOnImage(imageCopy, cornersErr, System.Drawing.Color.Green);
//if (cornersErr2.Size > 0) DrawCornersOnImage(imageCopy, cornersErr2, System.Drawing.Color.Red);
CvInvoke.Imwrite(Path.GetDirectoryName(myFile) + "\\Corners-" + Path.GetFileNameWithoutExtension(myFile) + ".png", imageCopy, new KeyValuePair <Emgu.CV.CvEnum.ImwriteFlags, int>(Emgu.CV.CvEnum.ImwriteFlags.PngCompression, 3));
}