3D motion support, change default behaviour

Major Release: - made the correct flip options the default (to revert: uncomment in Landmark.cs, and uncomment cv2.flip) - switch between the traditional anchored landmarks versus the full 3D movement using the appropriate Boolean in the unity inspector
2025-08-23 12:12:16 +08:00 · 2023-11-17 15:32:16 -07:00
parent 3548b6c0a8
commit 0f9b95334a
14 changed files with 384 additions and 191 deletions
--- a/UnityMediaPipeBody/Assets/CameraController.cs
+++ b/UnityMediaPipeBody/Assets/CameraController.cs
@ -0,0 +1,31 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class CameraController : MonoBehaviour
+{
+    [SerializeField]
+    private float distance = 10f;
+    public Vector3 offset;
+
+    Transform focus;
+    Vector3 originalDelta;
+
+    public void Calibrate(Transform focus)
+    {
+        this.focus = focus;
+        originalDelta = transform.position - focus.position;
+        originalDelta.x *= .01f;
+    }
+
+    private void LateUpdate()
+    {
+        if (focus == null) return;
+
+        Vector3 t = focus.position+offset*.5f + (originalDelta.normalized * (distance));
+        transform.position = Vector3.Lerp(transform.position,  t, Time.deltaTime*2.5f);
+
+        Quaternion r = Quaternion.LookRotation((focus.position+offset - transform.position).normalized, Vector3.up);
+        transform.rotation = Quaternion.Lerp(transform.rotation, r, Time.deltaTime * 1f);
+    }
+}
--- a/UnityMediaPipeBody/Assets/CameraController.cs.meta
+++ b/UnityMediaPipeBody/Assets/CameraController.cs.meta
@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: f433657deb089a8489125c68655afc6b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
--- a/UnityMediaPipeBody/Assets/Landmark.cs
+++ b/UnityMediaPipeBody/Assets/Landmark.cs
@ -1,4 +1,4 @@
-#define FLIP // Comment out this line to flip the landmarks (internally) [technically need to flip here, but kept like this for backward compatibility].
+// #define FLIP // Comment out this line to flip the landmarks (internally) [technically need to flip here, but kept like this for backward compatibility].
 // NOTE: image = cv2.flip(image, 1) in the Python side may also be of interest to you as well.

 #if FLIP
--- a/UnityMediaPipeBody/Assets/New
+++ b/UnityMediaPipeBody/Assets/New
@ -38,11 +38,11 @@ Material:
        m_Offset: {x: 0, y: 0}
    - _EmissionMap:
        m_Texture: {fileID: 0}
-        m_Scale: {x: 7.5, y: 10}
+        m_Scale: {x: 20, y: 20}
        m_Offset: {x: 0, y: 0}
    - _MainTex:
        m_Texture: {fileID: 2800000, guid: e78f8edec9c6e4340969a6fc5a0e69f4, type: 3}
-        m_Scale: {x: 7.5, y: 10}
+        m_Scale: {x: 20, y: 20}
        m_Offset: {x: 0, y: 0}
    - _MetallicGlossMap:
        m_Texture: {fileID: 0}
