Pixel coords

.gitignore

@@ -12,6 +12,7 @@
 /[Uu]serSettings
 /[Ii]mages
 /[Mm]askedImages
+/[Ss]afeJointAngles.csv
 
 # Asset meta data should only be ignored when the corresponding asset is also ignored
 !/[Aa]ssets/**/*.meta

RobotTesting/.gitignore

@@ -12,6 +12,7 @@
 /[Uu]serSettings
 /[Ii]mages
 /[Mm]askedImages
+/SafeJointAngles.csv
 
 # Asset meta data should only be ignored when the corresponding asset is also ignored
 !/[Aa]ssets/**/*.meta

RobotTesting/Assets/Scenes/ToolTipCalibration.unity

@@ -907,7 +907,7 @@ Transform:
   - {fileID: 2047610784}
   - {fileID: 462687139}
   m_Father: {fileID: 0}
-  m_RootOrder: 5
+  m_RootOrder: 3
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!114 &229871065
 MonoBehaviour:
@@ -1433,6 +1433,37 @@ MeshCollider:
   m_Convex: 1
   m_CookingOptions: 30
   m_Mesh: {fileID: 4300000, guid: 5169d436d9ee6d4928090a1034714748, type: 2}
+--- !u!1 &437559996
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 437559997}
+  m_Layer: 0
+  m_Name: ToolTipObj
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &437559997
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 437559996}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: -0.00185, z: 0.0701}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 1929355745}
+  m_RootOrder: 4
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1 &439716051
 GameObject:
   m_ObjectHideFlags: 8
@@ -2190,7 +2221,7 @@ Transform:
   m_ConstrainProportionsScale: 0
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 3
+  m_RootOrder: 5
   m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0}
 --- !u!1 &711385899
 GameObject:
@@ -2826,7 +2857,7 @@ Transform:
   m_ConstrainProportionsScale: 0
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 0
+  m_RootOrder: 2
   m_LocalEulerAnglesHint: {x: 90, y: 0, z: 0}
 --- !u!114 &963194229
 MonoBehaviour:
@@ -2841,6 +2872,8 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   MaskOn: 0
+  ToolTipObj: {fileID: 437559996}
+  cameraTexture: {fileID: 8400000, guid: 75f6817628763ee5cb49337cf1ca2974, type: 2}
 --- !u!4 &974854168 stripped
 Transform:
   m_CorrespondingSourceObject: {fileID: 4057682339749017850, guid: 9ae46c6947eeeca24812cce13c8f0bbd, type: 3}
@@ -3736,7 +3769,7 @@ Transform:
   m_ConstrainProportionsScale: 0
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 2
+  m_RootOrder: 4
   m_LocalEulerAnglesHint: {x: 0, y: 100, z: 0}
 --- !u!4 &1176513281 stripped
 Transform:
@@ -5486,7 +5519,7 @@ Transform:
   m_ConstrainProportionsScale: 0
   m_Children: []
   m_Father: {fileID: 0}
-  m_RootOrder: 4
+  m_RootOrder: 0
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1001 &1755522146
 PrefabInstance:
@@ -5928,6 +5961,8 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   MaskOn: 1
+  ToolTipObj: {fileID: 437559996}
+  cameraTexture: {fileID: 8400000, guid: 75f6817628763ee5cb49337cf1ca2974, type: 2}
 --- !u!1 &1891940991
 GameObject:
   m_ObjectHideFlags: 0
@@ -6143,6 +6178,11 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: e9090ae501ff9cd459bec5271d40249c, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
+--- !u!4 &1929355745 stripped
+Transform:
+  m_CorrespondingSourceObject: {fileID: -8679921383154817045, guid: 16c6048d09bc6d24f8dba7446026dc7b, type: 3}
+  m_PrefabInstance: {fileID: 1684978203}
+  m_PrefabAsset: {fileID: 0}
 --- !u!1 &1936333754
 GameObject:
   m_ObjectHideFlags: 8

RobotTesting/Assets/Scripts/CameraController.cs

@@ -1,16 +1,42 @@
 using System.Collections;
 using System.Collections.Generic;
 using System.IO;
