public static void writeRotatedRect(Mat mat, RotatedRect rRect)
{
Point[] vertices = new Point[4];
rRect.points(vertices);
for (int i = 0; i < 4; i++)
{
Imgproc.line(mat, vertices [i], vertices [(i + 1) % 4], new Scalar(0, 255, 0));
}
}
// Update is called once per frame
void Update()
{
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);
RotatedRect r = Video.CamShift(backProj, roiRect, termination);
r.points(points);
}
#if ((UNITY_ANDROID || UNITY_IOS) && !UNITY_EDITOR)
//Touch
int touchCount = Input.touchCount;
if (touchCount == 1)
{
if (Input.GetTouch(0).phase == TouchPhase.Ended)
{
roiPointList.Clear();
}
}
#else
if (Input.GetMouseButtonUp(0))
{
roiPointList.Clear();
}
#endif
}
if (roiPointList.Count < 4)
{
#if ((UNITY_ANDROID || UNITY_IOS) && !UNITY_EDITOR)
//Touch
int touchCount = Input.touchCount;
if (touchCount == 1)
{
Touch t = Input.GetTouch(0);
if (t.phase == TouchPhase.Ended)
{
roiPointList.Add(convertScreenPoint(new Point(t.position.x, t.position.y), gameObject, Camera.main));
// Debug.Log ("touch X " + t.position.x);
// Debug.Log ("touch Y " + t.position.y);
if (!(new OpenCVForUnity.Rect(0, 0, hsvMat.width(), hsvMat.height()).contains(roiPointList [roiPointList.Count - 1])))
{
roiPointList.RemoveAt(roiPointList.Count - 1);
}
}
}
#else
//Mouse
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);
}
}
#endif
if (roiPointList.Count == 4)
{
using (MatOfPoint roiPointMat = new MatOfPoint(roiPointList.ToArray())) {
roiRect = Imgproc.boundingRect(roiPointMat);
}
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++)
{
Imgproc.circle(rgbaMat, points [i], 6, new Scalar(0, 0, 255, 255), 2);
}
}
else
{
for (int i = 0; i < 4; i++)
{
Imgproc.line(rgbaMat, points [i], points [(i + 1) % 4], new Scalar(255, 0, 0, 255), 2);
}
Imgproc.rectangle(rgbaMat, roiRect.tl(), roiRect.br(), new Scalar(0, 255, 0, 255), 2);
}
Imgproc.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, Imgproc.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, webCamTextureToMatHelper.GetBufferColors());
}
}
// Update is called once per frame
void Update()
{
// Debug.Log("Did update this frame: " + webCamTextureToMatHelper.didUpdateThisFrame());
// Debug.Log("WebCam Texture is playing: " + webCamTextureToMatHelper.isPlaying());
if (webCamTextureToMatHelper.isPlaying() && webCamTextureToMatHelper.didUpdateThisFrame())
{
///////////////////////////////////////////////////////////////
// Acquire the next frame from the camera and undistort if necessary
///////////////////////////////////////////////////////////////
Mat rgbaMat = webCamTextureToMatHelper.GetMat();
if (AppControl.control.calibrationComplete && false)
{
Mat rgbaMatUndistorted = rgbaMat.clone();
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);
///////////////////////////////////////////////////////////////
// If the object color spectrum is initialized, find the object
///////////////////////////////////////////////////////////////
// Debug.Log("Number of ROI points: " + roiPointList.Count);
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);
// Video.meanShift(backProj, roiRect, termination);
RotatedRect r = Video.CamShift(backProj, roiRect, termination);
r.points(points);
if (roiRect.height > 0 && roiRect.width > 0)
{
objectFound = true;
AppControl.control.TargetBox = roiRect;
// Debug.Log("Object found: " + objectFound);
}
else
{
objectFound = false;
}
}
// Estimate the 3D position of the object
if (objectFound && AppControl.control.calibrationComplete)
{
float fx = AppControl.control.fx; // focal length x-axis
float fy = AppControl.control.fy; // focal length y-axis
float cx = AppControl.control.cx; // focal length x-axis
float cy = AppControl.control.cy; // focal length y-axis
float xu = (roiRect.x) + 0.5f * roiRect.width - cx; // undistorted object center along x-axis in pixels
float yu = -((roiRect.y) + 0.5f * roiRect.height - cy); // undistorted object center along y-axis in pixels
float du = (roiRect.height + roiRect.width) / 2f; // Size of the object in pixels
float pz = (objectSize / du) * (1 / Mathf.Sqrt(1f / Mathf.Pow(fx, 2) + 1f / Mathf.Pow(fy, 2)));
float px = xu * pz / fx;
float py = yu * pz / fy;
objectPosition = new Vector3(px, py, pz); // in units of mm
target = GameObject.FindWithTag("Target");
stick = GameObject.FindWithTag("Breadboard");
if (target != null && stick != null)
{
target.transform.position = mainCamera.transform.position + 0.001f * objectPosition; // in units of meters
AppControl.control.camObservations = new double[] { xu, yu, du };
AppControl.control.writeData();
// stick.transform.rotation = mainCamera.transform.rotation * stick.transform.rotation * Quaternion.Inverse(mainCamera.transform.rotation);
// stick.transform.position = stick.transform.position
}
}
else
{
objectPosition = new Vector3(0, 0, 0);
}
// not necessary to reset the object tracking by clicking
/*
#if ((UNITY_ANDROID || UNITY_IOS) && !UNITY_EDITOR)
* //Touch
* int touchCount = Input.touchCount;
* if (touchCount == 1)
* {
* if(Input.GetTouch(0).phase == TouchPhase.Ended){
* roiPointList.Clear ();
* }
* }
#else
* if (Input.GetMouseButtonUp (0)) {
* roiPointList.Clear ();
* }
#endif
*/
}
///////////////////////////////////////////////////////////////
// Capture the ROI from user input; compute the HSV histogram
// of the ROI
///////////////////////////////////////////////////////////////
if (roiPointList.Count < 4)
{
#if ((UNITY_ANDROID || UNITY_IOS) && !UNITY_EDITOR)
//Touch
int touchCount = Input.touchCount;
if (touchCount == 1)
{
Touch t = Input.GetTouch(0);
if (t.phase == TouchPhase.Ended)
{
roiPointList.Add(convertScreenPoint(new Point(t.position.x, t.position.y), gameObject, Camera.main));
// Debug.Log ("touch X " + t.position.x);
// Debug.Log ("touch Y " + t.position.y);
if (!(new OpenCVForUnity.Rect(0, 0, hsvMat.width(), hsvMat.height()).contains(roiPointList [roiPointList.Count - 1])))
{
roiPointList.RemoveAt(roiPointList.Count - 1);
}
}
}
#else
// Capture mouse input events and add the points to the ROI List
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);
}
}
#endif
// If the user has selected four points, lock in the ROI; compute
// the HSV histogram of the ROI
if (roiPointList.Count == 4)
{
using (MatOfPoint roiPointMat = new MatOfPoint(roiPointList.ToArray())) {
roiRect = Imgproc.boundingRect(roiPointMat);
}
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 ());
}
}
}
///////////////////////////////////////////////////////////////
// Draw the an image that displays where the user has pressed
///////////////////////////////////////////////////////////////
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
{
for (int i = 0; i < 4; i++)
{
Core.line(rgbaMat, points [i], points [(i + 1) % 4], new Scalar(255, 0, 0, 255), 2);
}
Core.rectangle(rgbaMat, roiRect.tl(), roiRect.br(), new Scalar(0, 255, 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);
}
}
// Update is called once per frame
void Update()
{
if (!initDone)
{
return;
}
if (screenOrientation != Screen.orientation)
{
screenOrientation = Screen.orientation;
updateLayout();
}
#if UNITY_IOS && !UNITY_EDITOR && (UNITY_4_6_3 || UNITY_4_6_4 || UNITY_5_0_0 || UNITY_5_0_1)
if (webCamTexture.width > 16 && webCamTexture.height > 16)
{
#else
if (webCamTexture.didUpdateThisFrame)
{
#endif
Utils.webCamTextureToMat(webCamTexture, rgbaMat, colors);
//flip to correct direction.
if (webCamDevice.isFrontFacing)
{
if (webCamTexture.videoRotationAngle == 0)
{
Core.flip(rgbaMat, rgbaMat, 1);
}
else if (webCamTexture.videoRotationAngle == 90)
{
Core.flip(rgbaMat, rgbaMat, 0);
}
if (webCamTexture.videoRotationAngle == 180)
{
Core.flip(rgbaMat, rgbaMat, 0);
}
else if (webCamTexture.videoRotationAngle == 270)
{
Core.flip(rgbaMat, rgbaMat, 1);
}
}
else
{
if (webCamTexture.videoRotationAngle == 180)
{
Core.flip(rgbaMat, rgbaMat, -1);
}
else if (webCamTexture.videoRotationAngle == 270)
{
Core.flip(rgbaMat, rgbaMat, -1);
}
}
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);
RotatedRect r = Video.CamShift(backProj, roiRect, termination);
r.points(points);
}
#if ((UNITY_ANDROID || UNITY_IOS) && !UNITY_EDITOR)
//Touch
int touchCount = Input.touchCount;
if (touchCount == 1)
{
if (Input.GetTouch(0).phase == TouchPhase.Ended)
{
roiPointList.Clear();
}
}
#else
if (Input.GetMouseButtonUp(0))
{
roiPointList.Clear();
}
#endif
}
if (roiPointList.Count < 4)
{
#if ((UNITY_ANDROID || UNITY_IOS) && !UNITY_EDITOR)
//Touch
int touchCount = Input.touchCount;
if (touchCount == 1)
{
Touch t = Input.GetTouch(0);
if (t.phase == TouchPhase.Ended)
{
roiPointList.Add(convertScreenPoint(new Point(t.position.x, t.position.y), gameObject, Camera.main));
// Debug.Log ("touch X " + t.position.x);
// Debug.Log ("touch Y " + t.position.y);
if (!(new OpenCVForUnity.Rect(0, 0, hsvMat.width(), hsvMat.height()).contains(roiPointList [roiPointList.Count - 1])))
{
roiPointList.RemoveAt(roiPointList.Count - 1);
}
}
}
#else
//Mouse
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);
}
}
#endif
if (roiPointList.Count == 4)
{
using (MatOfPoint roiPointMat = new MatOfPoint(roiPointList.ToArray())) {
roiRect = Imgproc.boundingRect(roiPointMat);
}
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
{
for (int i = 0; i < 4; i++)
{
Core.line(rgbaMat, points [i], points [(i + 1) % 4], new Scalar(255, 0, 0, 255), 2);
}
Core.rectangle(rgbaMat, roiRect.tl(), roiRect.br(), new Scalar(0, 255, 0, 255), 2);
}
Core.putText(rgbaMat, "PLEASE TOUCH 4 POINTS", new Point(5, 25), Core.FONT_HERSHEY_SIMPLEX, 1.0, new Scalar(255, 255, 255, 255), 2, Core.LINE_AA, false);
Utils.matToTexture2D(rgbaMat, texture, colors);
gameObject.GetComponent <Renderer> ().material.mainTexture = texture;
}
}
void OnDisable()
{
webCamTexture.Stop();
}
void OnGUI()
{
float screenScale = Screen.height / 240.0f;
Matrix4x4 scaledMatrix = Matrix4x4.Scale(new Vector3(screenScale, screenScale, screenScale));
GUI.matrix = scaledMatrix;
GUILayout.BeginVertical();
if (GUILayout.Button("back"))
{
Application.LoadLevel("OpenCVForUnitySample");
}
if (GUILayout.Button("change camera"))
{
shouldUseFrontFacing = !shouldUseFrontFacing;
StartCoroutine(init());
}
GUILayout.EndVertical();
}
// Update is called once per frame
void Update()
{
#if ((UNITY_ANDROID || UNITY_IOS) && !UNITY_EDITOR)
//Touch
int touchCount = Input.touchCount;
if (touchCount == 1)
{
Touch t = Input.GetTouch(0);
if (t.phase == TouchPhase.Ended && !EventSystem.current.IsPointerOverGameObject(t.fingerId))
{
storedTouchPoint = new Point(t.position.x, t.position.y);
//Debug.Log ("touch X " + t.position.x);
//Debug.Log ("touch Y " + t.position.y);
}
}
#else
//Mouse
if (Input.GetMouseButtonUp(0) && !EventSystem.current.IsPointerOverGameObject())
{
storedTouchPoint = new Point(Input.mousePosition.x, Input.mousePosition.y);
//Debug.Log ("mouse X " + Input.mousePosition.x);
//Debug.Log ("mouse Y " + Input.mousePosition.y);
}
#endif
if (webCamTextureToMatHelper.IsPlaying() && webCamTextureToMatHelper.DidUpdateThisFrame())
{
Mat rgbaMat = webCamTextureToMatHelper.GetMat();
Imgproc.cvtColor(rgbaMat, hsvMat, Imgproc.COLOR_RGBA2RGB);
Imgproc.cvtColor(hsvMat, hsvMat, Imgproc.COLOR_RGB2HSV);
if (storedTouchPoint != null)
{
ConvertScreenPointToTexturePoint(storedTouchPoint, storedTouchPoint, gameObject, rgbaMat.cols(), rgbaMat.rows());
OnTouch(rgbaMat, storedTouchPoint);
storedTouchPoint = null;
}
Point[] points = roiPointList.ToArray();
if (shouldStartCamShift)
{
shouldStartCamShift = false;
using (MatOfPoint roiPointMat = new MatOfPoint(points)) {
roiRect = Imgproc.boundingRect(roiPointMat);
}
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 ());
}
}
else if (points.Length == 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);
RotatedRect r = Video.CamShift(backProj, roiRect, termination);
r.points(points);
}
}
if (points.Length < 4)
{
for (int i = 0; i < points.Length; i++)
{
Imgproc.circle(rgbaMat, points [i], 6, new Scalar(0, 0, 255, 255), 2);
}
}
else
{
for (int i = 0; i < 4; i++)
{
Imgproc.line(rgbaMat, points [i], points [(i + 1) % 4], new Scalar(255, 0, 0, 255), 2);
}
Imgproc.rectangle(rgbaMat, roiRect.tl(), roiRect.br(), new Scalar(0, 255, 0, 255), 2);
}
// Imgproc.putText (rgbaMat, "W:" + rgbaMat.width () + " H:" + rgbaMat.height () + " SO:" + Screen.orientation, new Point (5, rgbaMat.rows () - 10), Imgproc.FONT_HERSHEY_SIMPLEX, 1.0, new Scalar (255, 255, 255, 255), 2, Imgproc.LINE_AA, false);
Utils.fastMatToTexture2D(rgbaMat, texture);
}
}
// 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);
}
}
// Use this for initialization
void Run()
{
//if true, The error log of the Native side OpenCV will be displayed on the Unity Editor Console.
Utils.setDebugMode(true);
Mat img = Imgcodecs.imread(image_filepath, Imgcodecs.IMREAD_COLOR);
if (img.empty())
{
Debug.LogError(image_filepath + " is not loaded. Please see \"StreamingAssets/dnn/setup_dnn_module.pdf\". ");
img = new Mat(368, 368, CvType.CV_8UC3, new Scalar(0, 0, 0));
}
//Adust Quad.transform.localScale.
gameObject.transform.localScale = new Vector3(img.width(), img.height(), 1);
Debug.Log("Screen.width " + Screen.width + " Screen.height " + Screen.height + " Screen.orientation " + Screen.orientation);
float imageWidth = img.width();
float imageHeight = img.height();
float widthScale = (float)Screen.width / imageWidth;
float heightScale = (float)Screen.height / imageHeight;
if (widthScale < heightScale)
{
Camera.main.orthographicSize = (imageWidth * (float)Screen.height / (float)Screen.width) / 2;
}
else
{
Camera.main.orthographicSize = imageHeight / 2;
}
Net detector = null;
Net recognizer = null;
if (string.IsNullOrEmpty(detectionmodel_filepath) || string.IsNullOrEmpty(recognitionmodel_filepath))
{
Debug.LogError(detectionmodel_filepath + " or " + recognitionmodel_filepath + " is not loaded. Please see \"StreamingAssets/dnn/setup_dnn_module.pdf\". ");
}
else
{
detector = Dnn.readNet(detectionmodel_filepath);
recognizer = Dnn.readNet(recognitionmodel_filepath);
}
if (detector == null || recognizer == null)
{
Imgproc.putText(img, "model file is not loaded.", new Point(5, img.rows() - 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255), 2, Imgproc.LINE_AA, false);
Imgproc.putText(img, "Please read console message.", new Point(5, img.rows() - 10), Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255), 2, Imgproc.LINE_AA, false);
}
else
{
TickMeter tickMeter = new TickMeter();
List <Mat> outs = new List <Mat>();
List <string> outNames = new List <string>();
outNames.Add("feature_fusion/Conv_7/Sigmoid");
outNames.Add("feature_fusion/concat_3");
// Create a 4D blob from a frame.
Size inpSize = new Size(inpWidth > 0 ? inpWidth : img.cols(), inpHeight > 0 ? inpHeight : img.rows());
Mat blob = Dnn.blobFromImage(img, 1.0, inpSize, new Scalar(123.68, 116.78, 103.94), true, false); // blobFromImage(frame, blob, 1.0, Size(inpWidth, inpHeight), Scalar(123.68, 116.78, 103.94), true, false);
// Run detection model.
detector.setInput(blob);
tickMeter.start();
detector.forward(outs, outNames);
tickMeter.stop();
Mat scores = outs[0];
Mat geometry = outs[1];
// Decode predicted bounding boxes.
List <RotatedRect> boxes = new List <RotatedRect>();
List <float> confidences = new List <float>();
decodeBoundingBoxes(scores, geometry, confThreshold, boxes, confidences);
// Apply non-maximum suppression procedure.
MatOfRotatedRect boxesMat = new MatOfRotatedRect(boxes.ToArray());
MatOfFloat confidencesMat = new MatOfFloat(confidences.ToArray());
MatOfInt indicesMat = new MatOfInt();
Dnn.NMSBoxesRotated(boxesMat, confidencesMat, confThreshold, nmsThreshold, indicesMat);
List <int> indices = indicesMat.toList();
Point ratio = new Point(img.cols() / inpWidth, img.rows() / inpHeight);
// Render text.
for (int i = 0; i < indices.Count; ++i)
{
RotatedRect box = boxes[indices[i]];
Point[] vertices = new Point[4];
box.points(vertices);
for (int j = 0; j < 4; ++j)
{
vertices[j].x *= ratio.x;
vertices[j].y *= ratio.y;
}
for (int j = 0; j < 4; ++j)
{
Imgproc.line(img, vertices[j], vertices[(j + 1) % 4], new Scalar(0, 255, 0), 1);
}
if (recognizer != null)
{
Mat cropped = new Mat();
fourPointsTransform(img, vertices, cropped);
//Debug.Log(cropped);
Imgproc.cvtColor(cropped, cropped, Imgproc.COLOR_BGR2GRAY);
Mat blobCrop = Dnn.blobFromImage(cropped, 1.0 / 127.5, new Size(), Scalar.all(127.5));
recognizer.setInput(blobCrop);
//Debug.Log(blobCrop);
tickMeter.start();
Mat result = recognizer.forward();
tickMeter.stop();
string wordRecognized;
decodeText(result, out wordRecognized);
Imgproc.putText(img, wordRecognized, vertices[1], Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, new Scalar(255, 0, 0), 1, Imgproc.LINE_AA, false);
Debug.Log(wordRecognized);
cropped.Dispose();
blobCrop.Dispose();
result.Dispose();
}
}
Debug.Log("Inference time, ms: " + tickMeter.getTimeMilli());
for (int i = 0; i < outs.Count; i++)
{
outs[i].Dispose();
}
blob.Dispose();
detector.Dispose();
recognizer.Dispose();
}
Imgproc.cvtColor(img, img, Imgproc.COLOR_BGR2RGB);
Texture2D texture = new Texture2D(img.cols(), img.rows(), TextureFormat.RGBA32, false);
Utils.matToTexture2D(img, texture);
gameObject.GetComponent <Renderer>().material.mainTexture = texture;
Utils.setDebugMode(false);
}