--- a/UnityMediaPipeBody/Assets/New
+++ b/UnityMediaPipeBody/Assets/New
@ -0,0 +1,80 @@
+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!21 &2100000
+Material:
+  serializedVersion: 8
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_Name: New Material 2
+  m_Shader: {fileID: 46, guid: 0000000000000000f000000000000000, type: 0}
+  m_ValidKeywords: []
+  m_InvalidKeywords: []
+  m_LightmapFlags: 4
+  m_EnableInstancingVariants: 0
+  m_DoubleSidedGI: 0
+  m_CustomRenderQueue: -1
+  stringTagMap: {}
+  disabledShaderPasses: []
+  m_SavedProperties:
+    serializedVersion: 3
+    m_TexEnvs:
+    - _BumpMap:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 1, y: 1}
+        m_Offset: {x: 0, y: 0}
+    - _DetailAlbedoMap:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 1, y: 1}
+        m_Offset: {x: 0, y: 0}
+    - _DetailMask:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 1, y: 1}
+        m_Offset: {x: 0, y: 0}
+    - _DetailNormalMap:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 1, y: 1}
+        m_Offset: {x: 0, y: 0}
+    - _EmissionMap:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 5, y: 5}
+        m_Offset: {x: 0, y: 0}
+    - _MainTex:
+        m_Texture: {fileID: 2800000, guid: e78f8edec9c6e4340969a6fc5a0e69f4, type: 3}
+        m_Scale: {x: 5, y: 5}
+        m_Offset: {x: 0, y: 0}
+    - _MetallicGlossMap:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 1, y: 1}
+        m_Offset: {x: 0, y: 0}
+    - _OcclusionMap:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 1, y: 1}
+        m_Offset: {x: 0, y: 0}
+    - _ParallaxMap:
+        m_Texture: {fileID: 0}
+        m_Scale: {x: 1, y: 1}
+        m_Offset: {x: 0, y: 0}
+    m_Ints: []
+    m_Floats:
+    - _BumpScale: 1
+    - _Cutoff: 0.5
+    - _DetailNormalMapScale: 1
+    - _DstBlend: 0
+    - _GlossMapScale: 1
+    - _Glossiness: 0
+    - _GlossyReflections: 1
+    - _Metallic: 0
+    - _Mode: 0
+    - _OcclusionStrength: 1
+    - _Parallax: 0.02
+    - _SmoothnessTextureChannel: 0
+    - _SpecularHighlights: 1
+    - _SrcBlend: 1
+    - _UVSec: 0
+    - _ZWrite: 1
+    m_Colors:
+    - _Color: {r: 0.48212975, g: 0.5943396, b: 0.27698466, a: 1}
+    - _EmissionColor: {r: 0, g: 0, b: 0, a: 1}
+  m_BuildTextureStacks: []
--- a/UnityMediaPipeBody/Assets/New
+++ b/UnityMediaPipeBody/Assets/New
@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 86c5af092b1860345ba939ff867b48bc
+NativeFormatImporter:
+  externalObjects: {}
+  mainObjectFileID: 2100000
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
--- a/UnityMediaPipeBody/Assets/New
+++ b/UnityMediaPipeBody/Assets/New
@ -94,7 +94,7 @@ Material:
    - _ZWrite: 1
    m_Colors:
    - _CameraFadeParams: {r: 0, g: Infinity, b: 0, a: 0}
-    - _Color: {r: 0.57924527, g: 1, b: 0.86824924, a: 1}
+    - _Color: {r: 0.7137255, g: 1.4980392, b: 1.4352942, a: 1}
    - _ColorAddSubDiff: {r: 0, g: 0, b: 0, a: 0}
    - _EmissionColor: {r: 0.0001953125, g: 0.0004442402, b: 0.00041360295, a: 1}
    - _SoftParticleFadeParams: {r: 0, g: 0, b: 0, a: 0}
--- a/UnityMediaPipeBody/Assets/PNG/Light/texture_08.png.meta
+++ b/UnityMediaPipeBody/Assets/PNG/Light/texture_08.png.meta
@ -34,7 +34,7 @@ TextureImporter:
  textureSettings:
    serializedVersion: 2
    filterMode: 1
-    aniso: 1
+    aniso: 8
    mipBias: 0
    wrapU: 0
    wrapV: 0
