// Update is called once per frame
void Update()
{
// Sample rate update
++frames;
float timeNow = Time.realtimeSinceStartup;
if (timeNow > lastInterval + updateInterval)
{
fps = (float)(frames / (timeNow - lastInterval));
frames = 0;
lastInterval = timeNow;
}
// Time since last update
float dt = 1 / fps;
if (webCamTextureToMatHelper.isPlaying() && webCamTextureToMatHelper.didUpdateThisFrame())
{
Mat rgbaMat = webCamTextureToMatHelper.GetMat();
Imgproc.cvtColor(rgbaMat, hsvMat, Imgproc.COLOR_RGBA2RGB);
Imgproc.cvtColor(hsvMat, hsvMat, Imgproc.COLOR_RGB2HSV);
Point[] points = roiPointList.ToArray();
if (roiPointList.Count == 4)
{
using (Mat backProj = new Mat())
{
Imgproc.calcBackProject(new List <Mat>(new Mat[] { hsvMat }), new MatOfInt(0), roiHistMat, backProj, new MatOfFloat(0, 180), 1.0);
///////////////////////////////////////////////////////
// Kalman Filter Start
///////////////////////////////////////////////////////
// Process noise matrix, Q
Matrix Q2 = (Mathf.Pow(pn, 2) / dt) * Matrix.Build.DenseOfArray(new float[, ]
{
{ Mathf.Pow(dt, 4) / 4, Mathf.Pow(dt, 3) / 2f, Mathf.Pow(dt, 2) / 2f },
{ Mathf.Pow(dt, 3) / 2f, Mathf.Pow(dt, 2) / 2f, dt },
{ Mathf.Pow(dt, 2) / 2f, dt, 1 }
});
Q = (Mathf.Pow(pn, 2) / dt) * Matrix.Build.DenseOfMatrixArray(new Matrix[, ]
{
{ Q2, zero3x3, zero3x3 },
{ zero3x3, Q2, zero3x3 },
{ zero3x3, zero3x3, Q2 }
});
// Measurement noise matrix, R
R = Mathf.Pow(mn, 2) * Matrix.Build.DenseIdentity(3);
// Build transition and control matrices
Matrix F2 = Matrix.Build.DenseOfArray(new float[, ] {
{ 1, dt, Mathf.Pow(dt, 2) / 2f }, { 0, 1, dt }, { 0, 0, 1 }
});
Matrix F = Matrix.Build.DenseOfMatrixArray(new Matrix[, ]
{
{ F2, zero3x3, zero3x3 },
{ zero3x3, F2, zero3x3 },
{ zero3x3, zero3x3, F2 },
});
// Prediction
Pp = F * Pe * F.Transpose() + Q;
xp = F * xe;
roiPred = new OpenCVForUnity.Rect((int)(xp[0] - 0.5f * xp[6]), (int)(xp[3] - 0.5f * xp[6]), (int)xp[6], (int)xp[6]);
roiRect = new OpenCVForUnity.Rect((int)(xp[0] - 0.5f * roiSearch.width), (int)(xp[3] - 0.5f * roiSearch.height), roiSearch.width, roiSearch.height);
// roiRect = roiPred.clone();
RotatedRect r = Video.CamShift(backProj, roiRect, termination);
ObjectFound = (roiRect.height > 0 && roiRect.width > 0);
// Innovation
Vector nu;
if (ObjectFound)
{
// Innovation
Vector zk = Vector.Build.DenseOfArray(new float[] { (int)(roiRect.x + 0.5f * roiRect.width),
(int)(roiRect.y + 0.5f * roiRect.height),
(int)(0.5 * (roiRect.width + roiRect.height)) });
nu = zk - H * xp;
// Search window update
roiSearch = r.boundingRect().clone();
// Debug
SpeedAtFailure = -1f;
}
else
{
roiRect = roiPred.clone();
if (xp[0] < 0 || xp[0] < 0 || xp[0] > 640 || xp[3] > 480)
{
xp[0] = 320f; xp[1] = 0; xp[2] = 0;
xp[3] = 240f; xp[4] = 0; xp[5] = 0;
xp[6] = 40f; xp[7] = 0; xp[8] = 0;
roiRect.x = (int)(320 - 0.5f * 40);
roiRect.y = (int)(240 - 0.5f * 40);
roiRect.height = 40;
roiRect.width = 40;
roiPred = roiRect.clone();
}
// Innovation
Vector zk = Vector.Build.DenseOfArray(new float[] { (float)(roiRect.x + 0.5f * roiRect.width),
(float)(roiRect.y + 0.5f * roiRect.height),
(float)(0.5 * (roiRect.width + roiRect.height)) });
nu = zk - H * xp;
roiSearch = roiPred.clone();
}
// Kalman gain
Matrix K = Pp * H.Transpose() * R.Transpose();
// Innovation gain
Vector gain = K * nu;
// State update
xe = xp + gain;
// Covariance update
Pe = (Pp.Inverse() + H.Transpose() * R.Transpose() * H).Inverse();
// Display results to console
StateArray = xe.ToArray();
InnovationGain = gain.ToArray();
CovarianceTrace = Pe.Diagonal().ToArray();
///////////////////////////////////////////////////////
// Kalman Filter End
///////////////////////////////////////////////////////
r.points(points);
}
if (Input.GetMouseButtonUp(0))
{
roiPointList.Clear();
}
}
if (roiPointList.Count < 4)
{
if (Input.GetMouseButtonUp(0))
{
roiPointList.Add(convertScreenPoint(new Point(Input.mousePosition.x, Input.mousePosition.y), gameObject, Camera.main));
// Debug.Log ("mouse X " + Input.mousePosition.x);
// Debug.Log ("mouse Y " + Input.mousePosition.y);
if (!(new OpenCVForUnity.Rect(0, 0, hsvMat.width(), hsvMat.height()).contains(roiPointList[roiPointList.Count - 1])))
{
roiPointList.RemoveAt(roiPointList.Count - 1);
}
}
if (roiPointList.Count == 4)
{
using (MatOfPoint roiPointMat = new MatOfPoint(roiPointList.ToArray()))
{
roiRect = Imgproc.boundingRect(roiPointMat);
roiPred = roiRect.clone();
roiSearch = roiRect.clone();
}
///////////////////////////////////////////////////////
// Kalman Filter Initialize
///////////////////////////////////////////////////////
Pe = Matrix.Build.DenseIdentity(9, 9);
Vector z1 = roi2center(roiRect);
xe = Vector.Build.DenseOfArray(new float[] { z1[0], 0, 0, z1[1], 0, 0, (roiRect.width + roiRect.height) / 2, 0, 0 });
///////////////////////////////////////////////////////
// End Kalman Filter Initialize
///////////////////////////////////////////////////////
if (roiHistMat != null)
{
roiHistMat.Dispose();
roiHistMat = null;
}
roiHistMat = new Mat();
using (Mat roiHSVMat = new Mat(hsvMat, roiRect))
using (Mat maskMat = new Mat())
{
Imgproc.calcHist(new List <Mat>(new Mat[] { roiHSVMat }), new MatOfInt(0), maskMat, roiHistMat, new MatOfInt(16), new MatOfFloat(0, 180));
Core.normalize(roiHistMat, roiHistMat, 0, 255, Core.NORM_MINMAX);
// Debug.Log ("roiHist " + roiHistMat.ToString ());
}
}
}
if (points.Length < 4)
{
for (int i = 0; i < points.Length; i++)
{
Core.circle(rgbaMat, points[i], 6, new Scalar(0, 0, 255, 255), 2);
}
}
else
{
Core.rectangle(rgbaMat, roiRect.tl(), roiRect.br(), new Scalar(0, 255, 0, 255), 2);
Core.rectangle(rgbaMat, roiPred.tl(), roiPred.br(), new Scalar(0, 0, 255, 255), 2);
Core.rectangle(rgbaMat, roiSearch.tl(), roiSearch.br(), new Scalar(255, 0, 0, 255), 2);
}
Core.putText(rgbaMat, "PLEASE TOUCH 4 POINTS", new Point(5, rgbaMat.rows() - 10), Core.FONT_HERSHEY_SIMPLEX, 1.0, new Scalar(255, 255, 255, 255), 2, Core.LINE_AA, false);
// Imgproc.putText (rgbaMat, "W:" + rgbaMat.width () + " H:" + rgbaMat.height () + " SO:" + Screen.orientation, new Point (5, rgbaMat.rows () - 10), Core.FONT_HERSHEY_SIMPLEX, 1.0, new Scalar (255, 255, 255, 255), 2, Imgproc.LINE_AA, false);
Utils.matToTexture2D(rgbaMat, texture, colors);
}
}
/// <summary>
/// Raises the web cam texture to mat helper inited event.
/// </summary>
public void OnWebCamTextureToMatHelperInited()
{
Debug.Log("OnWebCamTextureToMatHelperInited");
Mat webCamTextureMat = webCamTextureToMatHelper.GetMat();
texture = new Texture2D(webCamTextureMat.cols(), webCamTextureMat.rows(), TextureFormat.RGBA32, false);
gameObject.GetComponent <Renderer> ().material.mainTexture = texture;
gameObject.transform.localScale = new Vector3(webCamTextureMat.cols(), webCamTextureMat.rows(), 1);
Debug.Log("Screen.width " + Screen.width + " Screen.height " + Screen.height + " Screen.orientation " + Screen.orientation);
float width = webCamTextureMat.width();
float height = webCamTextureMat.height();
float imageSizeScale = 1.0f;
float widthScale = (float)Screen.width / width;
float heightScale = (float)Screen.height / height;
if (widthScale < heightScale)
{
Camera.main.orthographicSize = (width * (float)Screen.height / (float)Screen.width) / 2;
imageSizeScale = (float)Screen.height / (float)Screen.width;
}
else
{
Camera.main.orthographicSize = height / 2;
}
//set cameraparam
int max_d = (int)Mathf.Max(width, height);
double fx = max_d;
double fy = max_d;
double cx = width / 2.0f;
double cy = height / 2.0f;
camMatrix = new Mat(3, 3, CvType.CV_64FC1);
camMatrix.put(0, 0, fx);
camMatrix.put(0, 1, 0);
camMatrix.put(0, 2, cx);
camMatrix.put(1, 0, 0);
camMatrix.put(1, 1, fy);
camMatrix.put(1, 2, cy);
camMatrix.put(2, 0, 0);
camMatrix.put(2, 1, 0);
camMatrix.put(2, 2, 1.0f);
Debug.Log("camMatrix " + camMatrix.dump());
distCoeffs = new MatOfDouble(0, 0, 0, 0);
Debug.Log("distCoeffs " + distCoeffs.dump());
//calibration camera
Size imageSize = new Size(width * imageSizeScale, height * imageSizeScale);
double apertureWidth = 0;
double apertureHeight = 0;
double[] fovx = new double[1];
double[] fovy = new double[1];
double[] focalLength = new double[1];
Point principalPoint = new Point(0, 0);
double[] aspectratio = new double[1];
Calib3d.calibrationMatrixValues(camMatrix, imageSize, apertureWidth, apertureHeight, fovx, fovy, focalLength, principalPoint, aspectratio);
Debug.Log("imageSize " + imageSize.ToString());
Debug.Log("apertureWidth " + apertureWidth);
Debug.Log("apertureHeight " + apertureHeight);
Debug.Log("fovx " + fovx [0]);
Debug.Log("fovy " + fovy [0]);
Debug.Log("focalLength " + focalLength [0]);
Debug.Log("principalPoint " + principalPoint.ToString());
Debug.Log("aspectratio " + aspectratio [0]);
//To convert the difference of the FOV value of the OpenCV and Unity.
double fovXScale = (2.0 * Mathf.Atan((float)(imageSize.width / (2.0 * fx)))) / (Mathf.Atan2((float)cx, (float)fx) + Mathf.Atan2((float)(imageSize.width - cx), (float)fx));
double fovYScale = (2.0 * Mathf.Atan((float)(imageSize.height / (2.0 * fy)))) / (Mathf.Atan2((float)cy, (float)fy) + Mathf.Atan2((float)(imageSize.height - cy), (float)fy));
Debug.Log("fovXScale " + fovXScale);
Debug.Log("fovYScale " + fovYScale);
//Adjust Unity Camera FOV https://github.com/opencv/opencv/commit/8ed1945ccd52501f5ab22bdec6aa1f91f1e2cfd4
if (widthScale < heightScale)
{
ARCamera.fieldOfView = (float)(fovx [0] * fovXScale);
}
else
{
ARCamera.fieldOfView = (float)(fovy [0] * fovYScale);
}
rgbMat = new Mat(webCamTextureMat.rows(), webCamTextureMat.cols(), CvType.CV_8UC3);
ids = new Mat();
corners = new List <Mat> ();
rejected = new List <Mat> ();
rvecs = new Mat();
tvecs = new Mat();
rotMat = new Mat(3, 3, CvType.CV_64FC1);
transformationM = new Matrix4x4();
invertYM = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, new Vector3(1, -1, 1));
Debug.Log("invertYM " + invertYM.ToString());
invertZM = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, new Vector3(1, 1, -1));
Debug.Log("invertZM " + invertZM.ToString());
detectorParams = DetectorParameters.create();
dictionary = Aruco.getPredefinedDictionary(Aruco.DICT_6X6_250);
//if WebCamera is frontFaceing,flip Mat.
if (webCamTextureToMatHelper.GetWebCamDevice().isFrontFacing)
{
webCamTextureToMatHelper.flipHorizontal = true;
}
}