/// <summary>
/// Performs raycasting for all keypoints for pose. If point not found,
/// sets position to null.
/// </summary>
/// <param name="pose"></param>
public void Update3DKeypoints(FritzPose pose)
{
var totalDistance = 0.0f;
foreach (var keypoint in pose.keypoints)
{
var viewportPoint = PlacePointInViewport(keypoint.position);
var screenPoint = new Vector2(
viewportPoint.x * Screen.width,
viewportPoint.y * Screen.height
);
if (keypoint.confidence < trackableManager.partConfidenceCutoff)
{
Debug.LogFormat("Skipping {0} for low confidence of {1}", keypoint.part, keypoint.confidence);
current3DPoints[(int)keypoint.part] = null;
continue;
}
List <ARRaycastHit> results = new List <ARRaycastHit>();
var raycastSuccess = trackableManager.RaycastManager.Raycast(
screenPoint,
results,
UnityEngine.XR.ARSubsystems.TrackableType.FeaturePoint
);
if (raycastSuccess)
{
if (results.Count == 0)
{
continue;
}
var kpVector = Vector3.zero;
var accepted = 0;
foreach (var hit in results)
{
if (hit.sessionRelativeDistance < trackableManager.minDistance)
{
continue;
}
accepted += 1;
totalDistance += hit.sessionRelativeDistance;
kpVector += hit.pose.position;
}
if (accepted == 0)
{
current3DPoints[(int)keypoint.part] = null;
continue;
}
var avgPoint = kpVector / results.Count;
var filter = filters[(int)keypoint.part];
current3DPoints[(int)keypoint.part] = filter.Filter(avgPoint);
}
}
}
/// <summary>
/// Update both Keypoints, Scale, and Center and location of deug spheres and bounding box (if enabled).
/// </summary>
/// <param name="pose"></param>
public void UpdateWithPose(FritzPose pose)
{
Update3DKeypoints(pose);
UpdateEstimatedScaleAndCenter();
if (trackableManager.DebugSpheresEnabled)
{
UpdateSpheres();
}
if (trackableManager.DebugBoundingBoxEnabled)
{
UpdateBoundingBox();
}
}
/// <summary>
/// Main method that creates and updates a trackable. The caller is responsible for
/// passing in the Trackable ID that the Pose corresponds to.
/// </summary>
/// <param name="trackableId"></param>
/// <param name="pose"></param>
/// <returns></returns>
public FritzHumanTrackable CreateOrUpdateTrackable(int trackableId, FritzPose pose)
{
FritzHumanTrackable trackable;
if (!trackables.TryGetValue(trackableId, out trackable))
{
trackable = CreateTrackable(trackableId);
trackables.Add(trackableId, trackable);
}
if (trackable == null)
{
return(null);
}
return(UpdateTrackable(trackable, pose));
}
FritzHumanTrackable UpdateTrackable(FritzHumanTrackable trackable, FritzPose pose)
{
trackable.UpdateWithPose(pose);
return(trackable);
}