--- a/UnityMediaPipeBody/Assets/PipeServer.cs
+++ b/UnityMediaPipeBody/Assets/PipeServer.cs
@ -13,22 +13,32 @@ using UnityEngine;

 public class PipeServer : MonoBehaviour
 {
-    public Transform rParent;
-    public Transform lParent;
+    public Transform parent;
    public GameObject landmarkPrefab;
    public GameObject linePrefab;
    public GameObject headPrefab;
+    public bool anchoredBody = false;
    public bool enableHead = true;
    public float multiplier = 10f;
    public float landmarkScale = 1f;
    public float maxSpeed = 50f;
-    public float debug_samplespersecond;
+    public int samplesForPose = 1;

-    NamedPipeServerStream server;
+    private Body body;
+    private NamedPipeServerStream server;

    const int LANDMARK_COUNT = 33;
    const int LINES_COUNT = 11;

+    private Vector3 GetNormal(Vector3 p1, Vector3 p2, Vector3 p3)
+    {
+        Vector3 u = p2 - p1;
+        Vector3 v = p3 - p1;
+        Vector3 n = new Vector3((u.y * v.z - u.z * v.y), (u.z * v.x - u.x * v.z), (u.x * v.y - u.y * v.x));
+        float nl = Mathf.Sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);
+        return new Vector3(n[0] / nl, n[1] / nl, n[2] / nl);
+    }
+
    public struct AccumulatedBuffer
    {
        public Vector3 value;
@ -47,18 +57,29 @@ public class PipeServer : MonoBehaviour
        public Vector3[] localPositionTargets = new Vector3[LANDMARK_COUNT];
        public GameObject[] instances = new GameObject[LANDMARK_COUNT];
        public LineRenderer[] lines = new LineRenderer[LINES_COUNT];
+        public GameObject head;

        public bool active;

+        public bool setCalibration = false;
+        public Vector3 calibrationOffset;
+
+        public Vector3 virtualHeadPosition;
+
        public Body(Transform parent, GameObject landmarkPrefab, GameObject linePrefab,float s, GameObject headPrefab)
        {
            this.parent = parent;
            for (int i = 0; i < instances.Length; ++i)
            {
-                instances[i] = Instantiate(landmarkPrefab);// GameObject.CreatePrimitive(PrimitiveType.Sphere);
+                instances[i] = Instantiate(landmarkPrefab);
                instances[i].transform.localScale = Vector3.one * s;
                instances[i].transform.parent = parent;
                instances[i].name = ((Landmark)i).ToString();
+
+                if (headPrefab && i >= 0 && i <= 10)
+                {
+                    instances[i].transform.localScale = Vector3.one * 0f;
+                }
            }
            for (int i = 0; i < lines.Length; ++i)
            {
@ -67,8 +88,7 @@ public class PipeServer : MonoBehaviour

            if (headPrefab)
            {
-                GameObject head = Instantiate(headPrefab);
-                head.transform.parent = instances[(int)Landmark.NOSE].transform;
+                head = Instantiate(headPrefab);
                head.transform.localPosition = headPrefab.transform.position;
                head.transform.localRotation = headPrefab.transform.localRotation;
                head.transform.localScale = headPrefab.transform.localScale;
@ -125,17 +145,25 @@ public class PipeServer : MonoBehaviour
            lines[8].SetPosition(2, Position((Landmark)19));
            lines[8].SetPosition(3, Position((Landmark)15));

-            lines[9].positionCount = 2;
-            lines[9].SetPosition(0, Position((Landmark)10));
-            lines[9].SetPosition(1, Position((Landmark)9));
+            if (!head)
+            {
+                lines[9].positionCount = 2;
+                lines[9].SetPosition(0, Position((Landmark)10));
+                lines[9].SetPosition(1, Position((Landmark)9));

-
-            lines[10].positionCount = 5;
-            lines[10].SetPosition(0, Position((Landmark)8));
-            lines[10].SetPosition(1, Position((Landmark)5));
-            lines[10].SetPosition(2, Position((Landmark)0));
-            lines[10].SetPosition(3, Position((Landmark)2));
-            lines[10].SetPosition(4, Position((Landmark)7));
+                lines[10].positionCount = 5;
+                lines[10].SetPosition(0, Position((Landmark)8));
+                lines[10].SetPosition(1, Position((Landmark)5));
+                lines[10].SetPosition(2, Position((Landmark)0));
+                lines[10].SetPosition(3, Position((Landmark)2));
+                lines[10].SetPosition(4, Position((Landmark)7));
+            }
+        }
+        public void Calibrate()
+        {
+            Vector3 centre = (localPositionTargets[(int)Landmark.LEFT_HIP] + localPositionTargets[(int)Landmark.RIGHT_HIP]) / 2f;
+            calibrationOffset = -centre;
+            setCalibration = true;
        }

        public float GetAngle(Landmark referenceFrom, Landmark referenceTo, Landmark from, Landmark to)
@ -159,17 +187,11 @@ public class PipeServer : MonoBehaviour

    }

-    private Body body;
-
-    public int samplesForPose = 1;
-
-    public bool active;
-
    private void Start()
    {
        System.Globalization.CultureInfo.DefaultThreadCurrentCulture = System.Globalization.CultureInfo.InvariantCulture;

-        body = new Body(lParent,landmarkPrefab,linePrefab,landmarkScale,enableHead?headPrefab:null);
+        body = new Body(parent,landmarkPrefab,linePrefab,landmarkScale,enableHead?headPrefab:null);

        Thread t = new Thread(new ThreadStart(Run));
        t.Start();
@ -179,26 +201,46 @@ public class PipeServer : MonoBehaviour
    {
        UpdateBody(body);
    }
-
    private void UpdateBody(Body b)
    {
+        if (b.active == false) return;
+
        for (int i = 0; i < LANDMARK_COUNT; ++i)
        {
            if (b.positionsBuffer[i].accumulatedValuesCount < samplesForPose)
                continue;
-            // b.instances[i].transform.localPosition = b.positionsBuffer[i] / (float)b.samplesCounter * multiplier;
            b.localPositionTargets[i] = b.positionsBuffer[i].value / (float)b.positionsBuffer[i].accumulatedValuesCount * multiplier;
            b.positionsBuffer[i] = new AccumulatedBuffer(Vector3.zero,0);
        }

+        if (!b.setCalibration)
+        {
+            print("Set Calibration Data");
+            b.Calibrate();
+
+            if(FindObjectOfType<CameraController>())
+                FindObjectOfType<CameraController>().Calibrate(b.instances[(int)Landmark.NOSE].transform);
+        }
+
        for (int i = 0; i < LANDMARK_COUNT; ++i)
        {
-            b.instances[i].transform.localPosition=Vector3.MoveTowards(b.instances[i].transform.localPosition, b.localPositionTargets[i], Time.deltaTime * maxSpeed);
+            b.instances[i].transform.localPosition=Vector3.MoveTowards(b.instances[i].transform.localPosition, b.localPositionTargets[i]+b.calibrationOffset, Time.deltaTime * maxSpeed);
        }
        b.UpdateLines();
+
+        b.virtualHeadPosition = (b.Position(Landmark.RIGHT_EAR) + b.Position(Landmark.LEFT_EAR)) / 2f;
+
+        if (b.head)
+        {
+            // Experimental method and getting the head pose.
+            b.head.transform.position = b.virtualHeadPosition+Vector3.up* .5f;
+            Vector3 n1 = Vector3.Scale(new Vector3(.1f, 1f, .1f), GetNormal(b.Position((Landmark)0), b.Position((Landmark)8), b.Position((Landmark)7))).normalized;
+            Vector3 n2 = Vector3.Scale(new Vector3(1f, .1f, 1f), GetNormal(b.Position((Landmark)0), b.Position((Landmark)4), b.Position((Landmark)1))).normalized;
+            b.head.transform.rotation = Quaternion.LookRotation(-n2, n1);
+        }
    }

-    void Run()
+    private void Run()
    {
        System.Globalization.CultureInfo.CurrentCulture = System.Globalization.CultureInfo.InvariantCulture;

@ -218,17 +260,22 @@ public class PipeServer : MonoBehaviour
                Body h = body;
                var len = (int)br.ReadUInt32();
                var str = new string(br.ReadChars(len));
-
                string[] lines = str.Split('\n');
+
                foreach (string l in lines)
                {
                    if (string.IsNullOrWhiteSpace(l))
                        continue;
+
                    string[] s = l.Split('|');
-                    if (s.Length < 4) continue;
+                    if (s.Length < 5) continue;
+
+                    if (anchoredBody && s[0] != "ANCHORED") continue;
+                    if (!anchoredBody && s[0] != "FREE") continue;
+
                    int i;
-                    if (!int.TryParse(s[0], out i)) continue;
-                    h.positionsBuffer[i].value += new Vector3(float.Parse(s[1]), float.Parse(s[2]), float.Parse(s[3]));
+                    if (!int.TryParse(s[1], out i)) continue;
+                    h.positionsBuffer[i].value += new Vector3(float.Parse(s[2]), float.Parse(s[3]), float.Parse(s[4]));
                    h.positionsBuffer[i].accumulatedValuesCount += 1;
                    h.active = true;
                }
--- a/UnityMediaPipeBody/Assets/Scenes/SampleScene.unity
+++ b/UnityMediaPipeBody/Assets/Scenes/SampleScene.unity
@ -196,103 +196,6 @@ CanvasRenderer:
  m_PrefabAsset: {fileID: 0}
  m_GameObject: {fileID: 15310015}
  m_CullTransparentMesh: 1
--- !u!1 &118976246
-GameObject:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  serializedVersion: 6
-  m_Component:
-  - component: {fileID: 118976250}
-  - component: {fileID: 118976249}
-  - component: {fileID: 118976248}
-  - component: {fileID: 118976247}
-  m_Layer: 0
-  m_Name: Cube
-  m_TagString: Untagged
-  m_Icon: {fileID: 0}
-  m_NavMeshLayer: 0
-  m_StaticEditorFlags: 2147483647
-  m_IsActive: 0
--- !u!65 &118976247
-BoxCollider:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 118976246}
-  m_Material: {fileID: 0}
-  m_IsTrigger: 0
-  m_Enabled: 0
-  serializedVersion: 2
-  m_Size: {x: 1, y: 1, z: 1}
-  m_Center: {x: 0, y: 0, z: 0}
--- !u!23 &118976248
-MeshRenderer:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 118976246}
-  m_Enabled: 1
-  m_CastShadows: 1
-  m_ReceiveShadows: 0
-  m_DynamicOccludee: 1
-  m_StaticShadowCaster: 0
-  m_MotionVectors: 1
-  m_LightProbeUsage: 1
-  m_ReflectionProbeUsage: 1
-  m_RayTracingMode: 2
-  m_RayTraceProcedural: 0
-  m_RenderingLayerMask: 1
-  m_RendererPriority: 0
-  m_Materials:
-  - {fileID: 2100000, guid: 7da63296c7fc0dc4886345e842ce96f0, type: 2}
-  m_StaticBatchInfo:
-    firstSubMesh: 0
-    subMeshCount: 0
-  m_StaticBatchRoot: {fileID: 0}
-  m_ProbeAnchor: {fileID: 0}
-  m_LightProbeVolumeOverride: {fileID: 0}
-  m_ScaleInLightmap: 1
-  m_ReceiveGI: 1
-  m_PreserveUVs: 0
-  m_IgnoreNormalsForChartDetection: 0
-  m_ImportantGI: 0
-  m_StitchLightmapSeams: 1
-  m_SelectedEditorRenderState: 3
-  m_MinimumChartSize: 4
-  m_AutoUVMaxDistance: 0.5
-  m_AutoUVMaxAngle: 89
-  m_LightmapParameters: {fileID: 0}
-  m_SortingLayerID: 0
-  m_SortingLayer: 0
-  m_SortingOrder: 0
-  m_AdditionalVertexStreams: {fileID: 0}
--- !u!33 &118976249
-MeshFilter:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 118976246}
-  m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
--- !u!4 &118976250
-Transform:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 118976246}
-  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
-  m_LocalPosition: {x: 0, y: 0, z: 5}
-  m_LocalScale: {x: 15, y: 20, z: 1}
-  m_ConstrainProportionsScale: 0
-  m_Children: []
-  m_Father: {fileID: 0}
-  m_RootOrder: 3
-  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1 &499442781
 GameObject:
  m_ObjectHideFlags: 0
