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