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