@ -322,18 +225,16 @@ MonoBehaviour:
  m_Script: {fileID: 11500000, guid: 44d24d3dd190ce1489db2f91b5ab8db9, type: 3}
  m_Name: 
  m_EditorClassIdentifier: 
-  rParent: {fileID: 1661589169}
-  lParent: {fileID: 730897861}
+  parent: {fileID: 1661589169}
  landmarkPrefab: {fileID: 4198472772038802745, guid: 2d7c9010dd98a2c4ea18ff18d4704668, type: 3}
  linePrefab: {fileID: 6873139827369952066, guid: a26721b39f9a2544f8af1b49c82107d5, type: 3}
  headPrefab: {fileID: 6070663460479496251, guid: dd948a2582b6d7745bc83791cb1fa0bd, type: 3}
+  anchoredBody: 0
  enableHead: 1
  multiplier: 10
  landmarkScale: 0.5
  maxSpeed: 50
-  debug_samplespersecond: 0
  samplesForPose: 1
-  active: 0
 --- !u!4 &499442783
 Transform:
  m_ObjectHideFlags: 0
@ -341,16 +242,15 @@ Transform:
  m_PrefabInstance: {fileID: 0}
  m_PrefabAsset: {fileID: 0}
  m_GameObject: {fileID: 499442781}
