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;
}