arskrald/AR-2b/Assets/Homo_script.cs

using OpenCVForUnity.CoreModule;
using OpenCVForUnity.ImgprocModule;
using OpenCVForUnity.ObjdetectModule;
using OpenCVForUnity.UnityUtils;
using OpenCVForUnity.UtilsModule;
using UnityEngine;
using OpenCVForUnity.Calib3dModule;
using Vuforia;

public class Homo_script : MonoBehaviour
{

    public Camera cam;

    public GameObject corner1;
    public GameObject corner2;
    public GameObject corner3;
    public GameObject corner4;

    //Default values: guessed, not calibrated
    public float fx = 650;
    public float fy = 650;
    public float cx = 320;
    public float cy = 240;


    private int width = 420;
    private int height = 420;

    private MatOfPoint2f dstPoints;
    private Texture2D outputTexture;

    private Mat camImageMat;
    private byte[] texData;

    private Renderer rend;

    
    private GameObject pls;

    void Start()
    {


        dstPoints = new MatOfPoint2f();
        dstPoints.alloc(4);
        dstPoints.put(0, 0, 0, height);
        dstPoints.put(1, 0, width, height);
        dstPoints.put(2, 0, 0, 0);
        dstPoints.put(3, 0, width, 0);
        outputTexture = new Texture2D(width, height, TextureFormat.RGBA32, false);


        pls = GameObject.Find("flying_skull");
        rend = pls.GetComponent<Renderer>();

        // Debugging
        pls = GameObject.Find("pls");
        rend = pls.GetComponent<Renderer>();

    }

    void Update()
    {

        //Access camera image provided by Vuforia
        Image camImg = CameraDevice.Instance.GetCameraImage(Image.PIXEL_FORMAT.RGBA8888);

        if (camImg != null)
        {
            if (camImg != null)
            {
                if (camImageMat == null)
                {
                    camImageMat = new Mat(camImg.Height, camImg.Width, CvType.CV_8UC4);
                }
                camImageMat.put(0, 0, camImg.Pixels);
            }

            //---- TRANSFORM/PROJECT CORNER WORLD COORDINATES TO IMAGE COORDINATES ----

            Vector3 worldPnt1 = corner1.transform.position;
            Vector3 worldPnt2 = corner2.transform.position;
            Vector3 worldPnt3 = corner3.transform.position;
            Vector3 worldPnt4 = corner4.transform.position;

            //See lecture slides
            Matrix4x4 Rt = cam.transform.worldToLocalMatrix;
            Matrix4x4 A = Matrix4x4.identity;
            A.m00 = fx;
            A.m11 = fy;
            A.m02 = cx;
            A.m12 = cy;

            //See equation for pinhole camera model
            Matrix4x4 worldToImage = A * Rt;

            //Apply transform to get homogeneous image coordinates
            Vector3 hUV1 = worldToImage.MultiplyPoint3x4(worldPnt1);
            Vector3 hUV2 = worldToImage.MultiplyPoint3x4(worldPnt2);
            Vector3 hUV3 = worldToImage.MultiplyPoint3x4(worldPnt3);
            Vector3 hUV4 = worldToImage.MultiplyPoint3x4(worldPnt4);

            //hUV are the image coordinates in homogeneous coordinates, we need to normalize, i.e., divide by Z to get to Cartesian coordinates
            Vector2 uv1 = new Vector2(hUV1.x, hUV1.y) / hUV1.z;
            Vector2 uv2 = new Vector2(hUV2.x, hUV2.y) / hUV2.z;
            Vector2 uv3 = new Vector2(hUV3.x, hUV3.y) / hUV3.z;
            Vector2 uv4 = new Vector2(hUV4.x, hUV4.y) / hUV4.z;

            //Do not forget to alloc before putting values into a MatOfPoint2f (see Start() above)
            //We need to flip the v-coordinates, see coordinate system overview
            float maxV = camImg.Height - 1;

            MatOfPoint2f imagePoints = new MatOfPoint2f();
            imagePoints.alloc(4);

            imagePoints.put(0, 0, uv1.x, maxV - uv1.y);
            imagePoints.put(1, 0, uv2.x, maxV - uv2.y);
            imagePoints.put(2, 0, uv3.x, maxV - uv3.y);
            imagePoints.put(3, 0, uv4.x, maxV - uv4.y);


            //Debug draw points using OpenCV's drawing functions
            Point imgPnt1 = new Point(imagePoints.get(0, 0));
            Point imgPnt2 = new Point(imagePoints.get(1, 0));
            Point imgPnt3 = new Point(imagePoints.get(2, 0));
            Point imgPnt4 = new Point(imagePoints.get(3, 0));
            Mat camImgCopy = camImageMat.clone();
            Mat outputMat = camImgCopy.clone();
            Imgproc.circle(camImageMat, imgPnt1, 5, new Scalar(255, 0, 0, 255));
            Imgproc.circle(camImageMat, imgPnt2, 5, new Scalar(0, 255, 0, 255));
            Imgproc.circle(camImageMat, imgPnt3, 5, new Scalar(0, 0, 255, 255));
            Imgproc.circle(camImageMat, imgPnt4, 5, new Scalar(255, 255, 0, 255));


            Mat homo = Calib3d.findHomography(imagePoints, dstPoints);

            Imgproc.warpPerspective(camImgCopy, outputMat, homo, new Size(outputMat.width(), outputMat.height()));

            var rectOutputMat = new Mat(outputMat, new OpenCVForUnity.CoreModule.Rect(0, 0, width, height));

            MatDisplay.MatToTexture(rectOutputMat, ref outputTexture);

  
            //Display the Mat that includes video feed and debug points
            MatDisplay.DisplayMat(camImgCopy, MatDisplaySettings.FULL_BACKGROUND);

            rend.material.mainTexture = outputTexture;

            // Makes camera prettier
            cam.fieldOfView = 2 * Mathf.Atan(camImg.Height * 0.5f / fy) * Mathf.Rad2Deg;

            imagePoints.Dispose();

        }
    }

}