-  m_LocalRotation: {x: 0, y: 0, z: 1, w: 0}
-  m_LocalPosition: {x: 0, y: -0.7, z: 0}
+  m_LocalRotation: {x: 0, y: 1, z: 0, w: 0}
+  m_LocalPosition: {x: 0, y: 0.6, z: 0}
  m_LocalScale: {x: 1, y: 1, z: 1}
  m_ConstrainProportionsScale: 0
  m_Children:
  - {fileID: 1661589169}
-  - {fileID: 730897861}
  m_Father: {fileID: 0}
  m_RootOrder: 2
-  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 180}
+  m_LocalEulerAnglesHint: {x: 0, y: 180, z: 0}
 --- !u!1 &705507993
 GameObject:
  m_ObjectHideFlags: 0
@ -445,37 +345,6 @@ Transform:
  m_Father: {fileID: 0}
  m_RootOrder: 1
  m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0}
--- !u!1 &730897860
-GameObject:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  serializedVersion: 6
-  m_Component:
-  - component: {fileID: 730897861}
-  m_Layer: 0
-  m_Name: L
-  m_TagString: Untagged
-  m_Icon: {fileID: 0}
-  m_NavMeshLayer: 0
-  m_StaticEditorFlags: 0
-  m_IsActive: 1
--- !u!4 &730897861
-Transform:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 730897860}
-  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
-  m_LocalPosition: {x: -1, y: 0, z: 0}
-  m_LocalScale: {x: 1, y: 1, z: 1}
-  m_ConstrainProportionsScale: 0
-  m_Children: []
-  m_Father: {fileID: 499442783}
-  m_RootOrder: 1
-  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1 &963194225
 GameObject:
  m_ObjectHideFlags: 0
