public static void DrawCornersOnImage(Mat image, Emgu.CV.Util.VectorOfPointF cornerPoints, System.Drawing.Color col) { for (int i = 0; i < cornerPoints.Size; i++) { CvInvoke.Line(image, new Point((int)cornerPoints[i].X, (int)cornerPoints[i].Y), new Point((int)cornerPoints[i].X, (int)cornerPoints[i].Y), new Bgr(col).MCvScalar, 1); } }
public static Emgu.CV.Util.VectorOfPoint ToVectorOfPoint(this Emgu.CV.Util.VectorOfPointF vectorOfPointF) { List <Point> pts = new List <Point>(); for (int i = 0; i < vectorOfPointF.Size; i++) { pts.Add(Point.Round(vectorOfPointF[i])); } return(new Emgu.CV.Util.VectorOfPoint(pts.ToArray())); }
public static Emgu.CV.Util.VectorOfPointF DetectEllipses(Image <Gray, byte> grayImage, Mat imageCopy) { var centerPoints = new Emgu.CV.Util.VectorOfPointF(); var contours = new Emgu.CV.Util.VectorOfVectorOfPoint(); Mat heirarchy = null; var grayImageCopy = grayImage.Clone(); var res = CvInvoke.Threshold(grayImage, grayImageCopy, 170, 255, Emgu.CV.CvEnum.ThresholdType.Binary); //CvInvoke.Imwrite(Path.GetDirectoryName(myFile) + "\\" + Path.GetFileNameWithoutExtension(myFile) + "-threshold" + Path.GetExtension(myFile), grayImageCopy, new KeyValuePair<Emgu.CV.CvEnum.ImwriteFlags, int>(Emgu.CV.CvEnum.ImwriteFlags.JpegQuality, 95)); grayImageCopy._Not(); CvInvoke.FindContours(grayImageCopy, contours, heirarchy, Emgu.CV.CvEnum.RetrType.Ccomp, Emgu.CV.CvEnum.ChainApproxMethod.ChainApproxSimple); // var circles = CvInvoke.HoughCircles(grayImage, Emgu.CV.CvEnum.HoughType.Gradient, 1, grayImage.Rows / 16); if (contours.Size > 0) { double largestArea = 0; for (int i = 0; i < contours.Size; i++) { var contour = contours[i]; if (contour.Size > 4) { var rect = CvInvoke.FitEllipse(contour); var area = rect.Size.Width * rect.Size.Height; var width = rect.Size.Width > rect.Size.Height ? rect.Size.Width : rect.Size.Height; var height = rect.Size.Width > rect.Size.Height ? rect.Size.Height : rect.Size.Width; if (area > 1000 && width / height < 3) { var averageDist = AverageDistanceToEllipse(contour, rect); var furthestDist = FurthestDistanceToEllipse(contour, rect); if (averageDist < 1.5 && furthestDist < 4) { if (area > largestArea) { largestArea = area; } centerPoints.Push(new PointF[] { rect.Center }); DrawContoursOnImage(imageCopy, contours[i]); // CvInvoke.Ellipse(imageCopy, rect, new Bgr(System.Drawing.Color.Red).MCvScalar); CvInvoke.Line(imageCopy, new Point((int)rect.GetVertices()[0].X, (int)rect.GetVertices()[0].Y), new Point((int)rect.GetVertices()[1].X, (int)rect.GetVertices()[1].Y), new Bgr(System.Drawing.Color.Red).MCvScalar); CvInvoke.Line(imageCopy, new Point((int)rect.GetVertices()[1].X, (int)rect.GetVertices()[1].Y), new Point((int)rect.GetVertices()[2].X, (int)rect.GetVertices()[2].Y), new Bgr(System.Drawing.Color.Red).MCvScalar); CvInvoke.Line(imageCopy, new Point((int)rect.GetVertices()[2].X, (int)rect.GetVertices()[2].Y), new Point((int)rect.GetVertices()[3].X, (int)rect.GetVertices()[3].Y), new Bgr(System.Drawing.Color.Red).MCvScalar); CvInvoke.Line(imageCopy, new Point((int)rect.GetVertices()[3].X, (int)rect.GetVertices()[3].Y), new Point((int)rect.GetVertices()[0].X, (int)rect.GetVertices()[0].Y), new Bgr(System.Drawing.Color.Red).MCvScalar); } } } } } return(centerPoints); }
static bool CheckConnectedComponents(Mat grayImage) { // Threshold using Otsu bi-modal (black&white) assumption Mat binaryImage = grayImage.Clone(); double otsuThreshold = CvInvoke.Threshold(grayImage, binaryImage, 0.0, 255.0, Emgu.CV.CvEnum.ThresholdType.Otsu | Emgu.CV.CvEnum.ThresholdType.Binary); // dilate to connect two squares Mat kernel = new Mat(); CvInvoke.Dilate(binaryImage, binaryImage, kernel, new Point(-1, -1), 1, Emgu.CV.CvEnum.BorderType.Constant, CvInvoke.MorphologyDefaultBorderValue); CvInvoke.Imwrite("C:\\Temp\\Dilate.png", binaryImage, new KeyValuePair <Emgu.CV.CvEnum.ImwriteFlags, int>(Emgu.CV.CvEnum.ImwriteFlags.PngCompression, 3)); // compute number of labels (should be 2: 0 for background, 1 for white) Mat labelRegion = new Mat(new System.Drawing.Size(binaryImage.Width, binaryImage.Height), Emgu.CV.CvEnum.DepthType.Cv32S, 1); Mat statistics = new Mat(); Mat centroids = new Mat(); var numberOfLabels = CvInvoke.ConnectedComponentsWithStats(binaryImage, labelRegion, statistics, centroids, Emgu.CV.CvEnum.LineType.EightConnected, Emgu.CV.CvEnum.DepthType.Cv32S); Console.WriteLine(" - Number of labels: %d\n", numberOfLabels); if (numberOfLabels != 2) { return(false); } // compute centers of background and foreground (should also be close to image center) Emgu.CV.Util.VectorOfPoint imageCentre = new Emgu.CV.Util.VectorOfPoint(new Point [] { new Point((int)(grayImage.Cols / 2.0f), (int)(grayImage.Rows / 2.0f)) }); Emgu.CV.Util.VectorOfPointF blackCenter = new Emgu.CV.Util.VectorOfPointF(new PointF[] { new PointF((float)centroids.GetDoubleValue(0, 0), (float)centroids.GetDoubleValue(0, 1)) }); Emgu.CV.Util.VectorOfPointF whiteCenter = new Emgu.CV.Util.VectorOfPointF(new PointF[] { new PointF((float)centroids.GetDoubleValue(1, 0), (float)centroids.GetDoubleValue(1, 1)) }); var blackCentroidDistance = CvInvoke.Norm(blackCenter, imageCentre, Emgu.CV.CvEnum.NormType.L2); var whiteCentroidDistance = CvInvoke.Norm(whiteCenter, imageCentre, Emgu.CV.CvEnum.NormType.L2); for (var label = 0; label < numberOfLabels; label++) { Console.WriteLine(" - [%d] centroid at (%.1lf,%.1lf)\n", label, (float)centroids.GetDoubleValue(label, 0), (float)centroids.GetDoubleValue(label, 1)); } return(numberOfLabels == 2 && blackCentroidDistance < 10.0 && whiteCentroidDistance < 10.0); }
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(); }
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 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]; } }
public static void initBundleAdjust(string myCalibFile, int xsize, int ysize, int npoints, float[] points, int nMeasurements, float[] matrices) { // camera related parameters Mat cameraMat; Mat distCoeffs; LoadCameraFromFile(myCalibFile, out cameraMat, out distCoeffs); Size cameraRes = new Size(xsize, ysize); List <Emgu.CV.Structure.MCvPoint3D32f> objectPoints = new List <Emgu.CV.Structure.MCvPoint3D32f>(); objectPoints.Add(new Emgu.CV.Structure.MCvPoint3D32f(points[0], points[1], points[2])); var points3d = new Emgu.CV.Util.VectorOfPoint3D32F(objectPoints.ToArray()); var points2d = new Emgu.CV.Util.VectorOfVectorOfPointF(); var visibility = new Emgu.CV.Util.VectorOfVectorOfInt(); var cameraMatrix = new Emgu.CV.Util.VectorOfMat(); var R_true = new Emgu.CV.Util.VectorOfMat(); var T_true = new Emgu.CV.Util.VectorOfMat(); var distortionCoeffs = new Emgu.CV.Util.VectorOfMat(); Emgu.CV.Structure.MCvTermCriteria criteria = new Emgu.CV.Structure.MCvTermCriteria(70, 1e-10); // define cameras for (int i = 0; i < nMeasurements; i++) { cameraMatrix.Push(cameraMat); distortionCoeffs.Push(distCoeffs); Mat _R_true = 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 + i] = matrices[i * 12 + j * 3 + k]; } } Marshal.Copy(matrixArray, 0, _R_true.DataPointer, 9); R_true.Push(_R_true); Mat _T_true = new Mat(3, 1, Emgu.CV.CvEnum.DepthType.Cv64F, 1); double[] vectorArray = new double[3]; for (int j = 0; j < 3; j++) { vectorArray[j] = matrices[i * 12 + 9 + j]; } Marshal.Copy(vectorArray, 0, _T_true.DataPointer, 3); T_true.Push(_T_true); } // project points to image coordinates for (int i = 0; i < nMeasurements; i++) { // project var imagePoints2 = new Emgu.CV.Util.VectorOfPointF(); Emgu.CV.CvInvoke.ProjectPoints(points3d, R_true[i], T_true[i], cameraMatrix[i], distortionCoeffs[i], imagePoints2); // check if the point is in camera shot Emgu.CV.Util.VectorOfInt vis = new Emgu.CV.Util.VectorOfInt(1); // if the image point is within camera resolution then the point is visible if ((0 <= imagePoints2[0].X) && (imagePoints2[0].X <= cameraRes.Width) && (0 <= imagePoints2[0].Y) && (imagePoints2[0].Y <= cameraRes.Height)) { vis.Push(new int[] { 1 }); } // else, the point is not visible else { vis.Push(new int[] { 0 }); } points2d.Push(imagePoints2); visibility.Push(vis); } //Emgu.CV. // cv::LevMarqSparse lv; //lv.bundleAdjust(points_true, imagePoints, visibility, cameraMatrix, R_true, T_true, distCoeffs, criteria); }
private void button1_Click(object sender, EventArgs e) { var myDlg = new FolderBrowserDialog(); myDlg.SelectedPath = "C:\\Customer\\Stannah\\Photogrammetry\\Photos"; var ret = myDlg.ShowDialog(); if (ret != DialogResult.OK) { return; } var myFolder = myDlg.SelectedPath; var myFiles = new List <string>(); int myWidth = 0, myHeight = 0, nImages = 0, nCount = 0; foreach (string myFile in Directory.GetFiles(myFolder)) { if (Path.GetFileNameWithoutExtension(myFile).ToLower().StartsWith("calibration") && myFile.ToLower().EndsWith(".png") && !myFile.ToLower().Contains("-adj.png")) { if (Path.GetFileNameWithoutExtension(myFile).Contains("17")) { continue; } nImages = nImages + 1; myFiles.Add(myFile); if (myWidth == 0) { Image myImage = Image.FromFile(myFile); myWidth = myImage.Width; myHeight = myImage.Height; } } } // ARToolKitFunctions.Instance.arwInitialiseAR(); ARToolKitFunctions.Instance.arwInitChessboardCorners(17, 13, 20, 3264, 2448, nImages); float[] corners = new float[442]; int cornerCount; //string cornerFile = "C:\\Temp\\CornersARToolkit.txt"; //if (File.Exists(cornerFile)) { // try { // File.Delete(cornerFile); // } // catch (Exception ex) { // string s = ex.ToString(); // } //} myFiles.Sort(new AlphaNumericCompare()); //StreamWriter sw = new StreamWriter(cornerFile); foreach (string myFile in myFiles) { var image = new Image <Gray, byte>(myFile); var size = image.Width * image.Height; var cornerPoints = new Emgu.CV.Util.VectorOfPointF(); var mBoardSize = new Size(13, 17); var res = CvInvoke.FindChessboardCorners(image, mBoardSize, cornerPoints); byte[] imageBytes = new Byte[size]; System.Buffer.BlockCopy(image.Data, 0, imageBytes, 0, size); //byte[] imageBytes = ImageToGrayscaleByteArray((Bitmap)Image.FromFile(myFile)); int result = ARToolKitFunctions.Instance.arwFindChessboardCorners(corners, out cornerCount, imageBytes); var imagePoints = new Emgu.CV.Util.VectorOfPointF(); int l = 0; for (int i = 0; i < 17; i++) { for (int j = 0; j < 13; j++) { //sw.WriteLine(corners[l * 2].ToString() + '\t' + corners[l * 2 + 1].ToString()); imagePoints.Push(new PointF[] { new PointF(corners[l * 2], corners[l * 2 + 1]) }); l++; } } if (result == 1) { if (imagePoints.Size > 0) { Mat imageCopy = Emgu.CV.CvInvoke.Imread(myFile, Emgu.CV.CvEnum.ImreadModes.Color); if (imagePoints.Size > 0) { mdlEmguDetection.DrawCornersOnImage(imageCopy, imagePoints, System.Drawing.Color.Green); } CvInvoke.Imwrite(Path.GetDirectoryName(myFile) + "\\Corners-" + Path.GetFileNameWithoutExtension(myFile) + ".png", imageCopy, new KeyValuePair <Emgu.CV.CvEnum.ImwriteFlags, int>(Emgu.CV.CvEnum.ImwriteFlags.PngCompression, 3)); } cornerCount = ARToolKitFunctions.Instance.arwCaptureChessboardCorners(); System.Diagnostics.Debug.Print("Processed image " + cornerCount.ToString()); if (cornerCount == nImages) { nCount = nCount + 1; float[] reprojectionErrors = new float[nImages]; float reprojectionError = ARToolKitFunctions.Instance.arwCalibChessboardCorners(nImages, "C:\\Temp\\Calib.dat", out reprojectionErrors); System.Diagnostics.Debug.Print("Total reprojection error: " + reprojectionError.ToString()); for (int i = 0; i < reprojectionErrors.Length; i++) { System.Diagnostics.Debug.Print("Reprojection error " + (i + 1).ToString() + ": " + reprojectionErrors[i].ToString()); } } } else { System.Diagnostics.Debug.Print("Failed to process image " + myFile); } } //sw.Close(); }
private static void GetCenterPointForDatum(clsPoint pt, double[,] model, ARParam arParams, int[] vp, Image <Gray, byte> grayImage, ref Emgu.CV.Util.VectorOfPointF centerPoints) { var cpt = ModelToImageSpace(arParams, model, pt); var halfSquare = GetSquareForDatum(arParams, model, pt); if (halfSquare < 8) { return; } if (cpt.x - halfSquare < 0 || cpt.x + halfSquare > vp[2] || cpt.y - halfSquare <0 || cpt.y + halfSquare> vp[3]) { return; } var rect = new Rectangle((int)cpt.x - halfSquare, (int)cpt.y - halfSquare, 2 * halfSquare, 2 * halfSquare); var region = new Mat(grayImage.Mat, rect); var binaryRegion = region.Clone(); double otsuThreshold = CvInvoke.Threshold(region, binaryRegion, 0.0, 255.0, Emgu.CV.CvEnum.ThresholdType.Otsu); int nonzero = CvInvoke.CountNonZero(binaryRegion); var square = 4 * halfSquare * halfSquare; if (nonzero > square * 0.2f && nonzero < square * 0.8f) { centerPoints.Push(new PointF[] { new PointF((float)cpt.X, (float)cpt.Y) }); } }
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)); }