public virtual void Draw(Mat src, Mat dst)
{
var points = new MatOfPoint2f();
var patternSize = new Size((int)SizeX, (int)SizeY);
var found = false;
switch (boardType)
{
case BoardType.ChessBoard:
found = Calib3d.findChessboardCorners(src, patternSize, points, Calib3d.CALIB_CB_ADAPTIVE_THRESH | Calib3d.CALIB_CB_FAST_CHECK | Calib3d.CALIB_CB_NORMALIZE_IMAGE);
break;
case BoardType.CirclesGrid:
found = Calib3d.findCirclesGrid(src, patternSize, points, Calib3d.CALIB_CB_SYMMETRIC_GRID);
break;
case BoardType.AsymmetricCirclesGrid:
found = Calib3d.findCirclesGrid(src, patternSize, points, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
break;
}
if (found)
{
if (boardType == BoardType.ChessBoard)
{
Imgproc.cornerSubPix(src, points, new Size(5, 5), new Size(-1, -1), new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 30, 0.1));
}
Calib3d.drawChessboardCorners(dst, patternSize, points, found);
}
}
public static bool FindChessboardCorners(Mat grayTexMat, Vector2Int innerCornerCount, ref MatOfPoint2f cornerPoints, bool fastAndImprecise = false)
{
Vector2IntToSize(innerCornerCount, ref _tempSize);
if (cornerPoints == null)
{
cornerPoints = new MatOfPoint2f();
}
if (!fastAndImprecise)
{
// TODO: Enable CALIB_CB_LARGER and CALIB_CB_MARKER when they are supported.
// https://forum.unity.com/threads/released-opencv-for-unity.277080/page-47#post-5665141
const int flagsSB =
Calib3d.CALIB_CB_EXHAUSTIVE; // Run an exhaustive search to improve detection rate. (Note: this seems to have very positive impact).
//Calib3d.CALIB_CB_LARGER // The detected pattern is allowed to be larger than patternSize
//Calib3d.CALIB_CB_MARKER // The detected pattern must have a marker
//Calib3d.CALIB_CB_NORMALIZE_IMAGE // Normalize the image gamma with equalizeHist before detection
// findChessboardCornersSB() is supposed to work better than combined findChessboardCorners() and cornerSubPix().
return(Calib3d.findChessboardCornersSB(grayTexMat, _tempSize, cornerPoints, flagsSB));
}
// Normally, you would use find4QuadCornersSubpix after findChessboardCorners to improve the result, but
// we just want fast.
const int flags =
Calib3d.CALIB_CB_FAST_CHECK; // |
//Calib3d.CALIB_CB_NORMALIZE_IMAGE |
//Calib3d.CALIB_CB_FILTER_QUADS |
//Calib3d.CALIB_CB_ADAPTIVE_THRESH;
bool success = Calib3d.findChessboardCorners(grayTexMat, _tempSize, cornerPoints, flags);
// Because the old version prefers to start at black tile corner vs findChessboardCornersSB prefering to start
// at white tile corner, we have to reverse the order of points for the old version to match up.
if (success)
{
ReverseOrder(cornerPoints);
}
return(success);
}
private double CaptureFrame(Mat frameMat)
{
double repErr = -1;
switch (markerType)
{
default:
case MarkerType.ChArUcoBoard:
List <Mat> corners = new List <Mat>();
Mat ids = new Mat();
Aruco.detectMarkers(frameMat, dictionary, corners, ids, detectorParams, rejectedCorners, camMatrix, distCoeffs);
if (refineMarkerDetection)
{
Aruco.refineDetectedMarkers(frameMat, charucoBoard, corners, ids, rejectedCorners, camMatrix, distCoeffs, 10f, 3f, true, recoveredIdxs, detectorParams);
}
if (ids.total() > 0)
{
Debug.Log("Frame captured.");
allCorners.Add(corners);
allIds.Add(ids);
allImgs.Add(frameMat);
}
else
{
Debug.Log("Invalid frame.");
frameMat.Dispose();
if (ids != null)
{
ids.Dispose();
}
foreach (var item in corners)
{
item.Dispose();
}
corners.Clear();
return(-1);
}
// calibrate camera using aruco markers
//double arucoRepErr = CalibrateCameraAruco (allCorners, allIds, charucoBoard, frameMat.size(), camMatrix, distCoeffs, rvecs, tvecs, calibrationFlags);
//Debug.Log ("arucoRepErr: " + arucoRepErr);
// calibrate camera using charuco
repErr = CalibrateCameraCharuco(allCorners, allIds, charucoBoard, frameMat.size(), camMatrix, distCoeffs, rvecs, tvecs, calibrationFlags, calibrationFlags);
break;
case MarkerType.ChessBoard:
case MarkerType.CirclesGlid:
case MarkerType.AsymmetricCirclesGlid:
MatOfPoint2f points = new MatOfPoint2f();
Size patternSize = new Size((int)squaresX, (int)squaresY);
bool found = false;
switch (markerType)
{
default:
case MarkerType.ChessBoard:
found = Calib3d.findChessboardCorners(frameMat, patternSize, points, Calib3d.CALIB_CB_ADAPTIVE_THRESH | Calib3d.CALIB_CB_FAST_CHECK | Calib3d.CALIB_CB_NORMALIZE_IMAGE);
break;
case MarkerType.CirclesGlid:
found = Calib3d.findCirclesGrid(frameMat, patternSize, points, Calib3d.CALIB_CB_SYMMETRIC_GRID);
break;
case MarkerType.AsymmetricCirclesGlid:
found = Calib3d.findCirclesGrid(frameMat, patternSize, points, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
break;
}
if (found)
{
Debug.Log("Frame captured.");
if (markerType == MarkerType.ChessBoard)
{
Imgproc.cornerSubPix(frameMat, points, new Size(5, 5), new Size(-1, -1), new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 30, 0.1));
}
imagePoints.Add(points);
allImgs.Add(frameMat);
}
else
{
Debug.Log("Invalid frame.");
frameMat.Dispose();
if (points != null)
{
points.Dispose();
}
return(-1);
}
if (imagePoints.Count < 1)
{
Debug.Log("Not enough points for calibration.");
repErr = -1;
}
else
{
MatOfPoint3f objectPoint = new MatOfPoint3f(new Mat(imagePoints[0].rows(), 1, CvType.CV_32FC3));
CalcChessboardCorners(patternSize, squareSize, objectPoint, markerType);
List <Mat> objectPoints = new List <Mat>();
for (int i = 0; i < imagePoints.Count; ++i)
{
objectPoints.Add(objectPoint);
}
repErr = Calib3d.calibrateCamera(objectPoints, imagePoints, frameMat.size(), camMatrix, distCoeffs, rvecs, tvecs, calibrationFlags);
objectPoint.Dispose();
}
break;
}
Debug.Log("repErr: " + repErr);
Debug.Log("camMatrix: " + camMatrix.dump());
Debug.Log("distCoeffs: " + distCoeffs.dump());
return(repErr);
}
private void DrawFrame(Mat grayMat, Mat bgrMat)
{
Imgproc.cvtColor(grayMat, bgrMat, Imgproc.COLOR_GRAY2BGR);
switch (markerType)
{
default:
case MarkerType.ChArUcoBoard:
// detect markers.
Aruco.detectMarkers(grayMat, dictionary, corners, ids, detectorParams, rejectedCorners, camMatrix, distCoeffs);
// refine marker detection.
if (refineMarkerDetection)
{
Aruco.refineDetectedMarkers(grayMat, charucoBoard, corners, ids, rejectedCorners, camMatrix, distCoeffs, 10f, 3f, true, recoveredIdxs, detectorParams);
}
// if at least one marker detected
if (ids.total() > 0)
{
Aruco.interpolateCornersCharuco(corners, ids, grayMat, charucoBoard, charucoCorners, charucoIds, camMatrix, distCoeffs, charucoMinMarkers);
// draw markers.
Aruco.drawDetectedMarkers(bgrMat, corners, ids, new Scalar(0, 255, 0, 255));
// if at least one charuco corner detected
if (charucoIds.total() > 0)
{
Aruco.drawDetectedCornersCharuco(bgrMat, charucoCorners, charucoIds, new Scalar(0, 0, 255, 255));
}
}
break;
case MarkerType.ChessBoard:
case MarkerType.CirclesGlid:
case MarkerType.AsymmetricCirclesGlid:
// detect markers.
MatOfPoint2f points = new MatOfPoint2f();
bool found = false;
switch (markerType)
{
default:
case MarkerType.ChessBoard:
found = Calib3d.findChessboardCorners(grayMat, new Size((int)squaresX, (int)squaresY), points, Calib3d.CALIB_CB_ADAPTIVE_THRESH | Calib3d.CALIB_CB_FAST_CHECK | Calib3d.CALIB_CB_NORMALIZE_IMAGE);
break;
case MarkerType.CirclesGlid:
found = Calib3d.findCirclesGrid(grayMat, new Size((int)squaresX, (int)squaresY), points, Calib3d.CALIB_CB_SYMMETRIC_GRID);
break;
case MarkerType.AsymmetricCirclesGlid:
found = Calib3d.findCirclesGrid(grayMat, new Size((int)squaresX, (int)squaresY), points, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
break;
}
if (found)
{
if (markerType == MarkerType.ChessBoard)
{
Imgproc.cornerSubPix(grayMat, points, new Size(5, 5), new Size(-1, -1), new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 30, 0.1));
}
// draw markers.
Calib3d.drawChessboardCorners(bgrMat, new Size((int)squaresX, (int)squaresY), points, found);
}
break;
}
double[] camMatrixArr = new double[(int)camMatrix.total()];
camMatrix.get(0, 0, camMatrixArr);
double[] distCoeffsArr = new double[(int)distCoeffs.total()];
distCoeffs.get(0, 0, distCoeffsArr);
int ff = Imgproc.FONT_HERSHEY_SIMPLEX;
double fs = 0.4;
Scalar c = new Scalar(255, 255, 255, 255);
int t = 0;
int lt = Imgproc.LINE_AA;
bool blo = false;
int frameCount = (markerType == MarkerType.ChArUcoBoard) ? allCorners.Count : imagePoints.Count;
Imgproc.putText(bgrMat, frameCount + " FRAME CAPTURED", new Point(bgrMat.cols() - 300, 20), ff, fs, c, t, lt, blo);
Imgproc.putText(bgrMat, "IMAGE_WIDTH: " + bgrMat.width(), new Point(bgrMat.cols() - 300, 40), ff, fs, c, t, lt, blo);
Imgproc.putText(bgrMat, "IMAGE_HEIGHT: " + bgrMat.height(), new Point(bgrMat.cols() - 300, 60), ff, fs, c, t, lt, blo);
Imgproc.putText(bgrMat, "CALIBRATION_FLAGS: " + calibrationFlags, new Point(bgrMat.cols() - 300, 80), ff, fs, c, t, lt, blo);
Imgproc.putText(bgrMat, "CAMERA_MATRIX: ", new Point(bgrMat.cols() - 300, 100), ff, fs, c, t, lt, blo);
for (int i = 0; i < camMatrixArr.Length; i = i + 3)
{
Imgproc.putText(bgrMat, " " + camMatrixArr[i] + ", " + camMatrixArr[i + 1] + ", " + camMatrixArr[i + 2] + ",", new Point(bgrMat.cols() - 300, 120 + 20 * i / 3), ff, fs, c, t, lt, blo);
}
Imgproc.putText(bgrMat, "DISTORTION_COEFFICIENTS: ", new Point(bgrMat.cols() - 300, 180), ff, fs, c, t, lt, blo);
for (int i = 0; i < distCoeffsArr.Length; ++i)
{
Imgproc.putText(bgrMat, " " + distCoeffsArr[i] + ",", new Point(bgrMat.cols() - 300, 200 + 20 * i), ff, fs, c, t, lt, blo);
}
Imgproc.putText(bgrMat, "AVG_REPROJECTION_ERROR: " + repErr, new Point(bgrMat.cols() - 300, 300), ff, fs, c, t, lt, blo);
if (frameCount == 0)
{
Imgproc.putText(bgrMat, "To calibration start, please press the calibration button or do air tap gesture!", new Point(5, bgrMat.rows() - 10), Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, new Scalar(255, 255, 255, 255), 1, Imgproc.LINE_AA, false);
}
}
// Update is called once per frame
void Update()
{
if (webCamTextureToMatHelper.isPlaying() && webCamTextureToMatHelper.didUpdateThisFrame())
{
Mat rgbaMat = webCamTextureToMatHelper.GetMat();
if (AppControl.control.calibrationComplete)
{
Mat rgbaMatUndistorted = rgbaMat.clone();
Size SizeCm = AppControl.control.cameraMatrix.size();
Size SizeDc = AppControl.control.distCoeffs.size();
Imgproc.undistort(rgbaMat, rgbaMatUndistorted, AppControl.control.cameraMatrix, AppControl.control.distCoeffs);
rgbaMat = rgbaMatUndistorted;
}
Imgproc.cvtColor(rgbaMat, hsvMat, Imgproc.COLOR_RGBA2RGB);
Imgproc.cvtColor(hsvMat, hsvMat, Imgproc.COLOR_RGB2HSV);
// IMAGE PROCESSING STARTS (rgbaMat is the captured image)
// Convert the current rgba frame (rgbaMat) to a gray image (grayMat)
Imgproc.cvtColor(rgbaMat, grayMat, Imgproc.COLOR_RGBA2GRAY);
//CALIB_CB_FAST_CHECK saves a lot of time on images
//that do not contain any chessboard corners
bool patternfound = Calib3d.findChessboardCorners(grayMat, patternsize, pointbuf,
Calib3d.CALIB_CB_ADAPTIVE_THRESH + Calib3d.CALIB_CB_NORMALIZE_IMAGE
+ Calib3d.CALIB_CB_FAST_CHECK);
if (patternfound)
{
Imgproc.cornerSubPix(grayMat, pointbuf, new Size(11, 11), new Size(-1, -1), new TermCriteria(TermCriteria.EPS + TermCriteria.MAX_ITER, 30, 0.1));
float timeNow = Time.realtimeSinceStartup;
if ((timeNow > lastInterval + updateInterval))
{
imagePoints.Add(pointbuf.clone());
calcChessboardCorners(patternsize, squaresize, objectpointbuf);
objectPoints.Add(objectpointbuf.clone());
++numCalibImages;
lastInterval = timeNow;
}
}
if (numCalibImages > numCalibrationTakes && !calibrationComplete)
{
// Do calibration
int flags = 0;
calibrationComplete = true;
Calib3d.calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flags);
// Calibration is complete
List <float> perViewErrors = new List <float>();
float error;
error = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix, distCoeffs, perViewErrors);
// Debug statements
Debug.Log("Calibration complete!");
Debug.Log("Camera Matrix:");
Debug.Log(cameraMatrix.dump());
Debug.Log("Distortion Coefficients:");
Debug.Log(distCoeffs.dump()); // Debug.Log("fx: " + cameraMatrix.get(0,0)[0]);
Debug.Log("Reprojection error average: " + error); // Error should be in units of pixels
Debug.Log(perViewErrors.ToString());
// Save the calibration variables in AppControl
AppControl.control.calibrationComplete = calibrationComplete;
AppControl.control.cameraMatrix = cameraMatrix;
AppControl.control.distCoeffs = distCoeffs;
AppControl.control.reprojectionError = error;
AppControl.control.fx = (float)cameraMatrix.get(0, 0)[0];
AppControl.control.fy = (float)cameraMatrix.get(1, 1)[0];
AppControl.control.cx = (float)cameraMatrix.get(0, 2)[0];
AppControl.control.cy = (float)cameraMatrix.get(1, 2)[0];
// Store the calibration parameters in player preferences text file
PlayerPrefs.SetInt("Calibrated", 1);
PlayerPrefs.SetFloat("Error", error);
float k1 = (float)distCoeffs.get(0, 1)[0];
float k2 = (float)distCoeffs.get(0, 1)[0];
PlayerPrefs.SetFloat("k1", (float)distCoeffs.get(0, 0)[0]);
PlayerPrefs.SetFloat("k2", (float)distCoeffs.get(0, 1)[0]);
PlayerPrefs.SetFloat("p1", (float)distCoeffs.get(0, 2)[0]);
PlayerPrefs.SetFloat("p2", (float)distCoeffs.get(0, 3)[0]);
PlayerPrefs.SetFloat("k3", (float)distCoeffs.get(0, 4)[0]);
PlayerPrefs.SetFloat("fx", (float)cameraMatrix.get(0, 0)[0]);
PlayerPrefs.SetFloat("fy", (float)cameraMatrix.get(1, 1)[0]);
PlayerPrefs.SetFloat("cx", (float)cameraMatrix.get(0, 2)[0]);
PlayerPrefs.SetFloat("cy", (float)cameraMatrix.get(1, 2)[0]);
}
// SHOW IMAGE ON THE DISPLAY
// Draw the chessboard corners so it's obvious that the app is working
Calib3d.drawChessboardCorners(rgbaMat, patternsize, pointbuf, patternfound);
// Notify the user when the calibration is finished
if (!calibrationComplete)
{
Core.putText(rgbaMat, "Number of images collected: " + numCalibImages, new Point(5, rgbaMat.rows() - 10), Core.FONT_HERSHEY_SIMPLEX, 1.0, new Scalar(255, 255, 255, 255), 2, Core.LINE_AA, false);
}
else
{
// Mat rgbaMatUndistorted = new Mat();
Core.putText(rgbaMat, "Calibration complete", new Point(5, rgbaMat.rows() - 10), Core.FONT_HERSHEY_SIMPLEX, 1.0, new Scalar(255, 255, 255, 255), 2, Core.LINE_AA, false);
}
Utils.matToTexture2D(rgbaMat, texture); // Default display
// Utils.matToTexture2D (rgbaMat, texture, colors)
}
}
void handleCalibration()
{
for (int i = 0; i < AK_receiver.GetComponent <akplay>().camInfoList.Count; i++)
{
//create color mat:
byte[] colorBytes = ((Texture2D)(AK_receiver.GetComponent <akplay>().camInfoList[i].colorCube.GetComponent <Renderer>().material.mainTexture)).GetRawTextureData();
GCHandle ch = GCHandle.Alloc(colorBytes, GCHandleType.Pinned);
Mat colorMat = new Mat(AK_receiver.GetComponent <akplay>().camInfoList[i].color_height, AK_receiver.GetComponent <akplay>().camInfoList[i].color_width, CvType.CV_8UC4);
Utils.copyToMat(ch.AddrOfPinnedObject(), colorMat);
ch.Free();
//OpenCVForUnity.CoreModule.Core.flip(colorMat, colorMat, 0);
//detect a chessboard in the image, and refine the points, and save the pixel positions:
MatOfPoint2f positions = new MatOfPoint2f();
int resizer = 4;
resizer = 1; //noresize!
Mat colorMatSmall = new Mat(); //~27 ms each
Imgproc.resize(colorMat, colorMatSmall, new Size(colorMat.cols() / resizer, colorMat.rows() / resizer));
bool success = Calib3d.findChessboardCorners(colorMatSmall, new Size(7, 7), positions);
for (int ss = 0; ss < positions.rows(); ss++)
{
double[] data = positions.get(ss, 0);
data[0] = data[0] * resizer;
data[1] = data[1] * resizer;
positions.put(ss, 0, data);
}
//subpixel, drawing chessboard, and getting orange blobs takes 14ms
TermCriteria tc = new TermCriteria();
Imgproc.cornerSubPix(colorMat, positions, new Size(5, 5), new Size(-1, -1), tc);
Mat chessboardResult = new Mat();
colorMat.copyTo(chessboardResult);
Calib3d.drawChessboardCorners(chessboardResult, new Size(7, 7), positions, success);
//Find the orange blobs:
Mat orangeMask = new Mat();
Vector2[] blobs = getOrangeBlobs(ref colorMat, ref orangeMask);
//find blob closest to chessboard
if (success && (blobs.Length > 0))
{
Debug.Log("found a chessboard and blobs for camera: " + i);
// time to get pin1 and chessboard positions: 27ms
//find pin1:
Point closestBlob = new Point();
int pin1idx = getPin1(positions, blobs, ref closestBlob);
Imgproc.circle(chessboardResult, new Point(positions.get(pin1idx, 0)[0], positions.get(pin1idx, 0)[1]), 10, new Scalar(255, 0, 0), -1);
Imgproc.circle(chessboardResult, closestBlob, 10, new Scalar(255, 255, 0), -1);
//get world positions of chessboard
Point[] realWorldPointArray = new Point[positions.rows()];
Point3[] realWorldPointArray3 = new Point3[positions.rows()];
Point[] imagePointArray = new Point[positions.rows()];
//getChessBoardWorldPositions(positions, pin1idx, 0.0498f, ref realWorldPointArray, ref realWorldPointArray3, ref imagePointArray); //green and white checkerboard.
getChessBoardWorldPositions(positions, pin1idx, 0.07522f, ref realWorldPointArray, ref realWorldPointArray3, ref imagePointArray); //black and white checkerboard.
string text = "";
float decimals = 1000.0f;
int text_red = 255;
int text_green = 0;
int text_blue = 0;
text = ((int)(realWorldPointArray3[0].x * decimals)) / decimals + "," + ((int)(realWorldPointArray3[0].y * decimals)) / decimals + "," + ((int)(realWorldPointArray3[0].z * decimals)) / decimals;
//text = sprintf("%f,%f,%f", realWorldPointArray3[0].x, realWorldPointArray3[0].y, realWorldPointArray3[0].z);
Imgproc.putText(chessboardResult, text, new Point(positions.get(0, 0)[0], positions.get(0, 0)[1]), 0, .6, new Scalar(text_red, text_green, text_blue));
text = ((int)(realWorldPointArray3[6].x * decimals)) / decimals + "," + ((int)(realWorldPointArray3[6].y * decimals)) / decimals + "," + ((int)(realWorldPointArray3[6].z * decimals)) / decimals;
//text = sprintf("%f,%f,%f", realWorldPointArray3[0].x, realWorldPointArray3[0].y, realWorldPointArray3[0].z);
Imgproc.putText(chessboardResult, text, new Point(positions.get(6, 0)[0], positions.get(6, 0)[1]), 0, .6, new Scalar(text_red, text_green, text_blue));
text = ((int)(realWorldPointArray3[42].x * decimals)) / decimals + "," + ((int)(realWorldPointArray3[42].y * decimals)) / decimals + "," + ((int)(realWorldPointArray3[42].z * decimals)) / decimals;
//text = sprintf("%f,%f,%f", realWorldPointArray3[0].x, realWorldPointArray3[0].y, realWorldPointArray3[0].z);
Imgproc.putText(chessboardResult, text, new Point(positions.get(42, 0)[0], positions.get(42, 0)[1]), 0, .6, new Scalar(text_red, text_green, text_blue));
text = ((int)(realWorldPointArray3[48].x * decimals)) / decimals + "," + ((int)(realWorldPointArray3[48].y * decimals)) / decimals + "," + ((int)(realWorldPointArray3[48].z * decimals)) / decimals;
//text = sprintf("%2.2f,%2.2f,%2.2f", realWorldPointArray3[48].x, realWorldPointArray3[48].y, realWorldPointArray3[48].z);
Imgproc.putText(chessboardResult, text, new Point(positions.get(48, 0)[0], positions.get(48, 0)[1]), 0, .6, new Scalar(text_red, text_green, text_blue));
Mat cameraMatrix = Mat.eye(3, 3, CvType.CV_64F);
cameraMatrix.put(0, 0, AK_receiver.GetComponent <akplay>().camInfoList[i].color_fx);
cameraMatrix.put(1, 1, AK_receiver.GetComponent <akplay>().camInfoList[i].color_fy);
cameraMatrix.put(0, 2, AK_receiver.GetComponent <akplay>().camInfoList[i].color_cx);
cameraMatrix.put(1, 2, AK_receiver.GetComponent <akplay>().camInfoList[i].color_cy);
double[] distortion = new double[8];
distortion[0] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_k1;
distortion[1] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_k2;
distortion[2] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_p1;
distortion[3] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_p2;
distortion[4] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_k3;
distortion[5] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_k4;
distortion[6] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_k5;
distortion[7] = AK_receiver.GetComponent <akplay>().camInfoList[i].color_k6;
/*
* distortion[0] = 0.0;
* distortion[1] = 0.0;
* distortion[2] = 0.0;
* distortion[3] = 0.0;
* distortion[4] = 0.0;
* distortion[5] = 0.0;
* distortion[6] = 0.0;
* distortion[7] = 0.0;
*/
//~1 ms to solve for pnp
Mat rvec = new Mat();
Mat tvec = new Mat();
bool solvepnpSucces = Calib3d.solvePnP(new MatOfPoint3f(realWorldPointArray3), new MatOfPoint2f(imagePointArray), cameraMatrix, new MatOfDouble(distortion), rvec, tvec);
Mat R = new Mat();
Calib3d.Rodrigues(rvec, R);
//calculate unity vectors, and camera transforms
Mat camCenter = -R.t() * tvec;
Mat forwardOffset = new Mat(3, 1, tvec.type());
forwardOffset.put(0, 0, 0);
forwardOffset.put(1, 0, 0);
forwardOffset.put(2, 0, 1);
Mat upOffset = new Mat(3, 1, tvec.type());
upOffset.put(0, 0, 0);
upOffset.put(1, 0, -1);
upOffset.put(2, 0, 0);
Mat forwardVectorCV = R.t() * (forwardOffset - tvec);
forwardVectorCV = forwardVectorCV - camCenter;
Mat upVectorCV = R.t() * (upOffset - tvec);
upVectorCV = upVectorCV - camCenter;
Vector3 forwardVectorUnity = new Vector3((float)forwardVectorCV.get(0, 0)[0], (float)forwardVectorCV.get(2, 0)[0], (float)forwardVectorCV.get(1, 0)[0]); //need to flip y and z due to unity coordinate system
Vector3 upVectorUnity = new Vector3((float)upVectorCV.get(0, 0)[0], (float)upVectorCV.get(2, 0)[0], (float)upVectorCV.get(1, 0)[0]); //need to flip y and z due to unity coordinate system
Vector3 camCenterUnity = new Vector3((float)camCenter.get(0, 0)[0], (float)camCenter.get(2, 0)[0], (float)camCenter.get(1, 0)[0]);
Quaternion rotationUnity = Quaternion.LookRotation(forwardVectorUnity, upVectorUnity);
GameObject colorMarker = GameObject.CreatePrimitive(PrimitiveType.Cube);
//colorMarker.transform.localScale = new Vector3(0.1f, 0.1f, 0.2f);
//colorMarker.transform.parent = AK_receiver.transform;
colorMarker.layer = LayerMask.NameToLayer("Debug");
colorMarker.transform.position = camCenterUnity;
colorMarker.transform.rotation = Quaternion.LookRotation(forwardVectorUnity, upVectorUnity);
colorMarker.GetComponent <Renderer>().material.color = Color.blue;
Vector3 forwardDepth = AK_receiver.GetComponent <akplay>().camInfoList[i].color_extrinsics.MultiplyPoint(forwardVectorUnity);
Vector3 upDepth = AK_receiver.GetComponent <akplay>().camInfoList[i].color_extrinsics.MultiplyPoint(upVectorUnity);
Vector3 camCenterDepth = AK_receiver.GetComponent <akplay>().camInfoList[i].color_extrinsics.MultiplyPoint(camCenterUnity);
Quaternion rotationDepth = Quaternion.LookRotation(forwardDepth, upDepth);
GameObject depthMarker = GameObject.CreatePrimitive(PrimitiveType.Cube);
depthMarker.layer = LayerMask.NameToLayer("Debug");
depthMarker.transform.parent = colorMarker.transform;
//depthMarker.transform.localScale = AK_receiver.GetComponent<akplay>().camInfoList[i].color_extrinsics.lossyScale;
depthMarker.transform.localRotation = AK_receiver.GetComponent <akplay>().camInfoList[i].color_extrinsics.inverse.rotation;
Vector3 matrixPosition = new Vector3(AK_receiver.GetComponent <akplay>().camInfoList[i].color_extrinsics.inverse.GetColumn(3).x,
AK_receiver.GetComponent <akplay>().camInfoList[i].color_extrinsics.inverse.GetColumn(3).y,
AK_receiver.GetComponent <akplay>().camInfoList[i].color_extrinsics.inverse.GetColumn(3).z);
/*
* depthMarker.transform.localRotation = AK_receiver.GetComponent<akplay>().camInfoList[i].color_extrinsics.rotation;
*
* Vector3 matrixPosition = new Vector3(AK_receiver.GetComponent<akplay>().camInfoList[i].color_extrinsics.GetColumn(3).x,
* AK_receiver.GetComponent<akplay>().camInfoList[i].color_extrinsics.GetColumn(3).y,
* AK_receiver.GetComponent<akplay>().camInfoList[i].color_extrinsics.GetColumn(3).z);
*/
depthMarker.transform.localPosition = -matrixPosition;
depthMarker.transform.parent = null;
colorMarker.transform.localScale = new Vector3(0.1f, 0.1f, 0.2f);
depthMarker.transform.localScale = new Vector3(0.1f, 0.1f, 0.2f);
//depthMarker.transform.parent = AK_receiver.transform;
//depthMarker.transform.position = camCenterDepth;
//depthMarker.transform.rotation = Quaternion.LookRotation(forwardDepth-camCenterDepth, upDepth-camCenterDepth);
depthMarker.GetComponent <Renderer>().material.color = Color.red;
AK_receiver.GetComponent <akplay>().camInfoList[i].visualization.transform.position = depthMarker.transform.position; //need to flip y and z due to unity coordinate system
AK_receiver.GetComponent <akplay>().camInfoList[i].visualization.transform.rotation = depthMarker.transform.rotation;
}
//draw chessboard result to calibration ui:
Texture2D colorTexture = new Texture2D(chessboardResult.cols(), chessboardResult.rows(), TextureFormat.BGRA32, false);
colorTexture.LoadRawTextureData((IntPtr)chessboardResult.dataAddr(), (int)chessboardResult.total() * (int)chessboardResult.elemSize());
colorTexture.Apply();
checkerboard_display_list[i].GetComponent <Renderer>().material.mainTexture = colorTexture;
//draw threshold to calibration ui:
Texture2D orangeTexture = new Texture2D(orangeMask.cols(), orangeMask.rows(), TextureFormat.R8, false);
orangeTexture.LoadRawTextureData((IntPtr)orangeMask.dataAddr(), (int)orangeMask.total() * (int)orangeMask.elemSize());
orangeTexture.Apply();
threshold_display_list[i].GetComponent <Renderer>().material.mainTexture = orangeTexture;
}
}
public virtual double Calibrate(Mat mat)
{
var repError = -1.0;
var points = new MatOfPoint2f();
var patternSize = new Size((int)SizeX, (int)SizeY);
var found = false;
switch (boardType)
{
case BoardType.ChessBoard:
found = Calib3d.findChessboardCorners(mat, patternSize, points, Calib3d.CALIB_CB_ADAPTIVE_THRESH | Calib3d.CALIB_CB_FAST_CHECK | Calib3d.CALIB_CB_NORMALIZE_IMAGE);
break;
case BoardType.CirclesGrid:
found = Calib3d.findCirclesGrid(mat, patternSize, points, Calib3d.CALIB_CB_SYMMETRIC_GRID);
break;
case BoardType.AsymmetricCirclesGrid:
found = Calib3d.findCirclesGrid(mat, patternSize, points, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
break;
}
if (found)
{
if (boardType == BoardType.ChessBoard)
{
Imgproc.cornerSubPix(mat, points, new Size(5, 5), new Size(-1, -1), new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 30, 0.1));
}
imagePoints.Add(points);
}
else
{
Debug.Log("Invalid frame.");
if (points != null)
{
points.Dispose();
}
ErrorEvent?.Invoke("Invalid frame.");
return(repError);
}
if (imagePoints.Count < 1)
{
Debug.Log("Not enough points for calibration.");
ErrorEvent?.Invoke("Not enough points for calibration.");
return(repError);
}
else
{
var objectPoint = new MatOfPoint3f(new Mat(imagePoints[0].rows(), 1, CvType.CV_32FC3));
CalcCorners(patternSize, squareSize, objectPoint);
var objectPoints = new List <Mat>();
for (int i = 0; i < imagePoints.Count; ++i)
{
objectPoints.Add(objectPoint);
}
repError = Calib3d.calibrateCamera(objectPoints, imagePoints, mat.size(), cameraMatrix, distCoeffs, rvecs, tvecs);
CalibrationEvent?.Invoke(repError);
objectPoint.Dispose();
}
Debug.Log("-----calibrate-----");
Debug.Log("repErr: " + repError);
Debug.Log("camMatrix: " + cameraMatrix.dump());
Debug.Log("distCoeffs: " + distCoeffs.dump());
return(repError);
}
