867 lines
34 KiB
C#
867 lines
34 KiB
C#
//======= Copyright (c) Stereolabs Corporation, All rights reserved. ===============
|
|
|
|
using UnityEngine;
|
|
using System.Runtime.InteropServices;
|
|
using UnityEngine.VR;
|
|
using System.IO;
|
|
|
|
/// <summary>
|
|
/// In pass-through AR mode, handles the final output to the VR headset, positioning the final images
|
|
/// to make the pass-through effect natural and comfortable. Also moves/rotates the images to
|
|
/// compensate for the ZED image's latency using our Video Asynchronous Timewarp.
|
|
/// ZEDManager attaches this component to a second stereo rig called "ZEDRigDisplayer" that it
|
|
/// creates and hides in the editor at runtime; see ZEDManager.CreateZEDRigDisplayer() to see this process.
|
|
///
|
|
/// The Timewarp effect is achieved by logging the pose of the headset each time it's available within the
|
|
/// wrapper. Then, when a ZED image is available, the wrapper looks up the headset's position using the timestamp
|
|
/// of the image, and moves the final viewing planes according to that position. In this way, the ZED's images
|
|
/// line up with real-life, even after a ~60ms latency.
|
|
/// </summary>
|
|
public class ZEDMixedRealityPlugin : MonoBehaviour
|
|
{
|
|
#region DLL Calls
|
|
const string nameDll = sl.ZEDCommon.NameDLL;
|
|
[DllImport(nameDll, EntryPoint = "dllz_compute_size_plane_with_gamma")]
|
|
private static extern System.IntPtr dllz_compute_size_plane_with_gamma(sl.Resolution resolution, float perceptionDistance, float eyeToZedDistance, float planeDistance, float HMDFocal, float zedFocal);
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_compute_hmd_focal")]
|
|
private static extern float dllz_compute_hmd_focal(sl.Resolution r, float w, float h);
|
|
|
|
/*****LATENCY CORRECTOR***/
|
|
[DllImport(nameDll, EntryPoint = "dllz_latency_corrector_add_key_pose")]
|
|
private static extern void dllz_latency_corrector_add_key_pose(ref Vector3 translation, ref Quaternion rotation, ulong timeStamp);
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_latency_corrector_get_transform")]
|
|
private static extern int dllz_latency_corrector_get_transform(ulong timeStamp, bool useLatency,out Vector3 translation, out Quaternion rotation);
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_latency_corrector_initialize")]
|
|
private static extern void dllz_latency_corrector_initialize(int device);
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_latency_corrector_shutdown")]
|
|
private static extern void dllz_latency_corrector_shutdown();
|
|
|
|
/****ANTI DRIFT ***/
|
|
[DllImport(nameDll, EntryPoint = "dllz_drift_corrector_initialize")]
|
|
public static extern void dllz_drift_corrector_initialize();
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_drift_corrector_shutdown")]
|
|
public static extern void dllz_drift_corrector_shutdown();
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_drift_corrector_get_tracking_data")]
|
|
public static extern void dllz_drift_corrector_get_tracking_data(ref TrackingData trackingData, ref Pose HMDTransform, ref Pose latencyCorrectorTransform, int hasValidTrackingPosition,bool checkDrift);
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_drift_corrector_set_calibration_transform")]
|
|
public static extern void dllz_drift_corrector_set_calibration_transform(ref Pose pose);
|
|
|
|
[DllImport(nameDll, EntryPoint = "dllz_drift_corrector_set_calibration_const_offset_transform")]
|
|
public static extern void dllz_drift_corrector_set_calibration_const_offset_transform(ref Pose pose);
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// Container for storing historic pose information, used by the latency corrector.
|
|
/// </summary>
|
|
public struct KeyPose
|
|
{
|
|
public Quaternion Orientation;
|
|
public Vector3 Translation;
|
|
public ulong Timestamp;
|
|
};
|
|
|
|
/// <summary>
|
|
/// Container for position and rotation. Used when timestamps are not needed or have already
|
|
/// been processed, such as setting the initial camera offset or updating the stereo rig's
|
|
/// transform from data pulled from the wrapper.
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Pose
|
|
{
|
|
public Vector3 translation;
|
|
public Quaternion rotation;
|
|
|
|
public Pose(Vector3 t, Quaternion q)
|
|
{
|
|
translation = t;
|
|
rotation = q;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
///
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct TrackingData
|
|
{
|
|
public Pose zedPathTransform;
|
|
public Pose zedWorldTransform;
|
|
public Pose offsetZedWorldTransform;
|
|
|
|
public int trackingState;
|
|
}
|
|
/// <summary>
|
|
/// Gameobject holding the left camera in the final ZEDRigDisplayer rig, which captures the final image sent to the left HMD screen.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public GameObject finalCameraLeft;
|
|
/// <summary>
|
|
/// GameObject holding the right camera in the final ZEDRigDisplayer rig, which captures the final image sent to the right HMD screen.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public GameObject finalCameraRight;
|
|
|
|
/// <summary>
|
|
/// 'Intermediate' left camera GameObject, which is the one on the regular, always-visible ZED stereo rig (ZED_Rig_Stereo),
|
|
/// usually called 'Left_eye'.
|
|
/// </summary>
|
|
[Tooltip("'Intermediate' left camera GameObject, which is the one on the regular, always-visible ZED stereo rig (ZED_Rig_Stereo), "
|
|
+ "usually called 'Left_eye'. ")]
|
|
public GameObject ZEDEyeLeft;
|
|
/// <summary>
|
|
/// 'Intermediate' right camera GameObject, which is the one on the regular, always-visible ZED stereo rig (ZED_Rig_Stereo),
|
|
/// usually called 'Right_eye'.
|
|
/// </summary>
|
|
[Tooltip("'Intermediate' right camera GameObject, which is the one on the regular, always-visible ZED stereo rig (ZED_Rig_Stereo)," +
|
|
"usually called 'Right_eye'. ")]
|
|
public GameObject ZEDEyeRight;
|
|
|
|
/// <summary>
|
|
/// 'Intermediate' left screen/canvas object in the always-visible ZED stereo rig.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public ZEDRenderingPlane leftScreen;
|
|
/// <summary>
|
|
/// 'Intermediate' right screen/canvas object in the always-visible ZED stereo rig.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public ZEDRenderingPlane rightScreen;
|
|
|
|
/// <summary>
|
|
/// Final left viewing plane/canvas object in the final ZEDRigDisplayer rig. Displays the image from the left
|
|
/// 'intermediate' camera (ZEDEyeLeft) and is offset for image comfort and moved each frame for the Timewarp effect.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public Transform quadLeft;
|
|
/// <summary>
|
|
/// Final right viewing plane/canvas object in the final ZEDRigDisplayer rig. Displays the image from the right
|
|
/// 'intermediate' camera (ZEDEyeRight) and is offset for image comfort and moved each frame for the Timewarp effect.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public Transform quadRight;
|
|
|
|
/// <summary>
|
|
/// Camera object in 'finalCameraLeft', which captures the final image output to the headset's left screen.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public Camera finalLeftEye;
|
|
/// <summary>
|
|
/// Camera object in 'finalCameraRight', which captures the final image output to the headset's right screen.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public Camera finalRightEye;
|
|
|
|
/// <summary>
|
|
/// Material from the final left plane. Usually a new instance of Mat_ZED_Unlit.
|
|
/// </summary>
|
|
[Tooltip("Material from the final left plane. Usually a new instance of Mat_ZED_Unlit. ")]
|
|
public Material leftMaterial;
|
|
|
|
/// <summary>
|
|
/// Material from the final right plane. Usually a new instance of Mat_ZED_Unlit.
|
|
/// </summary>
|
|
[Tooltip("Material from the final right plane. Usually a new instance of Mat_ZED_Unlit. ")]
|
|
public Material rightMaterial;
|
|
|
|
/// <summary>
|
|
/// Base, pre-Timewarp offset between each final plane and its corresponding camera.
|
|
/// </summary>
|
|
[Tooltip("Offset between each final plane and its corresponding camera.")]
|
|
public Vector3 offset = new Vector3(0, 0, (float)sl.Constant.PLANE_DISTANCE);
|
|
|
|
/// <summary>
|
|
/// Distance to set each intermediate camera from the point between them. This is half of the post-calibration
|
|
/// distance between the ZED cameras, so X is usually very close to 0.0315m (63mm / 2).
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
public Vector3 halfBaselineOffset;
|
|
|
|
/// <summary>
|
|
/// Reference to the ZEDCamera instance, which communicates with the SDK.
|
|
/// </summary>
|
|
[Tooltip("Reference to the ZEDCamera instance, which communicates with the SDK.")]
|
|
public sl.ZEDCamera zedCamera;
|
|
|
|
/// <summary>
|
|
/// Reference to the scene's ZEDManager instance, usually contained in ZED_Rig_Stereo.
|
|
/// </summary>
|
|
[Tooltip("Reference to the scene's ZEDManager instance, usually contained in ZED_Rig_Stereo.")]
|
|
public ZEDManager manager;
|
|
|
|
/// <summary>
|
|
/// Flag set to true when the target textures from the ZEDRenderingPlane overlays are ready.
|
|
/// </summary>
|
|
[Tooltip("Flag set to true when the target textures from the ZEDRenderingPlane overlays are ready.")]
|
|
public bool ready = false;
|
|
|
|
/// <summary>
|
|
/// Flag set to true when a grab is ready, used to collect a pose from the latest time possible.
|
|
/// </summary>
|
|
[Tooltip("Flag set to true when a grab is ready, used to collect a pose from the latest time possible.")]
|
|
public bool grabSucceeded = false;
|
|
|
|
/// <summary>
|
|
/// Flag set to true when the ZED is ready (after ZEDManager.OnZEDReady is invoked).
|
|
/// </summary>
|
|
[Tooltip("Flag set to true when the ZED is ready (after ZEDManager.OnZEDReady is invoked).")]
|
|
public bool zedReady = false;
|
|
|
|
/// <summary>
|
|
/// If a VR device is still detected. Updated each frame. Used to know if certain updates should still happen.
|
|
/// </summary>
|
|
private bool hasVRDevice = false;
|
|
public bool HasVRDevice {
|
|
get { return hasVRDevice; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// The current latency pose - the pose the headset was at when the last ZED frame was captured (based on its timestamp).
|
|
/// </summary>
|
|
private Pose latencyPose;
|
|
|
|
/// <summary>
|
|
/// The physical offset of the HMD to the ZED. Represents the offset from the approximate center of the user's
|
|
/// head to the ZED's left sensor.
|
|
/// </summary>
|
|
private Pose hmdtozedCalibration;
|
|
|
|
/// <summary>
|
|
/// Public accessor for the physical offset of the HMD to the ZED. Represents the offset from the
|
|
/// approximate center of the user's head to the ZED's left sensor.
|
|
/// </summary>
|
|
public Pose HmdToZEDCalibration {
|
|
get { return hmdtozedCalibration; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Whether the latency correction is ready.
|
|
/// </summary>
|
|
private bool latencyCorrectionReady = false;
|
|
|
|
/// <summary>
|
|
/// Contains the last position computed by the anti-drift.
|
|
/// </summary>
|
|
public TrackingData trackingData = new TrackingData();
|
|
|
|
/// <summary>
|
|
/// Filename of the saved HMD to ZED calibration file loaded into hmdtozedCalibration.
|
|
/// //If it doesn't exist, it's created with hard-coded values.
|
|
/// </summary>
|
|
[Tooltip("")]
|
|
[SerializeField]
|
|
private string calibrationFile = "CalibrationZEDHMD.ini";
|
|
/// <summary>
|
|
/// Path of the saved HMD to ZED calibration file loaded into hmdtozedCalibration.
|
|
/// By default, corresponds to C:/ProgramData/Stereolabs/mr.
|
|
/// </summary>
|
|
private string calibrationFilePath = @"Stereolabs\mr";
|
|
|
|
/// <summary>
|
|
/// Delegate for the OnHMDCalibChanged event.
|
|
/// </summary>
|
|
public delegate void OnHmdCalibrationChanged();
|
|
/// <summary>
|
|
/// Event invoked if the calibration file that sets the physical ZED offset is changed at runtime.
|
|
/// Causes ZEDManger.CalibrationHasChanged() to get called, which re-initialized the ZED's position
|
|
/// with ZEDManager.AdjustZEDRigCameraPosition() at the next tracking update.
|
|
/// </summary>
|
|
public static event OnHmdCalibrationChanged OnHmdCalibChanged;
|
|
|
|
/// <summary>
|
|
/// Cached property id for _MainTex. use the mainTexID property instead.
|
|
/// </summary>
|
|
private int? _maintexid;
|
|
/// <summary>
|
|
/// Property id for _MainTex, which is the main texture from the ZED.
|
|
/// </summary>
|
|
private int mainTexID
|
|
{
|
|
get
|
|
{
|
|
if (_maintexid == null) _maintexid = Shader.PropertyToID("_MainTex");
|
|
return (int)_maintexid;
|
|
}
|
|
}
|
|
|
|
#if UNITY_2017_OR_NEWER
|
|
List<UnityEngine.VR.VRNodeState> nodes = new List<UnityEngine.VR.VRNodeState>();
|
|
|
|
UnityEngine.VR.VRNodeState nodeState = new UnityEngine.VR.VRNodeState();
|
|
#endif
|
|
private void Awake()
|
|
{
|
|
//Initialize the latency tracking only if a supported headset is detected.
|
|
//You can force it to work for unsupported headsets by implementing your own logic for calling
|
|
//dllz_latency_corrector_initialize.
|
|
hasVRDevice = UnityEngine.XR.XRDevice.isPresent;
|
|
if (hasVRDevice) {
|
|
if (UnityEngine.XR.XRDevice.model.ToLower().Contains ("vive")) //Vive or Vive Pro
|
|
dllz_latency_corrector_initialize (0);
|
|
else if (UnityEngine.XR.XRDevice.model.ToLower().Contains ("oculus") || //Oculus Rift
|
|
UnityEngine.XR.XRDevice.model.ToLower().Contains("windows") || //Most WMR headsets
|
|
UnityEngine.XR.XRDevice.model.ToLower().Contains("visor") || //Dell Visor
|
|
UnityEngine.XR.XRDevice.model.ToLower().Contains("explorer")) //Lenovo Explorer
|
|
dllz_latency_corrector_initialize (1);
|
|
else if (UnityEngine.XR.XRDevice.model.ToLower().Contains ("windows")) //Windows MR through SteamVR Only (Beta)
|
|
dllz_latency_corrector_initialize (1);
|
|
|
|
|
|
dllz_drift_corrector_initialize ();
|
|
}
|
|
#if UNITY_2017_OR_NEWER
|
|
|
|
nodeState.nodeType = VRNode.Head;
|
|
nodes.Add(nodeState);
|
|
#endif
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets references not set in ZEDManager.CreateZEDRigDisplayer(), sets materials,
|
|
/// adjusts final plane scale, loads the ZED calibration offset and other misc. values.
|
|
/// </summary>
|
|
void Start()
|
|
{
|
|
hasVRDevice = UnityEngine.XR.XRDevice.isPresent;
|
|
|
|
|
|
//iterate until we found the ZED Manager parent...
|
|
Transform ObjParent = gameObject.transform;
|
|
int tries = 0;
|
|
while (manager == null && tries < 50) {
|
|
if (ObjParent!=null)
|
|
manager= ObjParent.GetComponent<ZEDManager> ();
|
|
if (manager == null && ObjParent!=null)
|
|
ObjParent = ObjParent.parent;
|
|
tries++;
|
|
}
|
|
|
|
if (manager != null) {
|
|
manager.OnZEDReady += ZEDReady;
|
|
zedCamera = manager.zedCamera;
|
|
} else
|
|
return;
|
|
|
|
leftScreen = ZEDEyeLeft.GetComponent<ZEDRenderingPlane>();
|
|
rightScreen = ZEDEyeRight.GetComponent<ZEDRenderingPlane>();
|
|
finalLeftEye = finalCameraLeft.GetComponent<Camera>();
|
|
finalRightEye = finalCameraRight.GetComponent<Camera>();
|
|
|
|
rightMaterial = quadRight.GetComponent<Renderer>().material;
|
|
leftMaterial = quadLeft.GetComponent<Renderer>().material;
|
|
finalLeftEye.SetReplacementShader(leftMaterial.shader, "");
|
|
finalRightEye.SetReplacementShader(rightMaterial.shader, "");
|
|
|
|
float plane_dist = (float)sl.Constant.PLANE_DISTANCE;
|
|
scale(quadLeft.gameObject, new Vector2(1.78f*plane_dist, 1.0f*plane_dist));
|
|
scale(quadRight.gameObject, new Vector2(1.78f*plane_dist, 1.0f*plane_dist));
|
|
zedReady = false;
|
|
Camera.onPreRender += PreRender;
|
|
|
|
LoadHmdToZEDCalibration();
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the size of the final planes.
|
|
/// </summary>
|
|
/// <param name="resolution">ZED's current resolution. Usually 1280x720.</param>
|
|
/// <param name="perceptionDistance">Typically 1.</param>
|
|
/// <param name="eyeToZedDistance">Distance from your eye to the camera. Estimated at 0.1m.</param>
|
|
/// <param name="planeDistance">Distance to final quad (quadLeft or quadRight). Arbitrary but set by offset.z.</param>
|
|
/// <param name="HMDFocal">Focal length of the HMD, retrieved from the wrapper.</param>
|
|
/// <param name="zedFocal">Focal length of the ZED, retrieved from the camera's rectified calibration parameters.</param>
|
|
/// <returns></returns>
|
|
public Vector2 ComputeSizePlaneWithGamma(sl.Resolution resolution, float perceptionDistance, float eyeToZedDistance, float planeDistance, float HMDFocal, float zedFocal)
|
|
{
|
|
System.IntPtr p = dllz_compute_size_plane_with_gamma(resolution, perceptionDistance, eyeToZedDistance, planeDistance, HMDFocal, zedFocal);
|
|
|
|
if (p == System.IntPtr.Zero)
|
|
{
|
|
return new Vector2();
|
|
}
|
|
Vector2 parameters = (Vector2)Marshal.PtrToStructure(p, typeof(Vector2));
|
|
return parameters;
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compute the focal length of the HMD.
|
|
/// </summary>
|
|
/// <param name="targetSize">Resolution of the headset's eye textures.</param>
|
|
/// <returns></returns>
|
|
public float ComputeFocal(sl.Resolution targetSize)
|
|
{
|
|
float focal_hmd = dllz_compute_hmd_focal(targetSize, finalLeftEye.projectionMatrix.m00,finalLeftEye.projectionMatrix.m11);
|
|
return focal_hmd;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Called once the ZED is finished initializing. Subscribed to ZEDManager.OnZEDReady in OnEnable.
|
|
/// Uses the newly-available ZED parameters to scale the final planes (quadLeft and quadRight) to appear
|
|
/// properly in the currently-connected headset.
|
|
/// </summary>
|
|
void ZEDReady()
|
|
{
|
|
Vector2 scaleFromZED;
|
|
halfBaselineOffset.x = zedCamera.Baseline / 2.0f;
|
|
|
|
float perception_distance = 1.0f;
|
|
float zed2eye_distance = 0.1f; //Estimating 10cm between your eye and physical location of the ZED Mini.
|
|
hasVRDevice = UnityEngine.XR.XRDevice.isPresent;
|
|
|
|
if (hasVRDevice) {
|
|
sl.CalibrationParameters parameters = zedCamera.CalibrationParametersRectified;
|
|
|
|
scaleFromZED = ComputeSizePlaneWithGamma (new sl.Resolution ((uint)zedCamera.ImageWidth, (uint)zedCamera.ImageHeight),
|
|
perception_distance, zed2eye_distance, offset.z,
|
|
ComputeFocal (new sl.Resolution ((uint)UnityEngine.XR.XRSettings.eyeTextureWidth, (uint)UnityEngine.XR.XRSettings.eyeTextureHeight)),
|
|
parameters.leftCam.fx);
|
|
|
|
scale (quadLeft.gameObject, scaleFromZED);
|
|
scale (quadRight.gameObject, scaleFromZED);
|
|
}
|
|
ready = false;
|
|
|
|
// If using Vive, change ZED's settings to compensate for different screen.
|
|
if (UnityEngine.XR.XRDevice.model.ToLower().Contains ("vive")) {
|
|
zedCamera.SetCameraSettings (sl.CAMERA_SETTINGS.CONTRAST, 3);
|
|
zedCamera.SetCameraSettings (sl.CAMERA_SETTINGS.SATURATION, 3);
|
|
}
|
|
|
|
|
|
//Set eye layers to respective eyes. They were each set to Both during the loading screen to avoid one eye going blank at some rotations.
|
|
finalLeftEye.stereoTargetEye = StereoTargetEyeMask.Left;
|
|
finalRightEye.stereoTargetEye = StereoTargetEyeMask.Right;
|
|
|
|
/// AR Passtrough is recommended in 1280x720 at 60, due to FoV, FPS, etc.
|
|
/// If not set to this resolution, warn the user.
|
|
if (zedCamera.ImageWidth != 1280 && zedCamera.ImageHeight != 720)
|
|
Debug.LogWarning ("[ZED AR Passthrough] This resolution is not ideal for a proper AR passthrough experience. Recommended resolution is 1280x720.");
|
|
|
|
zedReady = true;
|
|
|
|
}
|
|
|
|
public void OnEnable()
|
|
{
|
|
latencyCorrectionReady = false;
|
|
if (manager !=null)
|
|
manager.OnZEDReady += ZEDReady;
|
|
}
|
|
|
|
public void OnDisable()
|
|
{
|
|
latencyCorrectionReady = false;
|
|
if (manager !=null)
|
|
manager.OnZEDReady -= ZEDReady;
|
|
}
|
|
|
|
void OnGrab()
|
|
{
|
|
grabSucceeded = true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Collects the position of the HMD with a timestamp, to be looked up later to correct for latency.
|
|
/// </summary>
|
|
public void CollectPose()
|
|
{
|
|
if (manager == null)
|
|
return;
|
|
|
|
KeyPose k = new KeyPose();
|
|
k.Orientation = UnityEngine.XR.InputTracking.GetLocalRotation(UnityEngine.XR.XRNode.Head);
|
|
k.Translation = UnityEngine.XR.InputTracking.GetLocalPosition(UnityEngine.XR.XRNode.Head);
|
|
if (manager.zedCamera.IsCameraReady)
|
|
{
|
|
k.Timestamp = manager.zedCamera.GetCurrentTimeStamp();
|
|
if (k.Timestamp >= 0)
|
|
{
|
|
dllz_latency_corrector_add_key_pose(ref k.Translation, ref k.Orientation, k.Timestamp); //Poses are handled by the wrapper.
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a pose at a specific time.
|
|
/// </summary>
|
|
/// <param name="r">Rotation of the latency pose.</param>
|
|
/// <param name="t">Translation/position of the latency pose.</param>
|
|
/// <param name="cameraTimeStamp">Timestamp for looking up the pose.</param>
|
|
/// <param name="useLatency">Whether to use latency.</param>
|
|
public int LatencyCorrector(out Quaternion r, out Vector3 t, ulong cameraTimeStamp, bool useLatency)
|
|
{
|
|
return dllz_latency_corrector_get_transform(cameraTimeStamp, useLatency, out t, out r);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets the GameObject's 3D local scale based on a 2D resolution (Z scale is unchanged).
|
|
/// Used for scaling quadLeft/quadRight.
|
|
/// </summary>
|
|
/// <param name="screen">Target GameObject to scale.</param>
|
|
/// <param name="s">2D scale factor.</param>
|
|
public void scale(GameObject screen, Vector2 s)
|
|
{
|
|
screen.transform.localScale = new Vector3(s.x, s.y, 1);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Set the planes/canvases to the proper position after accounting for latency.
|
|
/// </summary>
|
|
public void UpdateRenderPlane()
|
|
{
|
|
if (manager == null)
|
|
return;
|
|
|
|
if (!manager.IsStereoRig)
|
|
return; //Make sure we're in pass-through AR mode.
|
|
|
|
Quaternion r;
|
|
r = latencyPose.rotation;
|
|
|
|
//Plane's distance from the final camera never changes, but it's rotated around it based on the latency pose.
|
|
quadLeft.localRotation = r;
|
|
quadLeft.localPosition = finalLeftEye.transform.localPosition + r * (offset);
|
|
quadRight.localRotation = r;
|
|
quadRight.localPosition = finalRightEye.transform.localPosition + r * (offset);
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initialize the ZED's tracking with the current HMD position and HMD-ZED calibration.
|
|
/// This causes the ZED's internal tracking to start where the HMD is, despite being initialized later than the HMD.
|
|
/// </summary>
|
|
/// <returns>Initial offset for the ZED's tracking. </returns>
|
|
public Pose InitTrackingAR()
|
|
{
|
|
if (manager == null)
|
|
return new Pose ();
|
|
|
|
Transform tmpHMD = transform;
|
|
tmpHMD.position = UnityEngine.XR.InputTracking.GetLocalPosition(UnityEngine.XR.XRNode.Head);
|
|
tmpHMD.rotation = UnityEngine.XR.InputTracking.GetLocalRotation (UnityEngine.XR.XRNode.Head);
|
|
|
|
Quaternion r = Quaternion.identity;
|
|
Vector3 t = Vector3.zero;
|
|
Pose const_offset = new Pose(t, r);
|
|
dllz_drift_corrector_set_calibration_const_offset_transform(ref const_offset);
|
|
|
|
zedCamera.ResetTrackingWithOffset(tmpHMD.rotation,tmpHMD.position,HmdToZEDCalibration.rotation,HmdToZEDCalibration.translation);
|
|
|
|
return new Pose(tmpHMD.position, tmpHMD.rotation);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets latencyPose to the pose of the headset at a given timestamp and flags whether or not it's valid for use.
|
|
/// </summary>
|
|
/// <param name="cameraTimeStamp">Timestamp for looking up the pose.</param>
|
|
public void ExtractLatencyPose(ulong cameraTimeStamp)
|
|
{
|
|
Quaternion latency_rot;
|
|
Vector3 latency_pos;
|
|
if (LatencyCorrector (out latency_rot, out latency_pos, cameraTimeStamp, true) == 1) {
|
|
latencyPose = new Pose (latency_pos, latency_rot);
|
|
latencyCorrectionReady = true;
|
|
} else
|
|
latencyCorrectionReady = false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the most recently retrieved latency pose.
|
|
///
|
|
/// </summary>
|
|
/// <returns>Last retrieved latency pose. </returns>
|
|
public Pose LatencyPose()
|
|
{
|
|
return latencyPose;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the proper position of the ZED virtual camera, factoring in HMD offset, latency, and anti-drift.
|
|
/// Used by ZEDManager to set the pose of Camera_eyes in the 'intermediate' rig (ZED_Rig_Stereo).
|
|
/// </summary>
|
|
/// <param name="position">Current position as returned by the ZED's tracking.</param>
|
|
/// <param name="orientation">Current rotation as returned by the ZED's tracking.</param>
|
|
/// <param name="r">Final rotation.</param>
|
|
/// <param name="t">Final translation/position.</param>
|
|
public void AdjustTrackingAR(Vector3 position, Quaternion orientation, out Quaternion r, out Vector3 t, bool setimuprior)
|
|
{
|
|
hasVRDevice = UnityEngine.XR.XRDevice.isPresent;
|
|
|
|
Pose hmdTransform = new Pose(UnityEngine.XR.InputTracking.GetLocalPosition(UnityEngine.XR.XRNode.Head), UnityEngine.XR.InputTracking.GetLocalRotation(UnityEngine.XR.XRNode.Head)); //Current HMD position
|
|
trackingData.trackingState = (int)manager.ZEDTrackingState; //Whether the ZED's tracking is currently valid (not off or unable to localize).
|
|
trackingData.zedPathTransform = new Pose (position, orientation);
|
|
|
|
if (zedReady && latencyCorrectionReady && setimuprior == true) {
|
|
zedCamera.SetIMUOrientationPrior (ref latencyPose.rotation);
|
|
}
|
|
|
|
dllz_drift_corrector_get_tracking_data (ref trackingData, ref hmdTransform, ref latencyPose, 0, true);
|
|
r = trackingData.offsetZedWorldTransform.rotation;
|
|
t = trackingData.offsetZedWorldTransform.translation;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Close related ZED processes when the application ends.
|
|
/// </summary>
|
|
private void OnApplicationQuit()
|
|
{
|
|
dllz_latency_corrector_shutdown();
|
|
dllz_drift_corrector_shutdown();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Collects poses for latency correction, and updates the position of the rendering plane.
|
|
/// Also assigns textures from 'intermediate' cameras to the final quads' materials if ready and not done yet.
|
|
/// Called from ZEDManager.LateUpdate() so that it happens each frame after other tracking processes have finished.
|
|
/// </summary>
|
|
public void LateUpdateHmdRendering()
|
|
{
|
|
if (!ready) //Make sure intermediate cameras are rendering to the quad's materials.
|
|
{
|
|
if (leftScreen.target != null && leftScreen.target.IsCreated())
|
|
{
|
|
//leftMaterial.SetTexture("_MainTex", leftScreen.target);
|
|
leftMaterial.SetTexture(mainTexID, leftScreen.target);
|
|
ready = true;
|
|
}
|
|
else ready = false;
|
|
if (rightScreen.target != null && rightScreen.target.IsCreated())
|
|
{
|
|
rightMaterial.SetTexture(mainTexID, rightScreen.target);
|
|
ready = true;
|
|
}
|
|
else ready = false;
|
|
}
|
|
|
|
if (hasVRDevice) //Do nothing if we no longer have a HMD connected.
|
|
{
|
|
CollectPose (); //File the current HMD pose into the latency poses to reference later.
|
|
UpdateRenderPlane(); //Reposition the final quads based on the latency pose.
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Before the ZED is ready, lock the quads in front of the cameras as latency correction isn't available yet.
|
|
/// This allows us to see the loading messages (and other virtual objects if desired) while the ZED is still loading.
|
|
/// Called by Camera.OnPreRender anytime any camera renders.
|
|
/// </summary>
|
|
/// <param name="cam">Cam.</param>
|
|
public void PreRender(Camera cam)
|
|
{
|
|
if (cam == finalLeftEye || cam == finalRightEye)
|
|
{
|
|
|
|
if ((!manager.IsZEDReady && manager.IsStereoRig))
|
|
{
|
|
quadLeft.localRotation = UnityEngine.XR.InputTracking.GetLocalRotation(UnityEngine.XR.XRNode.Head);
|
|
quadLeft.localPosition = UnityEngine.XR.InputTracking.GetLocalPosition(UnityEngine.XR.XRNode.Head) + quadLeft.localRotation * offset;
|
|
|
|
quadRight.localRotation = UnityEngine.XR.InputTracking.GetLocalRotation(UnityEngine.XR.XRNode.Head);
|
|
quadRight.localPosition = UnityEngine.XR.InputTracking.GetLocalPosition(UnityEngine.XR.XRNode.Head) + quadRight.localRotation * offset;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Loads the HMD to ZED calibration file and applies it to the hmdtozedCalibration offset.
|
|
/// Note that the file it loads is created using hard-coded values
|
|
/// and the ZED plugin doesn't ever change it. See CreateDefaultCalibrationFile().
|
|
/// </summary>
|
|
public void LoadHmdToZEDCalibration()
|
|
{
|
|
if (hasVRDevice) {
|
|
/// Default calibration (may be changed)
|
|
hmdtozedCalibration.rotation = Quaternion.identity;
|
|
hmdtozedCalibration.translation.x = 0.0315f;//-zedCamera.Baseline/2;
|
|
hmdtozedCalibration.translation.y = 0.0f;
|
|
hmdtozedCalibration.translation.z = 0.11f;
|
|
|
|
|
|
//if a calibration exists then load it
|
|
//should be in ProgramData/stereolabs/mr/calibration.ini
|
|
string folder = System.Environment.GetFolderPath (System.Environment.SpecialFolder.CommonApplicationData);
|
|
string specificFolder = Path.Combine (folder, @"Stereolabs\mr");
|
|
calibrationFilePath = Path.Combine (specificFolder, calibrationFile);
|
|
|
|
|
|
// Check if folder exists and if not, create it
|
|
if (!Directory.Exists (specificFolder)) {
|
|
Directory.CreateDirectory (specificFolder);
|
|
}
|
|
|
|
// Check if file exist and if not, create a default one
|
|
if (!ParseCalibrationFile (calibrationFilePath))
|
|
CreateDefaultCalibrationFile (calibrationFilePath);
|
|
|
|
// Set the calibration in mr processing
|
|
dllz_drift_corrector_set_calibration_transform (ref hmdtozedCalibration);
|
|
|
|
// Create a file system watcher for online modifications
|
|
CreateFileWatcher (specificFolder);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a FileSystemEventHandler to watch the HMD-ZED calibration file and update settings if
|
|
/// it changes during runtime. If it does, calls OnChanged to fix tracking.
|
|
/// </summary>
|
|
/// <param name="folder"></param>
|
|
public void CreateFileWatcher(string folder)
|
|
{
|
|
// Create a new FileSystemWatcher and set its properties.
|
|
FileSystemWatcher watcher = new FileSystemWatcher();
|
|
watcher.Path = folder;
|
|
/* Watch for changes in LastAccess and LastWrite times, and
|
|
the renaming of files or directories. */
|
|
watcher.NotifyFilter = NotifyFilters.LastAccess | NotifyFilters.LastWrite
|
|
| NotifyFilters.FileName | NotifyFilters.DirectoryName;
|
|
// Only watch text files.
|
|
watcher.Filter = calibrationFile;
|
|
|
|
// Add event handlers.
|
|
watcher.Changed += new FileSystemEventHandler(OnChanged);
|
|
|
|
// Begin watching.
|
|
watcher.EnableRaisingEvents = true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reloads ZED-HMD offset calibration file and resets calibration accordintly.
|
|
/// Also calls OnHmdCalibChanged() which ZEDManager uses to run additional reset logic.
|
|
/// </summary>
|
|
/// <param name="source"></param>
|
|
/// <param name="e"></param>
|
|
private void OnChanged(object source, FileSystemEventArgs e)
|
|
{
|
|
if (hasVRDevice) {
|
|
ParseCalibrationFile (calibrationFilePath);
|
|
dllz_drift_corrector_set_calibration_transform (ref hmdtozedCalibration);
|
|
OnHmdCalibChanged ();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates and saves a text file with the default ZED-HMD offset calibration parameters, to be loaded anytime this class runs in the future.
|
|
/// Values correspond to the distance from the center of the user's head to the ZED's left sensor.
|
|
/// </summary>
|
|
/// <param name="path">Path to save the file.</param>
|
|
private void CreateDefaultCalibrationFile(string path)
|
|
{
|
|
//Default Calibration: DO NOT CHANGE.
|
|
hmdtozedCalibration.rotation = Quaternion.identity;
|
|
hmdtozedCalibration.translation.x = -0.0315f;
|
|
hmdtozedCalibration.translation.y = 0.0f;
|
|
hmdtozedCalibration.translation.z = 0.115f;
|
|
|
|
//Write calibration file using default calibration.
|
|
using (System.IO.StreamWriter file = new System.IO.StreamWriter (path)) {
|
|
string node = "[HMD]";
|
|
string tx = "tx=" + hmdtozedCalibration.translation.x.ToString (System.Globalization.CultureInfo.InvariantCulture) + " //Translation x";
|
|
string ty = "ty=" + hmdtozedCalibration.translation.y.ToString (System.Globalization.CultureInfo.InvariantCulture) + " //Translation y";
|
|
string tz = "tz=" + hmdtozedCalibration.translation.z.ToString (System.Globalization.CultureInfo.InvariantCulture) + " //Translation z";
|
|
string rx = "rx=" + hmdtozedCalibration.rotation.x.ToString (System.Globalization.CultureInfo.InvariantCulture) + " //Quaternion x";
|
|
string ry = "ry=" + hmdtozedCalibration.rotation.y.ToString (System.Globalization.CultureInfo.InvariantCulture) + " //Quaternion y";
|
|
string rz = "rz=" + hmdtozedCalibration.rotation.z.ToString (System.Globalization.CultureInfo.InvariantCulture) + " //Quaternion z";
|
|
string rw = "rw=" + hmdtozedCalibration.rotation.w.ToString (System.Globalization.CultureInfo.InvariantCulture) + " //Quaternion w";
|
|
|
|
file.WriteLine (node);
|
|
file.WriteLine (tx);
|
|
file.WriteLine (ty);
|
|
file.WriteLine (tz);
|
|
file.WriteLine (rx);
|
|
file.WriteLine (ry);
|
|
file.WriteLine (rz);
|
|
file.WriteLine (rw);
|
|
|
|
file.Close ();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reads the ZED-HMD offset calibration file, if it exists, and loads calibration values to be applied to the final cameras.
|
|
/// Values correspond to the distance from the center of the user's head to the ZED's left sensor.
|
|
/// </summary>
|
|
/// <param name="path">Path to save the file.</param>
|
|
/// <returns>False if the file couldn't be loaded, whether empty, non-existant, etc.</returns>
|
|
private bool ParseCalibrationFile(string path)
|
|
{
|
|
if (!System.IO.File.Exists(path)) return false;
|
|
|
|
string[] lines = null;
|
|
try
|
|
{
|
|
lines = System.IO.File.ReadAllLines(path);
|
|
}
|
|
catch (System.Exception)
|
|
{
|
|
return false;
|
|
}
|
|
if (lines.Length==0)
|
|
return false;
|
|
|
|
//Default to these values (which are the same ones put in the calibration file by default).
|
|
hmdtozedCalibration.rotation = Quaternion.identity;
|
|
hmdtozedCalibration.translation.x = -0.0315f;
|
|
hmdtozedCalibration.translation.y = 0.0f;
|
|
hmdtozedCalibration.translation.z = 0.115f;
|
|
|
|
foreach (string line in lines)
|
|
{
|
|
string[] splittedLine = line.Split('=');
|
|
|
|
if (splittedLine != null && splittedLine.Length >= 2)
|
|
{
|
|
string key = splittedLine[0];
|
|
string field = splittedLine[1].Split(' ')[0];
|
|
|
|
if (key == "tx")
|
|
{
|
|
hmdtozedCalibration.translation.x = float.Parse(field, System.Globalization.CultureInfo.InvariantCulture);
|
|
}
|
|
else if (key == "ty")
|
|
{
|
|
hmdtozedCalibration.translation.y = float.Parse(field, System.Globalization.CultureInfo.InvariantCulture);
|
|
}
|
|
else if (key == "tz")
|
|
{
|
|
hmdtozedCalibration.translation.z = float.Parse(field, System.Globalization.CultureInfo.InvariantCulture);
|
|
}
|
|
else if (key == "rx")
|
|
{
|
|
hmdtozedCalibration.rotation.x = float.Parse(field, System.Globalization.CultureInfo.InvariantCulture);
|
|
}
|
|
else if (key == "ry")
|
|
{
|
|
hmdtozedCalibration.rotation.y = float.Parse(field, System.Globalization.CultureInfo.InvariantCulture);
|
|
}
|
|
else if (key == "rz")
|
|
{
|
|
hmdtozedCalibration.rotation.z = float.Parse(field, System.Globalization.CultureInfo.InvariantCulture);
|
|
}
|
|
else if (key == "rw")
|
|
{
|
|
hmdtozedCalibration.rotation.w = float.Parse(field, System.Globalization.CultureInfo.InvariantCulture);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//Check if the calibration has values but they're all zeros.
|
|
if (hmdtozedCalibration.translation.x == 0.0f && hmdtozedCalibration.translation.y == 0.0f && hmdtozedCalibration.translation.z == 0.0f) {
|
|
CreateDefaultCalibrationFile (path);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
}
|