Continuing the posts that I’ve written recently on the RealSense camera, I wanted to take the bits that I’d done in the previous post and go beyond just tracking the joint data from the hands in front of sensor in 2D and move to representing them in 3D.
As usual, I’m a bit challenged when it comes to 3D and I tended to fall back on ‘bigger building blocks’ such as the Helix WPF toolkit that I've used here in order to draw basic spheres and tubes to make up hands as in the short video below;
I managed to plug this into the same ‘framework’ that I’ve used in the previous posts such as this one and that meant defining a control that I called HandDisplay3DControl and plugging it into my main UI;
<Window x:Class="WpfApplication2.MainWindow"
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
xmlns:controls="clr-namespace:WpfApplication2.Controls"
Title="MainWindow"
Height="600"
Width="800">
<Grid x:Name="parentGrid">
<Grid.ColumnDefinitions>
<ColumnDefinition />
<ColumnDefinition />
<ColumnDefinition />
</Grid.ColumnDefinitions>
<Grid.RowDefinitions>
<RowDefinition />
<RowDefinition />
<RowDefinition />
</Grid.RowDefinitions>
<controls:DepthVideoControl Grid.Row="0"
Grid.Column="1" />
<controls:HandDisplay3DControl Grid.Row="0"
Grid.Column="0"
Grid.ColumnSpan="3"
Grid.RowSpan="3"/>
</Grid>
</Window>
and then making some ‘UI’ for the control itself which really boils down to using a HelixViewport3D with a default light (SunLight) and camera (PerspectiveCamera) and then having a ModelVisual3D that I can add additional elements to at runtime.
I stuck with the scheme that I previously used where I only take notice of events coming from hands that the alerting capabilities in the SDK have told me are calibrated&detected&within bounds&tracked and I used the same code for that in my HandAlertManager class that I’ve used previously.
Drawing with 3D rather than 2D using Helix isn’t radically different from what I did in the previous post with the exception being that I tend to find that performance suffers if I keep re-drawing everything on every frame of data and so I take some efforts to try and re-use spheres and columns across iterations of drawing so as to avoid the cost of creation on every frame.
The code for that control then ends up looking like;
namespace WpfApplication2.Controls
{
using HelixToolkit.Wpf;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Media.Media3D;
using BoneVisualMap =
System.Collections.Generic.Dictionary<int, System.Collections.Generic.List<HelixToolkit.Wpf.TubeVisual3D>>;
using HandMap =
System.Collections.Generic.Dictionary<int, System.Collections.Generic.Dictionary<PXCMHandData.JointType, PXCMPoint3DF32>>;
using HandVisualMap =
System.Collections.Generic.Dictionary<int, System.Collections.Generic.Dictionary<PXCMHandData.JointType, System.Windows.Media.Media3D.ModelVisual3D>>;
using JointPositionMap =
System.Collections.Generic.Dictionary<PXCMHandData.JointType, PXCMPoint3DF32>;
using JointVisual3DMap =
System.Collections.Generic.Dictionary<PXCMHandData.JointType, System.Windows.Media.Media3D.ModelVisual3D>;
static class Constants
{
static public readonly float SphereRadius = 0.005f;
static public readonly float TubeDiameter = 0.003f;
}
public partial class HandDisplay3DControl : UserControl, ISampleRenderer
{
public HandDisplay3DControl()
{
InitializeComponent();
this.spheresInColours = brushes.Select(b => MakeSphereForBrush(b)).ToArray();
}
public void Initialise(PXCMSenseManager senseManager)
{
this.senseManager = senseManager;
this.senseManager.EnableHand().ThrowOnFail();
using (var handModule = this.senseManager.QueryHand())
{
using (var handConfiguration = handModule.CreateActiveConfiguration())
{
this.alertManager = new HandAlertManager(handConfiguration);
handConfiguration.EnableTrackedJoints(true).ThrowOnFail();
handConfiguration.ApplyChanges().ThrowOnFail();
}
this.handData = handModule.CreateOutput();
}
}
public void ProcessSampleWorkerThread(PXCMCapture.Sample sample)
{
// this is me being very lazy and simply letting the GC get rid of
// this instance every time around.
this.handMap = new HandMap();
if (this.handData.Update().Succeeded())
{
var handsInfoFromAlerts = this.alertManager.GetHandsInfo();
if ((handsInfoFromAlerts != null) && (handsInfoFromAlerts.Count() > 0))
{
var goodHands =
handsInfoFromAlerts
.Where(kvp => kvp.Value == HandAlertManager.HandStatus.Ok);
foreach (var entry in goodHands)
{
PXCMHandData.IHand iHand;
// gather the data to display that hand.
if (this.handData.QueryHandDataById(entry.Key, out iHand).Succeeded())
{
foreach (PXCMHandData.JointType joint in Enum.GetValues(typeof(PXCMHandData.JointType)))
{
PXCMHandData.JointData jointData;
if (iHand.QueryTrackedJoint(joint, out jointData).Succeeded())
{
if (joint == PXCMHandData.JointType.JOINT_INDEX_JT1)
{
Trace.WriteLine(string.Format("{0},{1},{2}",
jointData.positionWorld.x, jointData.positionWorld.y, jointData.positionWorld.z));
}
if (!this.handMap.ContainsKey(entry.Key))
{
this.handMap[entry.Key] = new JointPositionMap();
}
this.handMap[entry.Key][joint] = jointData.positionWorld;
}
}
}
}
}
}
}
public void RenderUI(PXCMCapture.Sample sample)
{
if ((this.handMap == null) || (this.handMap.Count == 0))
{
this.ClearAll();
}
else
{
if (this.drawnHandJointVisualMap == null)
{
this.drawnHandJointVisualMap = new HandVisualMap();
}
if (this.drawnHandBoneVisualMap == null)
{
this.drawnHandBoneVisualMap = new BoneVisualMap();
}
// get rid of anything we drew for a previous hand that is not
// present in the current frame of data.
this.ClearLostHands();
// draw the stuff that *is* present in the current frame of
// data...
this.DrawHands();
this.borderHighlight.BorderBrush = Brushes.Green;
}
}
void DrawHands()
{
foreach (var handId in this.handMap.Keys)
{
if (!this.drawnHandJointVisualMap.ContainsKey(handId))
{
this.drawnHandJointVisualMap[handId] = new JointVisual3DMap();
}
foreach (var joint in this.handMap[handId].Keys)
{
this.DrawJoint(handId, joint);
}
this.DrawBones(handId);
}
}
void DrawBones(int handId)
{
if (!this.drawnHandBoneVisualMap.ContainsKey(handId))
{
this.drawnHandBoneVisualMap[handId] = new List<TubeVisual3D>();
}
int tubeCount = 0;
foreach (var joints in jointConnections)
{
for (int i = 0; i < joints.Length - 1; i++)
{
var point = this.handMap[handId][joints[i]];
var next = this.handMap[handId][joints[i + 1]];
if (this.drawnHandBoneVisualMap[handId].Count() <= tubeCount)
{
var tube = MakeTubeForPositions(point, next);
this.drawnHandBoneVisualMap[handId].Add(tube);
this.modelVisual.Children.Add(tube);
}
else
{
var tube = this.drawnHandBoneVisualMap[handId][tubeCount];
tube.Path[0] = new Point3D(point.x, point.y, point.z);
tube.Path[1] = new Point3D(next.x, next.y, next.z);
}
tubeCount++;
}
}
}
void DrawJoint(int handId, PXCMHandData.JointType joint)
{
var position = this.handMap[handId][joint];
ModelVisual3D visual;
if (!this.drawnHandJointVisualMap[handId].TryGetValue(joint, out visual))
{
visual =
MakeSphereForHandPosition(handId, position.x, position.y, position.z);
this.drawnHandJointVisualMap[handId][joint] = visual;
this.modelVisual.Children.Add(visual);
}
var transform = ((TranslateTransform3D)visual.Transform);
transform.OffsetX = position.x;
transform.OffsetY = position.y;
transform.OffsetZ = position.z;
}
void ClearLostHands()
{
foreach (var oldHandId in this.drawnHandJointVisualMap.Keys.Where(
k => !this.handMap.ContainsKey(k)).ToList())
{
foreach (var joint in this.drawnHandJointVisualMap[oldHandId])
{
this.modelVisual.Children.Remove(joint.Value);
}
this.drawnHandJointVisualMap.Remove(oldHandId);
foreach (var bone in this.drawnHandBoneVisualMap[oldHandId])
{
this.modelVisual.Children.Remove(bone);
}
this.drawnHandBoneVisualMap.Remove(oldHandId);
}
}
void ClearAll()
{
this.modelVisual.Children.Clear();
this.drawnHandJointVisualMap = null;
this.drawnHandBoneVisualMap = null;
this.borderHighlight.BorderBrush = Brushes.Red;
}
ModelVisual3D MakeSphereForHandPosition(int handId, float x, float y, float z)
{
var modelVisual3d = new ModelVisual3D()
{
Content = this.spheresInColours[handId % brushes.Length].Model
};
modelVisual3d.Transform = new TranslateTransform3D()
{
OffsetX = x,
OffsetY = y,
OffsetZ = z
};
return (modelVisual3d);
}
static SphereVisual3D MakeSphereForBrush(Brush brush)
{
var sphere = new SphereVisual3D();
sphere.Radius = Constants.SphereRadius;
sphere.Fill = brush;
return (sphere);
}
static TubeVisual3D MakeTubeForPositions(PXCMPoint3DF32 p1, PXCMPoint3DF32 p2)
{
var tube = new TubeVisual3D();
tube.Diameter = Constants.TubeDiameter;
Point3DCollection points = new Point3DCollection();
points.Add(new Point3D(p1.x, p1.y, p1.z));
points.Add(new Point3D(p2.x, p2.y, p2.z));
tube.Path = points;
tube.Fill = Brushes.Silver;
return (tube);
}
static Brush[] brushes =
{
Brushes.Red,
Brushes.Green,
Brushes.Blue,
Brushes.Yellow,
Brushes.Cyan,
Brushes.Purple
};
static PXCMHandData.JointType[][] jointConnections =
{
new []
{
PXCMHandData.JointType.JOINT_WRIST,
PXCMHandData.JointType.JOINT_PINKY_BASE,
PXCMHandData.JointType.JOINT_PINKY_JT1,
PXCMHandData.JointType.JOINT_PINKY_JT2,
PXCMHandData.JointType.JOINT_PINKY_TIP
},
new []
{
PXCMHandData.JointType.JOINT_WRIST,
PXCMHandData.JointType.JOINT_RING_BASE,
PXCMHandData.JointType.JOINT_RING_JT1,
PXCMHandData.JointType.JOINT_RING_JT2,
PXCMHandData.JointType.JOINT_RING_TIP
},
new []
{
PXCMHandData.JointType.JOINT_WRIST,
PXCMHandData.JointType.JOINT_CENTER,
PXCMHandData.JointType.JOINT_MIDDLE_BASE,
PXCMHandData.JointType.JOINT_MIDDLE_JT1,
PXCMHandData.JointType.JOINT_MIDDLE_JT2,
PXCMHandData.JointType.JOINT_MIDDLE_TIP
},
new []
{
PXCMHandData.JointType.JOINT_WRIST,
PXCMHandData.JointType.JOINT_INDEX_BASE,
PXCMHandData.JointType.JOINT_INDEX_JT1,
PXCMHandData.JointType.JOINT_INDEX_JT2,
PXCMHandData.JointType.JOINT_INDEX_TIP
},
new []
{
PXCMHandData.JointType.JOINT_WRIST,
PXCMHandData.JointType.JOINT_THUMB_BASE,
PXCMHandData.JointType.JOINT_THUMB_JT1,
PXCMHandData.JointType.JOINT_THUMB_JT2,
PXCMHandData.JointType.JOINT_THUMB_TIP
}
};
SphereVisual3D[] spheresInColours;
HandMap handMap;
HandVisualMap drawnHandJointVisualMap;
BoneVisualMap drawnHandBoneVisualMap;
HandAlertManager alertManager;
PXCMHandData handData;
PXCMSenseManager senseManager;
}
}
and I found that implementing this in terms of;
- the ‘framework’ that I’ve wrapped around my controls
- the controls that I’ve ended up writing
- the specifics of the control that draws the hand skeleton via Helix 3D
is incredibly similar to the code that I wrote back in this post which was targeted at using the same technologies to draw whole body skeletons from the Kinect v2 sensor and, indeed, I borrowed some code from that post in order to write this post – the scenarios are so similar that the techniques seem to carry straight over.
The code for this is here for download if you want to have a look at it.
Mike Taulty 