@ -487,6 +356,7 @@ GameObject:
  - component: {fileID: 963194228}
  - component: {fileID: 963194227}
  - component: {fileID: 963194226}
+  - component: {fileID: 963194229}
  m_Layer: 0
  m_Name: Main Camera
  m_TagString: MainCamera
@ -512,7 +382,7 @@ Camera:
  m_Enabled: 1
  serializedVersion: 2
  m_ClearFlags: 2
-  m_BackGroundColor: {r: 0.23047346, g: 0.3587611, b: 0.3679245, a: 0}
+  m_BackGroundColor: {r: 0.24827339, g: 0.40201032, b: 0.41509432, a: 0}
  m_projectionMatrixMode: 1
  m_GateFitMode: 2
  m_FOVAxisMode: 0
@ -560,6 +430,20 @@ Transform:
  m_Father: {fileID: 0}
  m_RootOrder: 0
  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &963194229
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 963194225}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: f433657deb089a8489125c68655afc6b, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  distance: 44.76
+  offset: {x: 0, y: -5.5, z: 0}
 --- !u!1 &1081063075
 GameObject:
  m_ObjectHideFlags: 0
@ -653,14 +537,88 @@ RectTransform:
  m_ConstrainProportionsScale: 0
  m_Children:
  - {fileID: 15310016}
+  - {fileID: 1580706275}
  m_Father: {fileID: 0}
-  m_RootOrder: 4
+  m_RootOrder: 3
  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
  m_AnchorMin: {x: 0, y: 0}
  m_AnchorMax: {x: 0, y: 0}
  m_AnchoredPosition: {x: 0, y: 0}
  m_SizeDelta: {x: 0, y: 0}
  m_Pivot: {x: 0, y: 0}
+--- !u!1 &1580706274
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 1580706275}
+  - component: {fileID: 1580706277}
+  - component: {fileID: 1580706276}
+  m_Layer: 5
+  m_Name: RawImage (1)
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!224 &1580706275
+RectTransform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1580706274}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 1081063079}
+  m_RootOrder: 1
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+  m_AnchorMin: {x: 0, y: 0}
+  m_AnchorMax: {x: 1, y: 1}
+  m_AnchoredPosition: {x: 0, y: 0}
+  m_SizeDelta: {x: 0, y: 0.000015258789}
+  m_Pivot: {x: 0.5, y: 0.5}
+--- !u!114 &1580706276
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1580706274}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 1344c3c82d62a2a41a3576d8abb8e3ea, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  m_Material: {fileID: 0}
+  m_Color: {r: 1, g: 1, b: 1, a: 0.5686275}
+  m_RaycastTarget: 1
+  m_RaycastPadding: {x: 0, y: 0, z: 0, w: 0}
+  m_Maskable: 1
+  m_OnCullStateChanged:
+    m_PersistentCalls:
+      m_Calls: []
+  m_Texture: {fileID: 2800000, guid: 2b524b2e71371994c8f4b0ef66213905, type: 3}
+  m_UVRect:
+    serializedVersion: 2
+    x: 0
+    y: 0
+    width: 1
+    height: 1
+--- !u!222 &1580706277
+CanvasRenderer:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 1580706274}
+  m_CullTransparentMesh: 1
 --- !u!1001 &1590897428
 PrefabInstance:
  m_ObjectHideFlags: 0
@ -674,7 +632,15 @@ PrefabInstance:
      objectReference: {fileID: 0}
    - target: {fileID: 2552428416732922575, guid: fd089d0f3683f1441a3c6b9f7e955f4a, type: 3}
      propertyPath: m_RootOrder
