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

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..

And a video of it working here..

https://www.youtube.com/watch?v=VxM1RMlYdAo

Answered By - Lee Englestone