private bool localise() { var watch = new System.Diagnostics.Stopwatch(); watch.Start(); ground.transform.position = new Vector3(0, 0, 0); ground.transform.rotation = new Quaternion(0, 0, 0, 0); int selectedLine = 0; /* * if (currentLine.Equals("Left")) * { * selectedLine = 0; * } * else if (currentLine.Equals("Center")) * { * selectedLine = 1; * } * else * { * selectedLine = 2; * } */ if (selectedLine == 0) { //ground.transform.rotation.SetEulerAngles(0, 0, 0); } if (selectedLine == 2) { //ground.transform.rotation.SetEulerAngles(0, 0, (float) - Math.PI/2); } if (OpenCVInterop.sendImage(ref pixels, ref width, ref height, ref selectedLine) == 1) { Vector3 offset = ground.transform.localPosition; // THIS DOESNT DO ANYTHING ATM, LEGACY CODE, REFACTOR LATER Vector2[] points = { new Vector2(0 + 150, 0 + 150), new Vector2(0 + 150, 1440 / 4 + 150), new Vector2(800 / 4 + 150, 0 + 150), new Vector2(800 / 4 + 150, 1440 / 4 + 150), }; Vector3[] vertices = new Vector3[4]; if (selectedLine == 2) { Vector3[] tempVert = new Vector3[4]; tempVert[0] = vertices[3]; tempVert[1] = vertices[2]; tempVert[2] = vertices[1]; tempVert[3] = vertices[0]; vertices = tempVert; } for (int i = 0; i < 4; i++) { vertices[i] = ground.transform.TransformPoint(ground.GetComponent <MeshFilter>().mesh.vertices[i]); //Get world positions of vertices } Matrix4x4 transformationMatrix = OpenCVInterop.ComputePNP(points, vertices); Vector3 internalPosition = Vector3.zero; Quaternion internalRotation = Quaternion.identity; internalPosition = transformationMatrix.MultiplyPoint3x4(ground.transform.position); internalRotation = ground.transform.rotation * Quaternion.LookRotation(transformationMatrix.GetColumn(2), -transformationMatrix.GetColumn(1)); var p = ground.transform.parent; //make stadium the camera's child ground.transform.parent = this.gameObject.transform; //apply stored transformation to recreate the position of stadium when camera is at (0,0,0) to localPosition (in relative to camera movement) ground.transform.localPosition = internalPosition; ground.transform.localRotation = internalRotation; //set back the parent of the stadium to what it was before (highest) ground.transform.parent = p; //different coordinate system therefore inverse y, use translate instead of just transform.position as we are moving along self coordinate system ground.transform.Translate(-(new Vector3(offset.x, -offset.y, offset.z)), Space.Self); isDone = true; isLocalised = "Localisation Complete"; watch.Stop(); Debug.Log("Execution Time: " + watch.ElapsedMilliseconds + " ms"); readyToClick = false; MatToTexture2D(); return(true); // Success } else { isLocalised = "Localisation Failed"; readyToClick = false; MatToTexture2D(); return(false); // Failure } }
// Update is called once per frame void Update() { if (Input.GetMouseButtonDown(0)) { Vector2 mousePos = new Vector2(); mousePos = Input.mousePosition; if (i < 4) { points[i] = mousePos; i++; Debug.Log(mousePos); } else if (i == 4) { // Let the user select points in clockwise order from top left, rather than the natural order of vertices in a quad Vector2[] reorderedPoints = { points[3], points[2], points[0], points[1] }; points = reorderedPoints; Vector3[] vertices = new Vector3[4]; for (int i = 0; i < 4; i++) { vertices[i] = ground.transform.TransformPoint(ground.GetComponent <MeshFilter>().mesh.vertices[i]); //Get world positions of vertices } Debug.Log("Vertices: " + vertices[0] + ", " + vertices[1] + ", " + vertices[2] + ", " + vertices[3]); float[] rotationMatrix = new float[9]; float[] translationMatrix = new float[3]; Matrix4x4 transformationMatrix = new Matrix4x4(); OpenCVInterop.ComputePNP(ref vertices, ref points, ref rotationMatrix, ref translationMatrix); transformationMatrix[0, 0] = rotationMatrix[0]; transformationMatrix[0, 1] = rotationMatrix[1]; transformationMatrix[0, 2] = rotationMatrix[2]; transformationMatrix[0, 3] = translationMatrix[0]; transformationMatrix[1, 0] = rotationMatrix[3]; transformationMatrix[1, 1] = rotationMatrix[4]; transformationMatrix[1, 2] = rotationMatrix[5]; transformationMatrix[1, 3] = translationMatrix[1]; transformationMatrix[2, 0] = rotationMatrix[6]; transformationMatrix[2, 1] = rotationMatrix[7]; transformationMatrix[2, 2] = rotationMatrix[8]; transformationMatrix[2, 3] = translationMatrix[2]; transformationMatrix[3, 3] = 1; //Convert from OpenCV to Unity coordinates Matrix4x4 invertYM = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, new Vector3(1, -1, 1)); transformationMatrix = transformationMatrix * invertYM; //transformationMatrix = transformationMatrix * this.gameObject.transform.localToWorldMatrix; Debug.Log(transformationMatrix.ToString()); ground.transform.position = transformationMatrix.MultiplyPoint3x4(ground.transform.position); ground.transform.rotation *= Quaternion.LookRotation(transformationMatrix.GetColumn(2), -transformationMatrix.GetColumn(1)); ground.GetComponent <MeshRenderer>().enabled = true; } } }
void DoTheThing() { ground.transform.position = new Vector3(0, 0, 0); ground.transform.rotation = new Quaternion(0, 0, 0, 0); Vector3 offset = ground.transform.localPosition; /*Vector2[] points = { new Vector2(0, 0), * new Vector2(0, 1440), * new Vector2(800, 0), * new Vector2(800, 1440), * };*/ Vector2[] points = { new Vector2(0 + 150, 0 + 150), new Vector2(0 + 150, 1440 / 4 + 150), new Vector2(800 / 4 + 150, 0 + 150), new Vector2(800 / 4 + 150, 1440 / 4 + 150), }; Vector3[] vertices = new Vector3[4]; for (int i = 0; i < 4; i++) { vertices[i] = ground.transform.TransformPoint(ground.GetComponent <MeshFilter>().mesh.vertices[i]); //Get world positions of vertices } //Debug.Log("Vertices: " + vertices[0] + ", " + vertices[1] + ", " + vertices[2] + ", " + vertices[3]); float[] rotationMatrix = new float[9]; float[] translationMatrix = new float[3]; Matrix4x4 transformationMatrix = new Matrix4x4(); int h = Screen.height; int w = Screen.width; //OpenCVInterop.ComputePNP(ref vertices, ref points, ref rotationMatrix, ref translationMatrix); IntPtr rotationPtr, translationPtr; //Debug.Log("Rotation Matrix: " + rotationMatrix[0] + ", " + rotationMatrix[1] + ", " + rotationMatrix[2]); //Debug.Log("Size of float: " + sizeof(float)); OpenCVInterop.ComputePNP(ref vertices, ref points, out rotationPtr, out translationPtr, ref w, ref h); //Debug.Log("AFTER DLL CALL ~~~~~~~~~~~~~~~~~~~~~~~~~~~~"); Marshal.Copy(rotationPtr, rotationMatrix, 0, rotationMatrix.Length); Marshal.Copy(translationPtr, translationMatrix, 0, translationMatrix.Length); Marshal.FreeCoTaskMem(rotationPtr); Marshal.FreeCoTaskMem(translationPtr); //////////////////////////// transformationMatrix[0, 0] = rotationMatrix[0]; transformationMatrix[0, 1] = rotationMatrix[1]; transformationMatrix[0, 2] = rotationMatrix[2]; transformationMatrix[0, 3] = translationMatrix[0]; transformationMatrix[1, 0] = rotationMatrix[3]; transformationMatrix[1, 1] = rotationMatrix[4]; transformationMatrix[1, 2] = rotationMatrix[5]; transformationMatrix[1, 3] = translationMatrix[1]; transformationMatrix[2, 0] = rotationMatrix[6]; transformationMatrix[2, 1] = rotationMatrix[7]; transformationMatrix[2, 2] = rotationMatrix[8]; transformationMatrix[2, 3] = translationMatrix[2]; transformationMatrix[3, 3] = 1; //Convert from OpenCV to Unity coordinates Matrix4x4 invertYM = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, new Vector3(-1, 1, -1)); transformationMatrix = transformationMatrix * invertYM; //transformationMatrix = transformationMatrix * this.gameObject.transform.localToWorldMatrix; Debug.Log(transformationMatrix.ToString()); Vector3 position = Vector3.zero; Quaternion rotation = Quaternion.identity; position = transformationMatrix.MultiplyPoint3x4(ground.transform.position); rotation = ground.transform.rotation * Quaternion.LookRotation(transformationMatrix.GetColumn(2), -transformationMatrix.GetColumn(1)); var p = ground.transform.parent; //put stadium into camera's child ground.transform.parent = this.gameObject.transform; //apply stored transformation to recreate the position of stadium when camera is at (0,0,0) to localPosition (in relative to camera movement) ground.transform.localPosition = position; ground.transform.localRotation = rotation; //set back the parent of the stadium to as before (highest) ground.transform.parent = p; //different coordinate system therefore inverse y, use translate instead of just transform.position as we are moving along self coordinate system ground.transform.Translate(-(new Vector3(offset.x, -offset.y, offset.z)), Space.Self); /* * ground.transform.position = transformationMatrix.MultiplyPoint3x4(ground.transform.position); * ground.transform.rotation *= Quaternion.LookRotation(transformationMatrix.GetColumn(2), -transformationMatrix.GetColumn(1)); */ //ground.GetComponent<MeshRenderer>().enabled = true; }