+using System;
 using UnityEngine;
 
 public class CameraController : MonoBehaviour{
 
   public bool MaskOn;
-
+  //Bool set in inspector to control if camera is masker or main
+
+  //public Shader ReplacShaderWhite;
+  //Shader with Ids used to replace robot and marker selectively
+  //No longer set in editor as shader is loaded with line 89
+
+  public GameObject ToolTipObj;
+  //Small sphere on end of tooltip that is used to track tooltip
+
+  public RenderTexture cameraTexture;
+  //REnder tecture for camera to set desired resolution 
+
+  private Vector4 _toolTipPos;
+  //value for theposition of the tooltip
+
+  private Matrix4x4 _camIntr;
+  //camera projection matrix (intrinsics)
+
+  private Vector3 _tooltipCoords ;
+  //answer matrix (pixel coords of image)
+
+  private Matrix4x4 cam_T_World ;
+  //camera extrinsics
+
+  private long _fileLength;
   private int _imageNumber;	
   private Texture2D _camView;
   private Camera _overHeadCam;
-  private string _filePath;
+  private string _imageFilePath ;
+
 
   void GetCamView() {
     RenderTexture currentRenText = RenderTexture.active;
@@ -27,21 +53,40 @@ void GetCamView() {
     byte[] bytes = _camView.EncodeToJPG();
 
     //If the tick box is selected in the inspector, then the path is changed accordingly
-
-    File.WriteAllBytes( _filePath + _imageNumber + ".jpg" , bytes );
-
+    File.WriteAllBytes( _imageFilePath + _imageNumber + ".jpg" , bytes );
+
+    if (!MaskOn) {
+      if (ToolTipVisible()) {
+        GetCoords();
+      } else {
+        System.IO.File.AppendAllText( @"coords.txt" , "null , null" + "\n" );
+      }
+    }
     _imageNumber++; 
   }
 
   private void Start(){
+    InitialiseFiles();
     _overHeadCam = GetComponent<Camera>();
-    Shader replacementShader = Resources.Load<Shader>("Shader/WhiteReplacementShader");
-    //  _overHeadCam.cullingMask = -1; //Sets the culling maks to everything
-
+
+    _camIntr[0,0] = ((cameraTexture.width * 0.5f )/( (float)Math.Tan(_overHeadCam.fieldOfView *0.5f 
+                      * (3.1415926/180))));
+    //Focal length in pixels calculation found from:
+    //https://answers.opencv.org/question/17076/conversion-focal-distance-from-mm-to-pixels/
+
+    _camIntr[0,2] = cameraTexture.width/2;
+    _camIntr[1,1] = ((cameraTexture.height * 0.5f )/( (float)Math.Tan(_overHeadCam.fieldOfView *0.5f
+                    * (3.1415926/180))));
+
+    _camIntr[1,2] = cameraTexture.height/2;
+    _camIntr[2,2] = 1.0f;
+    //Sets up the camera intrinsics matrix basedon the camera properties
+
     //If the tick box is selected in the inspector, the replacement shader is loaded for cam
     if (MaskOn) {
 
-      _filePath = "MaskedImages/MaskedImage";
+      _imageFilePath = "MaskedImages/MaskedImage";
+      Shader replacementShader = Resources.Load<Shader>("Shader/WhiteReplacementShader");
       _overHeadCam.SetReplacementShader( replacementShader , "RenderType");
       _overHeadCam.cullingMask = LayerMask.GetMask("MarkerTip") ;
 
@@ -50,7 +95,7 @@ private void Start(){
       //script or regular, just only once , ie not twice across the two scripts of the camera
 
     } else {
-      _filePath = "Images/Image";
+      _imageFilePath = "Images/Image";
     }
 
     _camView = new Texture2D(_overHeadCam.targetTexture.width ,_overHeadCam.targetTexture.height);
@@ -59,6 +104,46 @@ private void Start(){
     InvokeRepeating("GetCamView",1.5f,3.0f);   
   }
 
+  private void GetCoords(){
+    _toolTipPos = ToolTipObj.transform.position ;
+    //Reads the position of the TCP
+
+    _toolTipPos[3] = 1.0f;
+    //This is needed to set the 1 in the W component of the vector as it is reset every time the 
+    //previous line is executed
+
+    cam_T_World = _overHeadCam.worldToCameraMatrix ;
+    //Sets the matrix that transforms the world space to camera space
+
+    _tooltipCoords = _camIntr * cam_T_World * _toolTipPos;  
+    //Camera projection equation, further details can be found at:
+    //https://eikosim.com/en/non-classe-en/camera-calibration-principles-and-procedures/
+    //https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+
+
+    System.IO.File.AppendAllText( @"coords.txt" , _tooltipCoords[0]/ _tooltipCoords[2] + " , " 
+                                 + _tooltipCoords[1]/ _tooltipCoords[2] + "\n" );
+      //Writes the coordinates to a txt file
+  }
+
+  private bool ToolTipVisible() {
+    _fileLength = new System.IO.FileInfo("MaskedImages/MaskedImage" + _imageNumber + ".jpg").Length;
+    if ( _fileLength > 250628 ) {
+      return true;
+    }
+    else {
+      return false;
+    }
+    //TODO : Change file length check from being hardcoded to reading file size from a dummy image
+    //with no pixels (full black image )
+  }
+
+  private void InitialiseFiles(){
+    if (File.Exists(@"coords.txt")) {
+      File.Delete(@"coords.txt");
+    }
+    //Deletes the coordinates file if it already exists
+  }
   void InitilaiseDirectories(){ 
     if (!Directory.Exists("MaskedImages")) {
       Directory.CreateDirectory("MaskedImages");