-      value: 5
+      value: 4
+      objectReference: {fileID: 0}
+    - target: {fileID: 2552428416732922575, guid: fd089d0f3683f1441a3c6b9f7e955f4a, type: 3}
+      propertyPath: m_LocalScale.x
+      value: 100
+      objectReference: {fileID: 0}
+    - target: {fileID: 2552428416732922575, guid: fd089d0f3683f1441a3c6b9f7e955f4a, type: 3}
+      propertyPath: m_LocalScale.z
+      value: 100
      objectReference: {fileID: 0}
    - target: {fileID: 2552428416732922575, guid: fd089d0f3683f1441a3c6b9f7e955f4a, type: 3}
      propertyPath: m_LocalPosition.x
@ -728,7 +694,7 @@ GameObject:
  m_Component:
  - component: {fileID: 1661589169}
  m_Layer: 0
-  m_Name: r
+  m_Name: parent
  m_TagString: Untagged
  m_Icon: {fileID: 0}
  m_NavMeshLayer: 0
--- a/UnityMediaPipeBody/Assets/head.prefab
+++ b/UnityMediaPipeBody/Assets/head.prefab
@ -124,9 +124,9 @@ Transform:
  m_PrefabAsset: {fileID: 0}
  m_GameObject: {fileID: 6070663460479496251}
  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
-  m_LocalPosition: {x: -1.59, y: -0.25, z: 0}
-  m_LocalScale: {x: 4.5, y: 5, z: 4.5}
-  m_ConstrainProportionsScale: 0
+  m_LocalPosition: {x: -2, y: -0.25, z: 0}
+  m_LocalScale: {x: 2.4, y: 2.666667, z: 2.4}
+  m_ConstrainProportionsScale: 1
  m_Children:
  - {fileID: 6091697369038753900}
  - {fileID: 8302660708804709166}
--- a/UnityMediaPipeBody/Assets/landmark.mat
+++ b/UnityMediaPipeBody/Assets/landmark.mat
@ -9,7 +9,8 @@ Material:
  m_PrefabAsset: {fileID: 0}
  m_Name: landmark
  m_Shader: {fileID: 46, guid: 0000000000000000f000000000000000, type: 0}
-  m_ValidKeywords: []
+  m_ValidKeywords:
+  - _SMOOTHNESS_TEXTURE_ALBEDO_CHANNEL_A
  m_InvalidKeywords: []
  m_LightmapFlags: 4
  m_EnableInstancingVariants: 0
@ -62,14 +63,14 @@ Material:
    - _Cutoff: 0.5
    - _DetailNormalMapScale: 1
    - _DstBlend: 0
-    - _GlossMapScale: 1
-    - _Glossiness: 0.239
+    - _GlossMapScale: 0.436
+    - _Glossiness: 0.596
    - _GlossyReflections: 1
-    - _Metallic: 0.537
+    - _Metallic: 0.76
    - _Mode: 0
    - _OcclusionStrength: 1
    - _Parallax: 0.02
-    - _SmoothnessTextureChannel: 0
+    - _SmoothnessTextureChannel: 1
    - _SpecularHighlights: 1
    - _SrcBlend: 1
    - _UVSec: 0
--- a/mediapipebody/body.py
+++ b/mediapipebody/body.py
@ -2,6 +2,7 @@
 import mediapipe as mp
 from mediapipe.tasks import python
 from mediapipe.tasks.python import vision
+import numpy as np

 import cv2
 import threading
@ -18,7 +19,7 @@ class CaptureThread(threading.Thread):
    counter = 0
    timer = 0.0
    def run(self):
-        self.cap = cv2.VideoCapture(0) # sometimes it can take a while for certain video captures
+        self.cap = cv2.VideoCapture(global_vars.WEBCAM_INDEX) # sometimes it can take a while for certain video captures 4
        if global_vars.USE_CUSTOM_CAM_SETTINGS:
            self.cap.set(cv2.CAP_PROP_FPS, global_vars.FPS)
            self.cap.set(cv2.CAP_PROP_FRAME_WIDTH,global_vars.WIDTH)
@ -46,6 +47,39 @@ class BodyThread(threading.Thread):
    timeSinceCheckedConnection = 0
    timeSincePostStatistics = 0

