/// <summary> /// Build a skeleton according to the markers /// </summary> /// <param name="markerData">A list of markers </param> /// <returns>A complete skeleton</returns> public BipedSkeleton SolveSkeleton(List <LabeledMarker> markerData) { if (skeleton == null || skeletonBuffer == null || mp == null || joints == null || ikApplier == null) { skeleton = new BipedSkeleton(); skeletonBuffer = new BipedSkeleton(); MarkersNames markersMap; mp = new MarkersPreprocessor(markerData, out markersMap, bodyPrefix: MarkerPrefix);; joints = new JointLocalization(markersMap); ikApplier = new IKApplier(skeleton); } Dictionary <string, OpenTK.Vector3> markers; mp.ProcessMarkers(markerData, out markers, MarkerPrefix); var temp = skeleton; skeleton = skeletonBuffer; skeletonBuffer = temp; joints.GetJointLocation(markers, ref skeleton); ikApplier.Interpolation = Interpolation; if (SolveWithIK) { ikApplier.ApplyIK(ref skeleton); } return(skeleton); }
/// <summary> /// ... /// </summary> public void ResetSkeleton() { if (DEBUG.enabled) { Debug.Log("Reset Skeleton..."); } charactersJoints.SetLimbs(this.transform, UseFingers); //charactersJoints.PrintAll(); skeletonBuilder = new SkeletonBuilder(); skeletonBuilder.MarkerPrefix = ActorMarkerPrefix; skeletonBuilder.SetBodyData(actorHeight, actorMass); skeletonBuilder.SolveWithIK = UseIK; skeletonBuilder.UseTrackingMarkers = UseTrackingMarkers; if (markerData != null) { skeleton = skeletonBuilder.SolveSkeleton(markerData); } else { skeleton = new BipedSkeleton(); } if (ScaleMovementToSize) { scale = FindScale(); } else { scale = 0; } }
/// <summary> /// Build a skeleton according to the markers /// </summary> /// <param name="markerData">A list of markers </param> /// <returns>A complete skeleton</returns> public BipedSkeleton SolveSkeleton(List<Marker> markerData) { if ( skeleton == null || skeletonBuffer == null || mp == null || joints == null || ikApplier == null) { skeleton = new BipedSkeleton(); skeletonBuffer = new BipedSkeleton(); MarkersNames markersMap; mp = new MarkersPreprocessor(markerData, out markersMap, bodyPrefix: MarkerPrefix); ; joints = new JointLocalization(markersMap); ikApplier = new IKApplier(skeleton); } Dictionary<string, OpenTK.Vector3> markers; mp.ProcessMarkers(markerData, out markers, MarkerPrefix); var temp = skeleton; skeleton = skeletonBuffer; skeletonBuffer = temp; joints.GetJointLocation(markers, ref skeleton); ikApplier.Interpolation = Interpolation; if (SolveWithIK) ikApplier.ApplyIK(ref skeleton); return skeleton; }
/// <summary> /// Updates the rotation and position of the Character /// </summary> public void Update() { if (rtClient == null) { rtClient = RTClient.GetInstance(); } if (!rtClient.GetStreamingStatus()) { return; } markerData = rtClient.Markers; if ((markerData == null || markerData.Count == 0)) { Debug.LogError("The stream does not contain any markers"); return; } if (skeletonBuilder != null) { skeleton = skeletonBuilder.SolveSkeleton(markerData); } else { ResetSkeleton(); } SetAll(); if (!headCam.UseHeadCamera && headCamera) { DestroyCamera(); } }
public IKApplier(BipedSkeleton firstSkel) { lastSkel = firstSkel; IKSolver = new CCD(); FABRIK = new FABRIK(); FABRIK.MaxIterations = 100; }
public void ResetSkeleton() { charactersJoints.SetLimbs(this.transform, UseFingers); skeletonBuilder = new SkeletonBuilder(); skeletonBuilder.MarkerPrefix = ActorMarkersPrefix; skeletonBuilder.SolveWithIK = UseIK; if (markerData != null) skeleton = skeletonBuilder.SolveSkeleton(markerData); else skeleton = new BipedSkeleton(); if (ScaleMovementToSize) scale = FindScale(); else scale = 0; }
/// <summary> /// Fills in the skeleton with the joint positions given the set of markers /// </summary> /// <param name="markerData">The dictionary contaiing the markers and their position</param> /// <param name="skeleton">The skeleton to be filled in</param> public void GetJointLocation(Dictionary <string, Vector3> markerData, ref BipedSkeleton skeleton) { o = new Values(); // reset joint pos and orientations markers = markerData; // collect data from markers about body proportions // this is necessary for shoulder joint localization // Locate hip orientation, hip orientation is important for IK solver, bd.CalculateBodyData(markers, ChestOrientation); // get all joints int i = 0; SetJointsRecursive(skeleton.Root, ref i); }
/// <summary> /// Build a skeleton according to the markers /// </summary> /// <param name="markerData">A list of markers </param> /// <returns>A complete skeleton</returns> public BipedSkeleton SolveSkeleton(List <Marker> markerData) { if (skeleton == null || skeletonBuffer == null || mp == null || joints == null || ikApplier == null) { skeleton = new BipedSkeleton(); skeletonBuffer = new BipedSkeleton(); MarkersNames markersMap; mp = new MarkersPreprocessor(markerData, out markersMap, bodyPrefix: MarkerPrefix); joints = new JointLocalization(markersMap); ikApplier = new IKApplier(skeleton); // Set segment tracking markers for virtual marker construction st = new ŚegmentTracking(skeleton, markersMap, markerData); } var temp = skeleton; skeleton = skeletonBuffer; skeletonBuffer = temp; Dictionary <string, OpenTK.Vector3> markers; mp.UpdateMarkerList(markerData, out markers); mp.ProcessMarkers(out markers); joints.BodyData.Height = bodyHeight; joints.BodyData.Mass = bodyMass; joints.GetJointLocations(markers, ref skeleton); // Try to reconstruct virtual markers if (UseTrackingMarkers) { if (st.ProcessMarkers(skeleton, markerData, ref markers, MarkerPrefix)) { mp.ProcessMarkers(out markers); joints.GetJointLocations(markers, ref skeleton); } } ikApplier.Interpolation = Interpolation; if (SolveWithIK) { ikApplier.ApplyIK(ref skeleton); } return(skeleton); }
/// <summary> /// Updates the rotation and position of the Character /// </summary> public void Update() { if (rtClient == null) rtClient = RTClient.GetInstance(); if (!rtClient.GetStreamingStatus()) return; markerData = rtClient.Markers; if ((markerData == null || markerData.Count == 0)) { Debug.LogError("The stream does not contain any markers"); return; } if (skeletonBuilder != null) skeleton = skeletonBuilder.SolveSkeleton(markerData); else ResetSkeleton(); SetAll(); if (!headCam.UseHeadCamera && headCamera) DestroyCamera(); }
public void ResetSkeleton() { charactersJoints.SetLimbs(this.transform, UseFingers); skeletonBuilder = new SkeletonBuilder(); skeletonBuilder.MarkerPrefix = ActorMarkersPrefix; skeletonBuilder.SolveWithIK = UseIK; if (markerData != null) { skeleton = skeletonBuilder.SolveSkeleton(markerData); } else { skeleton = new BipedSkeleton(); } if (ScaleMovementToSize) { scale = FindScale(); } else { scale = 0; } }
/// <summary> /// Checks for empty position in the skeleton, fills the from the last skeleton. /// Root and all of roots children MUST have set possition! /// </summary> /// <param name="skeleton">The skeleton to be checked </param> public void ApplyIK(ref BipedSkeleton skeleton) { //Root and all of roots children MUST have set possition skeleton.Root.Traverse(t => TraversFunc(t)); lastSkel = skeleton; }
/// <summary> /// Fills in the skeleton with the joint positions given the set of markers /// </summary> /// <param name="markerData">The dictionary contaiing the markers and their position</param> /// <param name="skeleton">The skeleton to be filled in</param> public void GetJointLocation(Dictionary<string, Vector3> markerData, ref BipedSkeleton skeleton) { o = new Values(); // reset joint pos and orientations markers = markerData; // collect data from markers about body proportions // this is necessary for shoulder joint localization // Locate hiporientation, hip orientation is important for IK solver, bd.CalculateBodyData(markers, ChestOrientation); // get all joints int i = 0; SetJointsRecursive(skeleton.Root, ref i); }
//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); }
/// <summary> /// Processes the markers. /// </summary> /// <returns><c>true</c>, if markers was processed, <c>false</c> otherwise.</returns> /// <param name="s">S.</param> /// <param name="lm">Lm.</param> /// <param name="nm">Nm.</param> /// <param name="prefix">Prefix.</param> public bool ProcessMarkers(BipedSkeleton s, List <Marker> lm, ref Dictionary <string, Vector3> nm, string prefix) { this.skeleton = s; // Convert labeled markers to marker dictionary this.markers.Clear(); for (int i = 0; i < lm.Count; i++) { this.markers.Add(lm[i].Label, lm[i].Position); } var virtualMarkersCreated = false; foreach (var segment in this.segments.Keys) { // Check for unavailable markers in tracked segments if (this.segmentIsTracked(segment)) { var availableMarkers = new List <string>(); var unavailableMarkers = new List <string>(); // Check if one of the tracking markers is not available foreach (var label in this.segments[segment].Keys) { if (label == ORIGIN) { continue; // No origin markers } if ( this.markers[label].IsNaN() // DEBUG //|| label == DEBUG_MARKER ) { unavailableMarkers.Add(label); } else { availableMarkers.Add(label); } } // Marker is not available, but there are enough tracking markers left if (unavailableMarkers.Count > 0 && availableMarkers.Count >= 2) { //UnityEngine.Debug.Log("real: " + this.markers[DEBUG_MARKER]); //var length1 = this.markers[DEBUG_MARKER].Length; createVirtualMarkers(segment, availableMarkers, unavailableMarkers); //UnityEngine.Debug.Log("virtual: " + this.markers[DEBUG_MARKER]); //var length2 = this.markers[DEBUG_MARKER].Length; //UnityEngine.Debug.Log("DISTANCE real to virtual is " + (length1-length2) ); virtualMarkersCreated = true; } else if (unavailableMarkers.Count == 0) { // Update segment tracking markers everytime all markers // are available to increase accuracy updateTrackingMarkers(segment); } // Update markers foreach (var label in unavailableMarkers) { nm[label] = this.markers[label]; } } else { // Update segment tracking markers updateTrackingMarkers(segment); } } // Return if virtual markers were created return(virtualMarkersCreated); }
public void SetConstraints(BipedSkeleton skeleton) { #region Cone constraints #region Spine too head skeleton[Joint.SPINE0].Constraints = (Spine); skeleton[Joint.SPINE1].Constraints = (Spine); skeleton[Joint.NECK].Constraints = (Neck); #endregion #region Legs skeleton[Joint.HIP_L].Constraints = (SwapXZ(Femur)); skeleton[Joint.HIP_R].Constraints = (Femur); skeleton[Joint.KNEE_L].Constraints = (SwapXZ(Knee)); skeleton[Joint.KNEE_R].Constraints = (Knee); skeleton[Joint.ANKLE_L].Constraints = (SwapXZ(Ankle)); skeleton[Joint.ANKLE_R].Constraints = (Ankle); skeleton[Joint.FOOTBASE_L].Constraints = (SwapXZ(FootBase)); skeleton[Joint.FOOTBASE_R].Constraints = (FootBase); #endregion #region Arms skeleton[Joint.CLAVICLE_L].Constraints = (SwapXZ(Clavicula)); skeleton[Joint.CLAVICLE_R].Constraints = (Clavicula); skeleton[Joint.SHOULDER_L].Constraints = (SwapXZ(Shoulder)); skeleton[Joint.SHOULDER_R].Constraints = (Shoulder); skeleton[Joint.ELBOW_L].Constraints = (SwapXZ(Elbow)); skeleton[Joint.ELBOW_R].Constraints = (Elbow); skeleton[Joint.WRIST_L].Constraints = (SwapXZ(Wrist)); skeleton[Joint.WRIST_R].Constraints = (Wrist); #endregion #endregion #region ParentPointers skeleton[Joint.CLAVICLE_R].ParentPointer = QuaternionHelper2.RotationZ(-MathHelper.PiOver2); skeleton[Joint.CLAVICLE_L].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.PiOver2); skeleton[Joint.HIP_R].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.Pi); skeleton[Joint.HIP_L].ParentPointer = QuaternionHelper2.RotationZ(MathHelper.Pi); skeleton[Joint.ANKLE_R].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(-MathHelper.PiOver4); skeleton[Joint.ANKLE_L].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(MathHelper.PiOver4); skeleton[Joint.FOOTBASE_L].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(-MathHelper.PiOver4); skeleton[Joint.FOOTBASE_R].ParentPointer = QuaternionHelper2.RotationX(MathHelper.PiOver4) * QuaternionHelper2.RotationZ(MathHelper.PiOver4); #endregion #region TwistConstraints #region Spine skeleton[Joint.SPINE0].TwistLimit = (SpineTwist); skeleton[Joint.SPINE1].TwistLimit = (SpineTwist); skeleton[Joint.SPINE3].TwistLimit = (NoTwist); skeleton[Joint.NECK].TwistLimit = (NeckTwist); #endregion #region Legs skeleton[Joint.HIP_L].TwistLimit = (FemurTwist); skeleton[Joint.HIP_R].TwistLimit = (FemurTwist); skeleton[Joint.KNEE_L].TwistLimit = (KneeTwist); skeleton[Joint.KNEE_R].TwistLimit = (KneeTwist); skeleton[Joint.ANKLE_L].TwistLimit = (AnkleTwist); skeleton[Joint.ANKLE_R].TwistLimit = (AnkleTwist); skeleton[Joint.FOOTBASE_L].TwistLimit = (FootBaseTwist); skeleton[Joint.FOOTBASE_R].TwistLimit = (FootBaseTwist); #endregion #region Arms skeleton[Joint.CLAVICLE_L].TwistLimit = (ClaviculaTwist); skeleton[Joint.SHOULDER_L].TwistLimit = (ShoulderTwist); skeleton[Joint.ELBOW_L].TwistLimit = (ElbowTwist); skeleton[Joint.WRIST_L].TwistLimit = (WristTwist); skeleton[Joint.CLAVICLE_R].TwistLimit = (ClaviculaTwist); skeleton[Joint.SHOULDER_R].TwistLimit = (ShoulderTwist); skeleton[Joint.ELBOW_R].TwistLimit = (ElbowTwist); skeleton[Joint.WRIST_R].TwistLimit = (WristTwist); #endregion #endregion #region stiffness #region Arms skeleton[Joint.CLAVICLE_L].Stiffness = (verystiff); skeleton[Joint.CLAVICLE_R].Stiffness = (verystiff); skeleton[Joint.WRIST_L].Stiffness = (barelymoving); skeleton[Joint.WRIST_R].Stiffness = (barelymoving); #endregion #region Legs skeleton[Joint.ANKLE_L].Stiffness = (barelymoving); skeleton[Joint.ANKLE_R].Stiffness = (barelymoving); skeleton[Joint.FOOTBASE_L].Stiffness = (barelymoving); skeleton[Joint.FOOTBASE_R].Stiffness = (barelymoving); #endregion #endregion }