/// <summary>
/// Constructor sets the markers name in the MarkesName class used for joint localization
/// </summary>
/// <param name="labelMarkers">The list of labelmarkets</param>
/// <param name="markerNames">A reference to the markers names</param>
/// <param name="bodyPrefix">Any possible prefix of the markersname</param>
public MarkersPreprocessor(List <Marker> labelMarkers, out MarkersNames markerNames, string bodyPrefix = "")
{
this.prefix = bodyPrefix;
markers = new Dictionary <string, Vector3>();
//Converting markers
for (int i = 0; i < labelMarkers.Count; i++)
{
markers.Add(labelMarkers[i].Label, labelMarkers[i].Position);
}
//Finding the markers aliases
markerNames = NameSet(markers.Keys);
//foreach (var n in markerNames) UnityEngine.Debug.Log(n);
m = markerNames;
// adding non existing markers
markersLastFrame = new Dictionary <string, Vector3>();
foreach (var mark in m)
{
markersLastFrame.Add(mark, Vector3Helper.NaN);
if (!markers.ContainsKey(mark))
{
markers.Add(mark, Vector3Helper.NaN);
}
}
// setting arbitrary hip makres positions
markersLastFrame[m.bodyBase] = lastSACRUMknown;
markersLastFrame[m.leftHip] = lastLIASknown;
markersLastFrame[m.rightHip] = lastRIASknown;
}
public BodyData(MarkersNames m)
{
this.m = m;
Height = 175; // cm
Mass = 75; //kg
ShoulderWidth = 400; //mm
}
/// <summary>
/// Setting up for joint localization
/// </summary>
/// <param name="markers">The aliases of the marker names</param>
public JointLocalization(MarkersNames markerNames)
{
this.m = markerNames;
bd = new BodyData(m);
jcFuncs = new List <Action <Bone> >()
{
(b) => Pelvis(b),
(b) => SpineRoot(b),
(b) => MidSpine(b),
(b) => SpineEnd(b),
(b) => Neck(b),
(b) => GetHead(b),
(b) => GetHeadTop(b),
(b) => GetShoulderLeft(b),
(b) => GetUpperArmLeft(b),
(b) => GetLowerArmLeft(b),
(b) => GetWristLeft(b),
(b) => GetHandLeft(b),
(b) => GetIndexLeft(b),
(b) => GetTrapLeft(b),
(b) => GetThumbLeft(b),
(b) => GetShoulderRight(b),
(b) => GetUpperArmRight(b),
(b) => GetLowerArmRight(b),
(b) => GetWristRight(b),
(b) => GetHandRight(b),
(b) => GetIndexRight(b),
(b) => GetTrapRight(b),
(b) => GetThumbRight(b),
(b) => UpperLegLeft(b),
(b) => LowerLegLeft(b),
(b) => GetAnkleLeft(b),
(b) => GetFootBaseLeft(b),
(b) => GetFootLeft(b),
(b) => UpperLegRight(b),
(b) => LowerLegRight(b),
(b) => GetAnkleRight(b),
(b) => GetFootBaseRight(b),
(b) => GetFootRight(b),
};
}
/// <summary>
/// Finds aliases of different markers and replaces the names
/// </summary>
/// <param name="markersNames">A collection of the names of the markers</param>
/// <returns>The set marker names</returns>
private MarkersNames NameSet(ICollection <string> markersNames)
{
MarkersNames m = new MarkersNames();
#region hip
var query = MarkerNames.bodyBaseAKA.FirstOrDefault(n => markersNames.Contains(prefix + n));
if (query == null)
{
var q2 = MarkerNames.bodyBasebetween.FirstOrDefault(n => markersNames.Contains(prefix + n[0]) && markersNames.Contains(prefix + n[1]));
if (q2 != null)
{
sacrumBetween = true;
sacrumBetweenMarkers = new string[2];
sacrumBetweenMarkers[0] = prefix + q2[0];
sacrumBetweenMarkers[1] = prefix + q2[1];
}
}
else
{
m.bodyBase = prefix + query;
}
SetName(markersNames, MarkerNames.leftHipAKA, ref m.leftHip, prefix);
SetName(markersNames, MarkerNames.rightHipAKA, ref m.rightHip, prefix);
#endregion
#region upperbody
SetName(markersNames, MarkerNames.spineAKA, ref m.spine, prefix);
SetName(markersNames, MarkerNames.neckAKA, ref m.neck, prefix);
SetName(markersNames, MarkerNames.chestAKA, ref m.chest, prefix);
SetName(markersNames, MarkerNames.leftShoulderAKA, ref m.leftShoulder, prefix);
SetName(markersNames, MarkerNames.rightShoulderAKA, ref m.rightShoulder, prefix);
#endregion
#region head
query = MarkerNames.headAKA.FirstOrDefault(n => markersNames.Contains(prefix + n));
if (query == null)
{
var q2 = MarkerNames.headBetween.FirstOrDefault(n => markersNames.Contains(prefix + n[0]) && markersNames.Contains(prefix + n[1]));
if (q2 != null)
{
frontHeadBetween = true;
frontHeadBetweenMarkers = new string[2];;
frontHeadBetweenMarkers[0] = prefix + q2[0];
frontHeadBetweenMarkers[1] = prefix + q2[1];
}
}
else
{
m.head = prefix + query;
}
SetName(markersNames, MarkerNames.leftHeadAKA, ref m.leftHead, prefix);
SetName(markersNames, MarkerNames.rightHeadAKA, ref m.rightHead, prefix);
#endregion
#region legs
SetName(markersNames, MarkerNames.leftUpperKneeAKA, ref m.leftUpperKnee, prefix);
SetName(markersNames, MarkerNames.rightUpperKneeAKA, ref m.rightUpperKnee, prefix);
SetName(markersNames, MarkerNames.leftOuterKneeAKA, ref m.leftOuterKnee, prefix);
SetName(markersNames, MarkerNames.rightOuterKneeAKA, ref m.rightOuterKnee, prefix);
SetName(markersNames, MarkerNames.leftInnerKneeAKA, ref m.leftInnerKnee, prefix);
SetName(markersNames, MarkerNames.rightInnerKneeAKA, ref m.rightInnerKnee, prefix);
SetName(markersNames, MarkerNames.leftLowerKneeAKA, ref m.leftLowerKnee, prefix);
SetName(markersNames, MarkerNames.rightLowerKneeAKA, ref m.rightLowerKnee, prefix);
#endregion
#region foot
SetName(markersNames, MarkerNames.leftOuterAnkleAKA, ref m.leftOuterAnkle, prefix);
SetName(markersNames, MarkerNames.rightOuterAnkleAKA, ref m.rightOuterAnkle, prefix);
SetName(markersNames, MarkerNames.rightInnerAnkleAKA, ref m.rightInnerAnkle, prefix);
SetName(markersNames, MarkerNames.leftInnerAnkleAKA, ref m.leftInnerAnkle, prefix);
SetName(markersNames, MarkerNames.leftHeelAKA, ref m.leftHeel, prefix);
SetName(markersNames, MarkerNames.rightHeelAKA, ref m.rightHeel, prefix);
query = MarkerNames.leftToe2AKA.FirstOrDefault(n => markersNames.Contains(prefix + n));
if (query == null)
{
query = MarkerNames.leftToe1AKA.FirstOrDefault(n => markersNames.Contains(prefix + n));
}
m.leftToe2 = ((query == null) ? m.leftToe2 :prefix + query);
query = MarkerNames.rightToe2AKA.FirstOrDefault(n => markersNames.Contains(prefix + n));
if (query == null)
{
query = MarkerNames.rightToe1AKA.FirstOrDefault(n => markersNames.Contains(prefix + n));
}
m.rightToe2 = ((query == null) ? m.rightToe2 :prefix + query);
#endregion
#region arms
SetName(markersNames, MarkerNames.leftElbowAKA, ref m.leftElbow, prefix);
SetName(markersNames, MarkerNames.rightElbowAKA, ref m.rightElbow, prefix);
SetName(markersNames, MarkerNames.leftInnerElbowAKA, ref m.leftInnerElbow, prefix);
SetName(markersNames, MarkerNames.rightInnerElbowAKA, ref m.rightInnerElbow, prefix);
SetName(markersNames, MarkerNames.leftOuterElbowAKA, ref m.leftOuterElbow, prefix);
SetName(markersNames, MarkerNames.rightOuterElbowAKA, ref m.rightOuterElbow, prefix);
SetName(markersNames, MarkerNames.leftWristAKA, ref m.leftWrist, prefix);
SetName(markersNames, MarkerNames.rightWristAKA, ref m.rightWrist, prefix);
SetName(markersNames, MarkerNames.leftWristRadiusAKA, ref m.leftWristRadius, prefix);
SetName(markersNames, MarkerNames.rightWristRadiusAKA, ref m.rightWristRadius, prefix);
#endregion
#region hands
SetName(markersNames, MarkerNames.leftHandAKA, ref m.leftHand, prefix);
SetName(markersNames, MarkerNames.rightHandAKA, ref m.rightHand, prefix);
SetName(markersNames, MarkerNames.rightIndexAKA, ref m.rightIndex, prefix);
SetName(markersNames, MarkerNames.leftIndexAKA, ref m.leftIndex, prefix);
SetName(markersNames, MarkerNames.rightThumbAKA, ref m.rightThumb, prefix);
SetName(markersNames, MarkerNames.leftThumbAKA, ref m.leftThumb, prefix);
#endregion
return(m);
}
//private readonly string DEBUG_MARKER = "L_HME";
/// <summary>
/// Initializes a new instance of the <see cref="QualisysRealTime.Unity.Skeleton.ŚegmentTracking"/> class.
/// </summary>
/// <param name="s">S.</param>
/// <param name="mn">Mn.</param>
/// <param name="lm">Lm.</param>
public ŚegmentTracking(BipedSkeleton s, MarkersNames mn, List <Marker> lm)
{
this.skeleton = s;
this.markerNames = mn;
// Converting markers
this.markers = new Dictionary <string, Vector3>();
for (int i = 0; i < lm.Count; i++)
{
this.markers.Add(lm[i].Label, lm[i].Position);
}
// Add the tracking markers for the joints / segments
string[] labels;
// Hips
labels = new string[] {
markerNames.bodyBase,
markerNames.leftHip,
markerNames.rightHip,
"L_PSIS", // Additional Tracking Marker
"R_PSIS" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.PELVIS, labels);
// Chest
labels = new string[] {
markerNames.neck,
markerNames.spine,
markerNames.chest,
"STRN" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.SPINE3, labels);
// Left Upper Arm
labels = new string[] {
markerNames.leftElbow,
markerNames.leftInnerElbow,
markerNames.leftOuterElbow,
"L_UPA" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.SHOULDER_L, labels);
// Right Upper Arm
labels = new string[] {
markerNames.rightElbow,
markerNames.rightInnerElbow,
markerNames.rightOuterElbow,
"R_UPA" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.SHOULDER_R, labels);
// Left Lower Arm
labels = new string[] {
markerNames.leftWrist,
markerNames.leftWristRadius,
"L_FRA" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.ELBOW_L, labels);
// Right Lower Arm
labels = new string[] {
markerNames.rightWrist,
markerNames.rightWristRadius,
"R_FRA" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.ELBOW_R, labels);
// Left Thigh
labels = new string[] {
markerNames.leftUpperKnee,
markerNames.leftInnerKnee,
markerNames.leftOuterKnee,
"L_THI" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.HIP_L, labels);
// Right Thigh
labels = new string[] {
markerNames.rightUpperKnee,
markerNames.rightInnerKnee,
markerNames.rightOuterKnee,
"R_THI" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.HIP_R, labels);
// Left Shank
labels = new string[] {
markerNames.leftLowerKnee,
markerNames.leftInnerAnkle,
markerNames.leftOuterAnkle,
"L_TIB" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.KNEE_L, labels);
// Right Shank
labels = new string[] {
markerNames.rightLowerKnee,
markerNames.rightInnerAnkle,
markerNames.rightOuterAnkle,
"R_TIB" // Additional Tracking Marker
};
this.addTrackingMarkers(Joint.KNEE_R, labels);
}
