234 lines
7.2 KiB
C#
234 lines
7.2 KiB
C#
|
using OpenCVForUnity.CoreModule;
|
|||
|
|
using OpenCVForUnity.ImgprocModule;
|
|||
|
|
using OpenCVForUnity.ObjdetectModule;
|
|||
|
|
using OpenCVForUnity.UnityUtils;
|
|||
|
|
using OpenCVForUnity.UtilsModule;
|
|||
|
|
using UnityEngine;
|
|||
|
|
using OpenCVForUnity.Calib3dModule;
|
|||
|
|
using System.Linq;
|
|||
|
|
using Vuforia;
|
|||
|
|
|
|||
|
|
public class homo_draw : MonoBehaviour
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
public Camera cam;
|
|||
|
|
|
|||
|
|
public GameObject corner1;
|
|||
|
|
public GameObject corner2;
|
|||
|
|
public GameObject corner3;
|
|||
|
|
public GameObject corner4;
|
|||
|
|
public GameObject fingerQuad;
|
|||
|
|
|
|||
|
|
//Default values: guessed, not calibrated
|
|||
|
|
public float fx = 650;
|
|||
|
|
public float fy = 650;
|
|||
|
|
public float cx = 320;
|
|||
|
|
public float cy = 240;
|
|||
|
|
|
|||
|
|
|
|||
|
|
private int width = 600;
|
|||
|
|
private int height = 424;
|
|||
|
|
|
|||
|
|
private MatOfPoint2f dstPoints;
|
|||
|
|
private Texture2D planeOutText;
|
|||
|
|
|
|||
|
|
private Mat camImageMat;
|
|||
|
|
|
|||
|
|
private Renderer planeRend;
|
|||
|
|
|
|||
|
|
public int DarknessThreshold;
|
|||
|
|
|
|||
|
|
private Mat drawingMat;
|
|||
|
|
|
|||
|
|
|
|||
|
|
void Start()
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
|
|||
|
|
dstPoints = new MatOfPoint2f();
|
|||
|
|
dstPoints.alloc(4);
|
|||
|
|
/*dstPoints.put(0, 0, 0, height);
|
|||
|
|
dstPoints.put(1, 0, width, height);
|
|||
|
|
dstPoints.put(3, 0, width, 0);
|
|||
|
|
dstPoints.put(2, 0, 0, 0);*/
|
|||
|
|
|
|||
|
|
dstPoints.put(2, 0, 0, 0);
|
|||
|
|
dstPoints.put(3, 0, width, 0);
|
|||
|
|
dstPoints.put(0, 0, 0, height);
|
|||
|
|
dstPoints.put(1, 0, width, height);
|
|||
|
|
|
|||
|
|
planeRend = GameObject.Find("Plane").GetComponent<Renderer>();
|
|||
|
|
|
|||
|
|
planeOutText = new Texture2D(width, height, TextureFormat.RGBA32, false);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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);
|
|||
|
|
drawingMat = new Mat(camImageMat.rows(), camImageMat.cols(), CvType.CV_8UC4, new Scalar(0, 0, 0, 0));
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
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;
|
|||
|
|
Vector3 fingerPnt = fingerQuad.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);
|
|||
|
|
|
|||
|
|
Vector3 hUV5 = worldToImage.MultiplyPoint3x4(fingerPnt);
|
|||
|
|
|
|||
|
|
|
|||
|
|
//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;
|
|||
|
|
|
|||
|
|
Vector2 uv5 = new Vector2(hUV5.x, hUV5.y) / hUV5.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));
|
|||
|
|
|
|||
|
|
Point imgPnt5 = new Point(uv5.x, maxV - uv5.y);
|
|||
|
|
|
|||
|
|
Mat camImgCopy = camImageMat.clone();
|
|||
|
|
Mat outputMat = camImgCopy.clone();
|
|||
|
|
|
|||
|
|
|
|||
|
|
if (false)
|
|||
|
|
{
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
|
|||
|
|
Mat output = camImgCopy.clone();
|
|||
|
|
//Mat outputSkullMat = skullTextureMat.clone();
|
|||
|
|
|
|||
|
|
Imgproc.warpPerspective(camImageMat, output, homo, output.size());
|
|||
|
|
|
|||
|
|
|
|||
|
|
Imgproc.circle(output, new Point(45, 390), 5, new Scalar(255, 0, 0, 255));
|
|||
|
|
|
|||
|
|
|
|||
|
|
var r = output.get(390, 45)[0];
|
|||
|
|
var g = output.get(390, 45)[1];
|
|||
|
|
var b = output.get(390, 45)[2];
|
|||
|
|
|
|||
|
|
var c_r = output.get(215, 45)[0];
|
|||
|
|
var c_g = output.get(215, 45)[1];
|
|||
|
|
var c_b = output.get(215, 45)[2];
|
|||
|
|
|
|||
|
|
|
|||
|
|
Mat yetAnotherClone = camImgCopy.clone();
|
|||
|
|
print(r + g + b);
|
|||
|
|
|
|||
|
|
if (r+g+b < DarknessThreshold)
|
|||
|
|
{
|
|||
|
|
print("finger lol");
|
|||
|
|
|
|||
|
|
var lelImg = output.clone();
|
|||
|
|
|
|||
|
|
Imgproc.cvtColor(output, lelImg, Imgproc.COLOR_BGR2HSV);
|
|||
|
|
|
|||
|
|
var newOutput = output.clone();
|
|||
|
|
MatDisplay.DisplayMat(newOutput, MatDisplaySettings.BOTTOM_LEFT);
|
|||
|
|
var nicePoint = LocateFinger(newOutput);
|
|||
|
|
if (nicePoint != null)
|
|||
|
|
{
|
|||
|
|
print("Finger Found");
|
|||
|
|
Imgproc.circle(drawingMat, nicePoint, 5, new Scalar(c_r, c_g, c_b, 255), -1);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MatDisplay.MatToTexture(newOutput, ref planeOutText);
|
|||
|
|
|
|||
|
|
planeRend.sharedMaterial.mainTexture = planeOutText;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Mat circleClone = drawingMat.clone();
|
|||
|
|
|
|||
|
|
Imgproc.warpPerspective(drawingMat, circleClone, homo.inv(), circleClone.size());
|
|||
|
|
|
|||
|
|
Core.addWeighted(camImageMat, 0.95f, circleClone, 0.7f, 0.0f, yetAnotherClone);
|
|||
|
|
|
|||
|
|
|
|||
|
|
//Display the Mat that includes video feed and debug points
|
|||
|
|
MatDisplay.DisplayMat(yetAnotherClone, MatDisplaySettings.FULL_BACKGROUND);
|
|||
|
|
|
|||
|
|
imagePoints.Dispose();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Point LocateFinger(Mat fingerMat)
|
|||
|
|
{
|
|||
|
|
for (int i = 70; i < height-30; i++)
|
|||
|
|
{
|
|||
|
|
for (int j = 85; j < width-30; j++)
|
|||
|
|
{
|
|||
|
|
if (fingerMat.get(i, j)[0] == 255)
|
|||
|
|
{
|
|||
|
|
return new Point(j, i);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|