ARKit Body Tracking using Xamarin and C# Inaccurate

Issue

I am trying to translate some Swift examples (such as this one https://github.com/iamfine/ARSkeleton) to C# that show how to use ARKit Body Tracking.

But I don’t quite seem able to position the joint nodes correctly over the corresponding joints. They follow my body movements, but the position of the nodes seem to be incorrect.

Can anyone familiar with ARKit Body Tracking see what I am doing wrong?

Thanks

enter image description here

using ARKit;
using Foundation;
using OpenTK;
using SceneKit;
using System;
using System.Collections.Generic;
using UIKit;

namespace XamarinArkitSample
{
    public partial class BodyDetectionViewController : UIViewController
    {
        private readonly ARSCNView sceneView;

        public BodyDetectionViewController()
        {
            this.sceneView  new ARSCNView
            {
                AutoenablesDefaultLighting  true,
                Delegate  new SceneViewDelegate()
            };

            this.View.AddSubview(this.sceneView);
        }

        public override void ViewDidLoad()
        {
            base.ViewDidLoad();

            this.sceneView.Frame  this.View.Frame;
        }

        public override void ViewDidAppear(bool animated)
        {
            base.ViewDidAppear(animated);

            var bodyTrackingConfiguration  new ARBodyTrackingConfiguration()
            {
                WorldAlignment  ARWorldAlignment.Gravity
            };

            this.sceneView.Session.Run(bodyTrackingConfiguration,
                ARSessionRunOptions.ResetTracking | ARSessionRunOptions.RemoveExistingAnchors);
        }

        public override void ViewDidDisappear(bool animated)
        {
            base.ViewDidDisappear(animated);
            this.sceneView.Session.Pause();
        }

        public override void DidReceiveMemoryWarning()
        {
            base.DidReceiveMemoryWarning();
        }

        public class SceneViewDelegate : ARSCNViewDelegate
        {
            Dictionary<string, JointNode> joints  new Dictionary<string, JointNode>();
            float jointRadius  0.02f;
            UIColor jointColour  UIColor.Green;

            public override void DidAddNode(ISCNSceneRenderer renderer, SCNNode node, ARAnchor anchor)
            {
                if (anchor is ARBodyAnchor bodyAnchor)
                {
                    foreach (var jointName in ARSkeletonDefinition.DefaultBody3DSkeletonDefinition.JointNames)
                    {
                        var jointNode  MakeJoint(jointRadius, jointColour);

                        try
                        {
                            var jointPosition  GetJointPosition(bodyAnchor, jointName);
                            jointNode.Position  jointPosition;

                            //System.Diagnostics.Debug.WriteLine($"Adding {jointName} node to position {jointPosition.X},{jointPosition.Y},{jointPosition.Z}");

                            if (!joints.ContainsKey(jointName))
                            {
                                node.AddChildNode(jointNode);
                                joints.Add(jointName, jointNode);
                            }
                        }
                        catch (Exception ex)
                        {
                            System.Diagnostics.Debug.WriteLine($"{ex.Message}");
                        }
                    }
                }
            }

            public override void DidUpdateNode(ISCNSceneRenderer renderer, SCNNode node, ARAnchor anchor)
            {
                if (anchor is ARBodyAnchor bodyAnchor)
                {
                    foreach (var jointName in ARSkeletonDefinition.DefaultBody3DSkeletonDefinition.JointNames)
                    {
                        try
                        {
                            var jointPosition  GetJointPosition(bodyAnchor, jointName);

                            if (joints.ContainsKey(jointName))
                            {
                                joints[jointName].Update(jointPosition);
                                //System.Diagnostics.Debug.WriteLine($"Updating {jointName} node to position {jointPosition.X},{jointPosition.Y},{jointPosition.Z}");
                            }
                        }
                        catch (Exception ex)
                        {
                            System.Diagnostics.Debug.WriteLine($"{ex.Message}");

                        }
                    }
                }
            }

            private SCNVector3 GetJointPosition(ARBodyAnchor bodyAnchor, string jointName)
            {
                // https://github.com/iamfine/ARSkeleton

                NMatrix4 jointTransform  bodyAnchor.Skeleton.GetModelTransform((NSString)jointName);

                // Approach 1
                SCNMatrix4 matrix  jointTransform.ToSCNMatrix4();
                SCNMatrix4 bodyAnchorTransform  bodyAnchor.Transform.ToSCNMatrix4();
                SCNMatrix4.Mult(ref matrix, ref bodyAnchorTransform, out matrix);
                return new SCNVector3(matrix.M41, matrix.M42, matrix.M43);

                // Approach 2
                /*
                var result  bodyAnchor.Transform.Column3 + jointTransform.Column3;
                return new SCNVector3(result);
                */
            }

            private JointNode MakeJoint(float jointRadius, UIColor jointColour)
            {
                var jointNode  new JointNode();

                var material  new SCNMaterial();
                material.Diffuse.Contents  jointColour;

                var jointGeometry  SCNSphere.Create(jointRadius);
                jointGeometry.FirstMaterial  material;
                jointNode.Geometry  jointGeometry;

                return jointNode;
            }
        }

        public class JointNode : SCNNode
        {
            public void Update(SCNVector3 position)
            {
                this.Position  position;
            }
        }
    }


    public static class Extensions
    {
        public static SCNMatrix4 ToSCNMatrix4(this NMatrix4 self)
        {
            var newMatrix  new SCNMatrix4(
                self.M11, self.M21, self.M31, self.M41,
                self.M12, self.M22, self.M32, self.M42,
                self.M13, self.M23, self.M33, self.M43,
                self.M14, self.M24, self.M34, self.M44
            );

            return newMatrix;
        }
    }

}

Solution

So I got it working. It seems I was overcomplicating how to determine the X,Y,Z position of each joint node.. all I needed to do was this..

private SCNVector3 GetJointPosition(ARBodyAnchor bodyAnchor, string jointName)
{
   NMatrix4 jointTransform  bodyAnchor.Skeleton.GetModelTransform((NSString)jointName);
   return new SCNVector3(jointTransform.Column3);
}

Here is the full listing..

using ARKit;
using Foundation;
using OpenTK;
using SceneKit;
using System;
using System.Collections.Generic;
using UIKit;

namespace XamarinArkitSample
{
    public partial class BodyDetectionViewController : UIViewController
    {
        private readonly ARSCNView sceneView;

        public BodyDetectionViewController()
        {
            this.sceneView  new ARSCNView
            {
                AutoenablesDefaultLighting  true,
                Delegate  new SceneViewDelegate()
            };

            this.View.AddSubview(this.sceneView);
        }

        public override void ViewDidLoad()
        {
            base.ViewDidLoad();

            this.sceneView.Frame  this.View.Frame;
        }

        public override void ViewDidAppear(bool animated)
        {
            base.ViewDidAppear(animated);

            var bodyTrackingConfiguration  new ARBodyTrackingConfiguration()
            {
                WorldAlignment  ARWorldAlignment.Gravity
            };

            this.sceneView.Session.Run(bodyTrackingConfiguration);
        }

        public override void ViewDidDisappear(bool animated)
        {
            base.ViewDidDisappear(animated);
            this.sceneView.Session.Pause();
        }

        public override void DidReceiveMemoryWarning()
        {
            base.DidReceiveMemoryWarning();
        }

        public class SceneViewDelegate : ARSCNViewDelegate
        { 
            Dictionary<string, JointNode> joints  new Dictionary<string, JointNode>();
            float jointRadius  0.04f;
            UIColor jointColour  UIColor.Yellow;

            public override void DidAddNode(ISCNSceneRenderer renderer, SCNNode node, ARAnchor anchor)
            {
                if (!(anchor is ARBodyAnchor bodyAnchor))
                    return;

                foreach (var jointName in ARSkeletonDefinition.DefaultBody3DSkeletonDefinition.JointNames)
                {
                    JointNode jointNode  MakeJoint(jointRadius, jointColour);

                    var jointPosition  GetJointPosition(bodyAnchor, jointName);
                    jointNode.Position  jointPosition;

                    if (!joints.ContainsKey(jointName))
                    {
                        node.AddChildNode(jointNode);
                        joints.Add(jointName, jointNode);
                    }
                }
            }

            public override void DidUpdateNode(ISCNSceneRenderer renderer, SCNNode node, ARAnchor anchor)
            {
                if (!(anchor is ARBodyAnchor bodyAnchor))
                    return;

                foreach (var jointName in ARSkeletonDefinition.DefaultBody3DSkeletonDefinition.JointNames)
                {       
                    var jointPosition  GetJointPosition(bodyAnchor, jointName);

                    if (joints.ContainsKey(jointName))
                    {
                        joints[jointName].Update(jointPosition);
                    }   
                }
            }

            private SCNVector3 GetJointPosition(ARBodyAnchor bodyAnchor, string jointName)
            {
                NMatrix4 jointTransform  bodyAnchor.Skeleton.GetModelTransform((NSString)jointName);
                return new SCNVector3(jointTransform.Column3);
            }

            private JointNode MakeJoint(float jointRadius, UIColor jointColour)
            {
                var jointNode  new JointNode();

                var material  new SCNMaterial();
                material.Diffuse.Contents  jointColour;

                var jointGeometry  SCNSphere.Create(jointRadius);
                jointGeometry.FirstMaterial  material;
                jointNode.Geometry  jointGeometry;

                return jointNode;
            }
        }

        public class JointNode : SCNNode
        {
            public void Update(SCNVector3 position)
            {
                this.Position  position;
            }
        }
    }


    public static class Extensions
    {
        public static SCNMatrix4 ToSCNMatrix4(this NMatrix4 self)
        {
            var row0  new SCNVector4(self.M11, self.M12, self.M13, self.M14);
            var row1  new SCNVector4(self.M21, self.M22, self.M23, self.M24);
            var row2  new SCNVector4(self.M31, self.M32, self.M33, self.M34);
            var row3  new SCNVector4(self.M41, self.M42, self.M43, self.M44);
            return new SCNMatrix4(row0, row1, row2, row3);
        }
    }
}

Which with a bit of tweaking looks like this..

enter image description here

And a video of it working here..

https://www.youtube.com/watch?vVxM1RMlYdAo

Answered By – Lee Englestone

Leave a Comment