+    def compute_real_world_landmarks(self,world_landmarks,image_landmarks,image_shape):
+        try:
+            # pseudo camera internals
+            # if you properly calibrated your camera tracking quality can improve...
+            frame_height,frame_width, channels = image_shape
+            focal_length = frame_width*.6
+            center = (frame_width/2, frame_height/2)
+            camera_matrix = np.array(
+                                    [[focal_length, 0, center[0]],
+                                    [0, focal_length, center[1]],
+                                    [0, 0, 1]], dtype = "double"
+                                    )
+            distortion = np.zeros((4, 1))
+
+            success, rotation_vector, translation_vector = cv2.solvePnP(objectPoints= world_landmarks, 
+                                                                        imagePoints= image_landmarks, 
+                                                                        cameraMatrix= camera_matrix, 
+                                                                        distCoeffs= distortion,
+                                                                        flags=cv2.SOLVEPNP_SQPNP)
+            transformation = np.eye(4)
+            transformation[0:3, 3] = translation_vector.squeeze()
+
+            # transform model coordinates into homogeneous coordinates
+            model_points_hom = np.concatenate((world_landmarks, np.ones((33, 1))), axis=1)
+
+            # apply the transformation
+            world_points = model_points_hom.dot(np.linalg.inv(transformation).T)
+
+            return world_points
+        except AttributeError:
+            print("Attribute Error: shouldn't happen frequently")
+            return world_landmarks 
+
    def run(self):
        mp_drawing = mp.solutions.drawing_utils
        mp_pose = mp.solutions.pose
@ -68,7 +102,7 @@ class BodyThread(threading.Thread):
                image = capture.frame
                                
                # Image transformations and stuff
-                image = cv2.flip(image, 1)
+                #image = cv2.flip(image, 1)
                image.flags.writeable = global_vars.DEBUG
                
                # Detections
@ -87,7 +121,7 @@ class BodyThread(threading.Thread):
                                                mp_drawing.DrawingSpec(color=(255, 255, 255), thickness=2, circle_radius=2),
                                                )
                    cv2.imshow('Body Tracking', image)
-                    cv2.waitKey(3)
+                    cv2.waitKey(1)

                if self.pipe==None and time.time()-self.timeSinceCheckedConnection>=1:
                    try:
@ -101,10 +135,22 @@ class BodyThread(threading.Thread):
                    # Set up data for piping
                    self.data = ""
                    i = 0
+                    
                    if results.pose_world_landmarks:
-                        hand_world_landmarks = results.pose_world_landmarks
+                        image_landmarks = results.pose_landmarks
+                        world_landmarks = results.pose_world_landmarks
+
+                        model_points = np.float32([[-l.x, -l.y, -l.z] for l in world_landmarks.landmark])
+                        image_points = np.float32([[l.x * image.shape[1], l.y * image.shape[0]] for l in image_landmarks.landmark])
+                        
+                        body_world_landmarks_world = self.compute_real_world_landmarks(model_points,image_points,image.shape)
+                        body_world_landmarks = results.pose_world_landmarks
+                        
                        for i in range(0,33):
-                            self.data += "{}|{}|{}|{}\n".format(i,hand_world_landmarks.landmark[i].x,hand_world_landmarks.landmark[i].y,hand_world_landmarks.landmark[i].z)
+                            self.data += "FREE|{}|{}|{}|{}\n".format(i,body_world_landmarks_world[i][0],body_world_landmarks_world[i][1],body_world_landmarks_world[i][2])
+                        for i in range(0,33):
+                            self.data += "ANCHORED|{}|{}|{}|{}\n".format(i,-body_world_landmarks.landmark[i].x,-body_world_landmarks.landmark[i].y,-body_world_landmarks.landmark[i].z)
+
                    
                    s = self.data.encode('utf-8') 
                    try:     
--- a/mediapipebody/global_vars.py
+++ b/mediapipebody/global_vars.py
@ -4,11 +4,14 @@ KILL_THREADS = False
 # Toggle this in order to view how your WebCam is being interpreted (reduces performance).
 DEBUG = True 

+# To switch cameras. Sometimes takes a while.
+WEBCAM_INDEX = 0
+
 # Settings do not universally apply, not all WebCams support all frame rates and resolutions
 USE_CUSTOM_CAM_SETTINGS = False
 FPS = 60
 WIDTH = 320
 HEIGHT = 240

-# [0, 2] Higher numbers are more precise, but also cost more performance. The demo video used 1.
+# [0, 2] Higher numbers are more precise, but also cost more performance. Good environment conditions = 1, otherwise 2.
 MODEL_COMPLEXITY = 1