protected void FindCorners(byte[] frame)
{
var mat = Cv2.ImDecode(frame, ImreadModes.Color);
var corners = new Mat <Point2f>();
var cornersFound = Cv2.FindChessboardCorners(
mat, _boardSize, corners,
ChessboardFlags.AdaptiveThresh |
ChessboardFlags.FastCheck |
ChessboardFlags.NormalizeImage);
if (!cornersFound)
{
return;
}
using (var grayFrame = new Mat())
{
Cv2.CvtColor(mat, grayFrame, ColorConversionCodes.BGR2GRAY);
var correctedCorners = Cv2.CornerSubPix(
grayFrame, corners, new Size(11, 11), new Size(-1, -1),
new TermCriteria(CriteriaType.Eps | CriteriaType.MaxIter, 30, 0.1));
_chessboardCorners.Enqueue(correctedCorners);
}
mat.Dispose();
}
static void Main(string[] args)
{
Mat src = Cv2.ImRead("dummy.jpg");
Mat gray = new Mat();
Mat dst = src.Clone();
Cv2.CvtColor(src, gray, ColorConversionCodes.BGR2GRAY);
Point2f[] corners = Cv2.GoodFeaturesToTrack(gray, 100, 0.03, 5, null, 3, false, 0);
Point2f[] sub_corners = Cv2.CornerSubPix(gray, corners, new Size(3, 3), new Size(-1, -1), TermCriteria.Both(10, 0.03));
for (int i = 0; i < corners.Length; i++)
{
Point pt = new Point((int)corners[i].X, (int)corners[i].Y);
Cv2.Circle(dst, pt, 5, Scalar.Yellow, Cv2.FILLED);
}
for (int i = 0; i < sub_corners.Length; i++)
{
Point pt = new Point((int)sub_corners[i].X, (int)sub_corners[i].Y);
Cv2.Circle(dst, pt, 5, Scalar.Red, Cv2.FILLED);
}
Cv2.ImShow("dst", dst);
Cv2.WaitKey(0);
Cv2.DestroyAllWindows();
}
protected override void CheckIfDetectedMarkers()
{
base.CheckIfDetectedMarkers();
for (int i = 0; i < ids.Length; i++)
{
Cv2.CornerSubPix(grayedImg, corners[i], new Size(5, 5), new Size(-1, -1), TermCriteria.Both(30, 0.1));
if (!MarkerManager.IsMarkerRegistered(ids[i]))
{
continue;
}
MarkerBehaviour m = MarkerManager.GetMarker(ids[i]);
if (!allDetectedMarkers.ContainsKey(ids[i]))
{
m.OnMarkerDetected.Invoke();
allDetectedMarkers.Add(m.GetMarkerID(), m);
}
// m.UpdateMarker(img.Cols, img.Rows, corners[i], rejectedImgPoints[i]);
m.UpdateMarker(corners[i], calibrationData.GetCameraMatrix(),
calibrationData.GetDistortionCoefficients(), grayedImg);
}
}
//Creates the Transformation matrix which transforms the marker from OpenCv camera space to unity world space.
//Here "k" is the camera matrix and "d" is the distortion coefficients received from the camera calibration
//If a grayed version of the img is passed then "CornerSubPix" algorithm from open cv will be applied which makes the
//marker pose better.
private Matrix4x4 CreateTransformationMatrix(double[,] k, double[] d, Mat grayMat = null)
{
if (currentMarkerData.corners.Length == 0)
{
Debug.LogError("Marker Is Not Updated");
}
float markerSizeInMeters = sizeInMeters;
//local space marker corner points
Point3f[] markerPoints = new Point3f[]
{
new Point3f(-markerSizeInMeters / 2f, markerSizeInMeters / 2f, 0f),
new Point3f(markerSizeInMeters / 2f, markerSizeInMeters / 2f, 0f),
new Point3f(markerSizeInMeters / 2f, -markerSizeInMeters / 2f, 0f),
new Point3f(-markerSizeInMeters / 2f, -markerSizeInMeters / 2f, 0f)
};
//create rotation/translating arrays
double[] rvec = new double[3] {
0d, 0d, 0d
};
double[] tvec = new double[3] {
0d, 0d, 0d
};
//create rotation matrix
double[,] rotMatrix = new double[3, 3] {
{ 0d, 0d, 0d }, { 0d, 0d, 0d }, { 0d, 0d, 0d }
};
//apply CornerSubPix algorithm if needed
if (grayMat != null)
{
Cv2.CornerSubPix(grayMat, currentMarkerData.corners, new Size(5, 5), new Size(-1, -1),
TermCriteria.Both(30, 0.001));
}
//Use SolvePnP algorithm to fill the above arrays with transformation data from open cv
Cv2.SolvePnP(markerPoints, currentMarkerData.corners, k, d, out rvec, out tvec, false, SolvePnPFlags.Iterative);
//Use Rodrigues algorithm to fill the rotation arrays with rotation data from open cv
Cv2.Rodrigues(rvec, out rotMatrix);
//Apply all the data to an Unity matrix for ease of use.
Matrix4x4 matrix = new Matrix4x4();
matrix.SetRow(0,
new Vector4((float)rotMatrix[0, 0], (float)rotMatrix[0, 1], (float)rotMatrix[0, 2], (float)tvec[0]));
matrix.SetRow(1,
new Vector4((float)rotMatrix[1, 0], (float)rotMatrix[1, 1], (float)rotMatrix[1, 2], (float)tvec[1]));
matrix.SetRow(2,
new Vector4((float)rotMatrix[2, 0], (float)rotMatrix[2, 1], (float)rotMatrix[2, 2], (float)tvec[2]));
matrix.SetRow(3, new Vector4(0f, 0f, 0f, 1f));
return(matrix);
}
protected override void CheckIfDetectedMarkers()
{
base.CheckIfDetectedMarkers();
for (int i = 0; i < ids.Length; i++)
{
Cv2.CornerSubPix(grayedImg, corners[i], new Size(5, 5), new Size(-1, -1), TermCriteria.Both(30, 0.1));
if (!MarkerManager.IsMarkerRegistered(ids[i]))
{
continue;
}
MarkerBehaviour m = MarkerManager.GetMarker(ids[i]);
if (!allDetectedMarkers.ContainsKey(ids[i]))
{
Debug.Log("FOUND MARKER: " + m.GetMarkerID());
m.OnMarkerDetected.Invoke();
allDetectedMarkers.Add(m.GetMarkerID(), m);
}
float rotZ = 0;
switch (Screen.orientation)
{
case ScreenOrientation.Portrait:
rotZ = 90;
break;
case ScreenOrientation.LandscapeLeft:
rotZ = 180;
break;
case ScreenOrientation.LandscapeRight:
rotZ = 0;
break;
case ScreenOrientation.PortraitUpsideDown:
rotZ = -90;
break;
}
if (!UseCustomCalibration)
{
cameraManager.TryGetIntrinsics(out cameraIntrinsics);
m.UpdateMarker(corners[i], cameraIntrinsics, grayedImg, Vector3.forward * rotZ);
}
else
{
m.UpdateMarker(corners[i], calibrationData.GetCameraMatrix(), calibrationData.GetDistortionCoefficients(), grayedImg, Vector3.forward * rotZ);
}
}
}
void CamUpdate()
{
CvUtil.GetWebCamMat(webCamTexture, ref mat);
Cv2.CvtColor(mat, gray, ColorConversionCodes.RGBA2GRAY);
Point2f[] corners;
bool ret = Cv2.FindChessboardCorners(gray, size, out corners);
if (ret)
{
TermCriteria criteria = TermCriteria.Both(30, 0.001f);
Point2f[] corners2 = Cv2.CornerSubPix(gray, corners, size, new Size(-1, -1), criteria);
Cv2.DrawChessboardCorners(mat, size, corners2, ret);
List <Point3f> lObjectPoints = new List <Point3f>();
for (int i = 0; i < size.Width; i++)
{
for (int j = 0; j < size.Height; j++)
{
lObjectPoints.Add(new Point3f(i, j, 0) * cellSize);
}
}
var objectPoints = new List <IEnumerable <Point3f> > {
lObjectPoints
};
var imagePoints = new List <IEnumerable <Point2f> > {
corners2
};
double[,] cameraMatrix = new double[3, 3];
double[] distCoefficients = new double[5];
Vec3d[] rvecs, tvecs;
Cv2.CalibrateCamera(objectPoints, imagePoints, mat.Size(), cameraMatrix, distCoefficients, out rvecs, out tvecs);
print(
cameraMatrix[0, 0] + ", " + cameraMatrix[0, 1] + ", " + cameraMatrix[0, 2] + "\n" +
cameraMatrix[1, 0] + ", " + cameraMatrix[1, 1] + ", " + cameraMatrix[1, 2] + "\n" +
cameraMatrix[2, 0] + ", " + cameraMatrix[2, 1] + ", " + cameraMatrix[2, 2]
);
print(tvecs[0].Item0 + ", " + tvecs[0].Item1 + ", " + tvecs[0].Item2);
}
CvConvert.MatToTexture2D(mat, ref tex);
rawImage.texture = tex;
}
//Capture a rendered texture frame and register the checkerboard pattern data
public void RegisterCurrentCalib()
{
corners.Clear();
obj.Clear();
//imagePoints.Clear();
//objPoints.Clear();
bool b = false;
//find the corners and populate the data for one sqaure
b = Cv2.FindChessboardCorners(mat, boardSize, OutputArray.Create(corners),
ChessboardFlags.AdaptiveThresh | ChessboardFlags.NormalizeImage | ChessboardFlags.FastCheck);
if (!b)
{
return;
}
Cv2.CornerSubPix(grayMat, corners, new Size(5, 5), new Size(-1, -1), TermCriteria.Both(30, 0.1));
Debug.Log(b);
// for debug draw the found squares
Cv2.DrawChessboardCorners(mat, boardSize, corners, b);
for (int i = 0; i < boardSize.Height; i++)
{
for (int j = 0; j < boardSize.Width; j++)
{
//add the space coordinates of the squares. Z = 0 since its a flat plane.
obj.Add(new Point3f((float)j * squareSizeMeters, (float)i * squareSizeMeters, 0));
if (b)
{
//register the data per square
CornerPoints.Add(corners);
objPoints.Add(obj);
}
}
}
}
private void Read_Corners_Click(object sender, EventArgs e)
{
BoardSize = new OpenCvSharp.Size(Convert.ToInt16(Corners_Nx.Text), Convert.ToInt16(Corners_Ny.Text));
WriteMessage("开始读取交点：" + Corners_Nx.Text + "," + Corners_Ny.Text + ",共：" + FileList.Length.ToString() + "个文件");
for (int i = 0; i < FileList.Length; i++)
{
String cFileName = FileList[i];
WriteMessage("开始读取第(" + (i + 1) + ")个文件:" + cFileName);
Mat vImage = Cv2.ImRead(cFileName, ImreadModes.AnyColor);
Mat vGrayImage = Cv2.ImRead(cFileName, ImreadModes.Grayscale);
if (i == 0)
{
ImageSize = new OpenCvSharp.Size(vImage.Cols, vImage.Rows);
int iImageCount = FileList.Length;
int iPointCount = BoardSize.Width * BoardSize.Height;
CPointList = new Point2f[iImageCount][];
GPointList = new Point3f[iImageCount][];
}
Boolean isOK = false;
Point2f[] ptList = new Point2f[BoardSize.Width * BoardSize.Height];
if (!Cv2.FindChessboardCorners(vImage, BoardSize, out ptList))
{
WriteMessage("第(" + (i + 1) + ")个文件：FindChessboardCorners..转换失败！");
continue;
}
else
{
WriteMessage("第(" + (i + 1) + ")个文件,FindChessboardCorners成功");
}
Cv2.DrawChessboardCorners(vImage, BoardSize, ptList, true);
Point2f[] ptSubPixList = new Point2f[BoardSize.Width * BoardSize.Height];
try
{
ptSubPixList = Cv2.CornerSubPix(vGrayImage, ptList, new OpenCvSharp.Size(ImageSize.Width / 2 - 5, ImageSize.Height / 2 - 5), new OpenCvSharp.Size(-1, -1), new TermCriteria(CriteriaType.MaxIter, 10, 0.1));
if (ptSubPixList.Length != BoardSize.Width * BoardSize.Height)
{
continue;
}
else
{
isOK = true;
}
}
catch (Exception ex)
{
WriteMessage(ex.Message);
}
if (isOK)
{
WriteMessage("第(" + (i + 1) + ")个文件,CornerSubPix成功");
}
else
{
WriteMessage("第(" + (i + 1) + ")个文件：CornerSubPix..转换失败！");
}
CPointList[i] = ptSubPixList;
if (i == FileList.Length - 1)
{
Cv2.DrawChessboardCorners(vImage, BoardSize, ptSubPixList, true);
vImage.SaveImage("LS.jpg");
this.pictureBox1.Load("LS.jpg");
}
}
WriteMessage("交点读取完成！");
}
private async void DoCalibrationButton_Click(object sender, Windows.UI.Xaml.RoutedEventArgs e)
{
var objList = new List <Point3f>();
var objPoints = new List <Point3f[]>();
var imgPoints = new List <Point2f[]>();
var chessboardSize = new Size(7, 5);
var terminationCriteria = new TermCriteria(CriteriaType.Eps | CriteriaType.MaxIter, 30, 0.001);
for (int y = 0; y < chessboardSize.Height; y++)
{
for (int x = 0; x < chessboardSize.Width; x++)
{
var point = new Point3f
{
X = x,
Y = y,
Z = 0
};
objList.Add(point);
}
}
foreach (var ci in calibrateImages)
{
var img = new Mat(ci.Height, ci.Width, MatType.CV_8UC4, ci.Buffer);
Mat grayImg = new Mat();
Point2f[] corners;
Cv2.CvtColor(img, grayImg, ColorConversionCodes.RGBA2GRAY);
var result = Cv2.FindChessboardCorners(grayImg, chessboardSize, out corners, ChessboardFlags.None);
if (result)
{
var winSize = new Size(11, 11);
var zeroZone = new Size(-1, -1);
var refinedCorners = Cv2.CornerSubPix(grayImg, corners, winSize, zeroZone, terminationCriteria);
objPoints.Add(objList.ToArray());
imgPoints.Add(corners);
Cv2.DrawChessboardCorners(img, chessboardSize, refinedCorners, result);
Cv2.ImShow("img", img);
Cv2.WaitKey(500);
}
}
if (objPoints.Count > 0)
{
var cameraMat = new double[3, 3];
var distCoeffVec = new double[14];
var rVecs = new Vec3d[0];
var tVecs = new Vec3d[0];
var calResult = Cv2.CalibrateCamera(objPoints, imgPoints, new Size(frameWidth, frameHeight), cameraMat, distCoeffVec, out rVecs, out tVecs);
}
calibrateImages.Clear();
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
ImageCount.Text = "Calibration Image Count: " + calibrateImages.Count;
});
}
public static Dictionary <string, double> Find(Mat img, System.Windows.Point squares, System.Windows.Point roiCenter, System.Windows.Point roiSize, bool export)
{
using var _img = img.Clone();
int im_width = _img.Cols / 2;
int im_height = _img.Rows / 2;
var clarity = VarianceOfLaplacian(img);
var roi = new Rect(im_width - (int)roiCenter.X - (int)roiSize.X / 2, im_height - (int)roiCenter.Y - (int)roiSize.Y / 2, (int)roiSize.X, (int)roiSize.Y);
using var _img_crop = new Mat(_img, roi);
using var _img_gray = _img_crop.Clone();
_img_crop.ConvertTo(_img_gray, -1, 1, 0);
Cv2.CvtColor(_img_gray, _img_gray, ColorConversionCodes.BGR2GRAY);
int chessboardCornersPerCol = (int)squares.X - 1;
int chessboardCornersPerRow = (int)squares.Y - 1;
var board_sz = new Size(chessboardCornersPerRow, chessboardCornersPerCol);
bool found = Cv2.FindChessboardCorners(_img_gray, board_sz, out Point2f[] corners, ChessboardFlags.AdaptiveThresh | ChessboardFlags.NormalizeImage);
CameraProperties cam = new CameraProperties();
if (found)
{
var termcrit = new TermCriteria(CriteriaTypes.Eps | CriteriaTypes.Count, 30, 0.001);
Point2f[] cornerSubPix = Cv2.CornerSubPix(_img_gray, corners, new Size(11, 11), new Size(-1, -1), termcrit);
var chessImg = new Mat(_img_crop.Cols, _img_crop.Rows, MatType.CV_8UC3, new Scalar(0.0, 0.0, 0.0, 255.0));
Cv2.DrawChessboardCorners(chessImg, board_sz, cornerSubPix, found);
var matCorners = new Mat(rows: chessboardCornersPerRow, cols: chessboardCornersPerCol, type: MatType.CV_32FC2, data: cornerSubPix);
var pointRow = matCorners.Reduce(ReduceDimension.Row, ReduceTypes.Avg, -1);
var pointCol = matCorners.Reduce(ReduceDimension.Column, ReduceTypes.Avg, -1);
var matCenter = pointCol.Reduce(ReduceDimension.Row, ReduceTypes.Avg, -1);
if (export)
{
_ = PrintMatAsync(matCorners);
}
var vecRow = pointRow.At <Point2f>(0, pointRow.Cols - 1) - pointRow.At <Point2f>(0, 0);
var vecCol = pointCol.At <Point2f>(pointCol.Rows - 1, 0) - pointCol.At <Point2f>(0, 0);
double rotationAngleRadians = Math.Abs(vecRow.X) > Math.Abs(vecCol.X) ? Math.Atan(vecRow.Y / vecRow.X) : Math.Atan(vecCol.Y / vecCol.X);
cam.Rotation = 180 * rotationAngleRadians / Math.PI;
float[] arrRow = { vecRow.X, vecRow.Y };
float[] arrCol = { vecCol.X, vecCol.Y };
var res_x = 1.0 / Math.Sqrt(Cv2.Norm(InputArray.Create(arrRow)) / (chessboardCornersPerRow - 1) * Cv2.Norm(InputArray.Create(arrRow)) / (chessboardCornersPerRow - 1));
var res_y = 1.0 / Math.Sqrt(Cv2.Norm(InputArray.Create(arrCol)) / (chessboardCornersPerCol - 1) * Cv2.Norm(InputArray.Create(arrCol)) / (chessboardCornersPerCol - 1));
cam.Resolution = new System.Windows.Point(res_x, res_y);
var center = new Point2f(matCenter.At <Point2f>(0).X, matCenter.At <Point2f>(0).Y);
cam.Center = new System.Windows.Point((center.X - roi.Width / 2) * cam.Resolution.X, (center.Y - roi.Height / 2) * cam.Resolution.Y);
//CameraCalibration(_img_crop, board_sz, cornerSubPix);
var roi_img = new Mat(img, roi);
chessImg.CopyTo(roi_img);
}
Cv2.Rectangle(img, roi, new Scalar(0, 0, 255));
var dict = new Dictionary <string, double>
{
{ "X um / px", cam.Resolution.X * 1000 },
{ "Y um / px", cam.Resolution.Y * 1000 },
{ "Rotation [*]", cam.Rotation },
{ "Center X [um]", cam.Center.X * 1000 },
{ "Center Y [um]", cam.Center.Y * 1000 },
{ "Clarity", clarity }
};
return(dict);
}
//Creates the Transformation matrix which transforms the marker from OpenCv camera space to unity world space.
//Here XRCameraIntrinsics is an ARFoundation which contains camera calibration info done by ARFoundation
//If a grayed version of the img is passed then "CornerSubPix" algorithm from open cv will be applied which makes the
//marker pose better.
private Matrix4x4 CreateTransformationMatrix(XRCameraIntrinsics cameraIntrinsics, Mat grayMat = null)
{
if (currentMarkerData.corners.Length == 0)
{
Debug.LogError("Marker Is Not Updated");
}
float markerSizeInMeters = sizeInMeters;
//local space marker corner points
Point3f[] markerPoints = new Point3f[]
{
new Point3f(-markerSizeInMeters / 2f, markerSizeInMeters / 2f, 0f),
new Point3f(markerSizeInMeters / 2f, markerSizeInMeters / 2f, 0f),
new Point3f(markerSizeInMeters / 2f, -markerSizeInMeters / 2f, 0f),
new Point3f(-markerSizeInMeters / 2f, -markerSizeInMeters / 2f, 0f)
};
double[,] rawCameraMatrix;
float fx = 0;
float fy = 0;
float cx = 0;
float cy = 0;
//Use the Landscape left orientation for our camera camera intrinsics since
//the picture that goes to opencv its always oriented that way so we need to edit our principal points,
//by rotation them 270 degrees.
fx = cameraIntrinsics.focalLength.x;
fy = cameraIntrinsics.focalLength.y;
cx = (float)(cameraIntrinsics.resolution.x) - cameraIntrinsics.principalPoint.x;
cy = (float)(cameraIntrinsics.resolution.y) - cameraIntrinsics.principalPoint.y;
rawCameraMatrix = new double[3, 3]
{
{ fx, 0d, cx },
{ 0d, fy, cy },
{ 0d, 0d, 1d }
};
//create rotation/translating arrays
double[] rvec = new double[3] {
0d, 0d, 0d
};
double[] tvec = new double[3] {
0d, 0d, 0d
};
//create rotation matrix
double[,] rotMatrix = new double[3, 3] {
{ 0d, 0d, 0d }, { 0d, 0d, 0d }, { 0d, 0d, 0d }
};
//apply CornerSubPix algorithm if needed
if (grayMat != null)
{
Cv2.CornerSubPix(grayMat, currentMarkerData.corners, new Size(5, 5), new Size(-1, -1),
TermCriteria.Both(30, 0.001));
}
//Use SolvePnP algorithm to fill the above arrays with transformation data from open cv
Cv2.SolvePnP(markerPoints, currentMarkerData.corners, rawCameraMatrix, new double[5] {
0, 0, 0, 0, 0
}, out rvec,
out tvec, false, SolvePnPFlags.Iterative);
//Use Rodrigues algorithm to fill the rotation arrays with rotation data from open cv
Cv2.Rodrigues(rvec, out rotMatrix);
Matrix4x4 matrix = new Matrix4x4();
matrix.SetRow(0,
new Vector4((float)rotMatrix[0, 0], (float)rotMatrix[0, 1], (float)rotMatrix[0, 2], (float)tvec[0]));
matrix.SetRow(1,
new Vector4((float)rotMatrix[1, 0], (float)rotMatrix[1, 1], (float)rotMatrix[1, 2], (float)tvec[1]));
matrix.SetRow(2,
new Vector4((float)rotMatrix[2, 0], (float)rotMatrix[2, 1], (float)rotMatrix[2, 2], (float)tvec[2]));
matrix.SetRow(3, new Vector4(0f, 0f, 0f, 1f));
return(matrix);
}
