コード例 #1
0
ファイル: CamShift.cs プロジェクト: chrissav/thesis
        // 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);
            }
        }
コード例 #2
0
        /// <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;
            }
        }