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