private bool ProcessArUcoData(HeadsetCalibrationData headsetData, Texture2D dslrTexture)
{
if (dslrTexture == null ||
dslrTexture.format != TextureFormat.RGB24)
{
return(false);
}
int imageWidth = dslrTexture.width;
int imageHeight = dslrTexture.height;
var unityPixels = dslrTexture.GetRawTextureData <byte>();
var pixels = unityPixels.ToArray();
if (!CalibrationAPI.Instance.ProcessArUcoData(headsetData, pixels, imageWidth, imageHeight))
{
return(false);
}
for (int i = 0; i < unityPixels.Length; i++)
{
unityPixels[i] = pixels[i];
}
dslrTexture.Apply();
if (lastArUcoImage)
{
lastArUcoImage.texture = dslrTexture;
}
return(true);
}
private void OnHeadsetCalibrationUpdated(byte[] data)
{
if (HeadsetCalibrationData.TryDeserialize(data, out var headsetCalibrationData))
{
this.headsetData = headsetCalibrationData;
}
}
private void OnCalibrationDataReceived(SocketEndpoint endpoint, string command, BinaryReader reader, int remainingDataSize)
{
Debug.Log("Received calibration data payload.");
HeadsetCalibrationData headsetCalibrationData;
if (HeadsetCalibrationData.TryDeserialize(reader, out headsetCalibrationData))
{
headsetData = headsetCalibrationData;
}
}
private void OnDataReceived(string playerId, byte[] payload)
{
Debug.Log($"Received payload of {payload.Length} bytes");
HeadsetCalibrationData headsetCalibrationData;
if (HeadsetCalibrationData.TryDeserialize(payload, out headsetCalibrationData))
{
headsetData = headsetCalibrationData;
}
}
private void CreateVisual(HeadsetCalibrationData data, string fileName)
{
var parent = new GameObject();
parent.name = $"Dataset {fileName}";
var inScene = new GameObject();
inScene.name = $"Objects in scene position";
inScene.transform.parent = parent.transform;
for (int i = 0; i < data.markers.Count; i++)
{
GameObject temp = null;
var corners = data.markers[i].arucoMarkerCorners;
float dist = Vector3.Distance(corners.topLeft, corners.topRight);
markerVisualHelper.CreateOrUpdateVisual(ref temp, corners.topLeft, corners.orientation, dist * Vector3.one);
temp.name = $"Marker {fileName}.{data.markers[i].id}";
temp.transform.parent = inScene.transform;
}
GameObject camera = null;
cameraVisualHelper.CreateOrUpdateVisual(ref camera, data.headsetData.position, data.headsetData.rotation);
camera.name = $"HoloLens {fileName}";
camera.transform.parent = inScene.transform;
var origin = new GameObject();
origin.name = $"Objects adjusted to origin";
origin.transform.parent = parent.transform;
GameObject originCamera = null;
cameraVisualHelper.CreateOrUpdateVisual(ref originCamera, Vector3.zero, Quaternion.identity);
originCamera.name = $"HoloLens {fileName}";
originCamera.transform.parent = origin.transform;
var allCorners = CalibrationAPI.CalcMarkerCornersRelativeToCamera(data);
for (int i = 0; i < allCorners.Count; i++)
{
GameObject temp = null;
var corners = allCorners[i];
float dist = Vector3.Distance(corners.topLeft, corners.topRight);
markerVisualHelper.CreateOrUpdateVisual(ref temp, corners.topLeft, corners.orientation, dist * Vector3.one);
temp.name = $"Marker {fileName}.{data.markers[i].id}";
temp.transform.parent = origin.transform;
}
parentVisuals.Add(parent);
}
public static HeadsetCalibrationData LoadHeadsetData(string filename)
{
string path = Path.Combine(GetDocumentsFolderPath(), RootDirectoryName, HeadsetDataDirectory, $"{filename}.json");
if (File.Exists(path))
{
var fileData = File.ReadAllBytes(path);
if (HeadsetCalibrationData.TryDeserialize(fileData, out var calibrationData))
{
return(calibrationData);
}
}
return(null);
}
public static bool TryDeserialize(byte[] payload, out HeadsetCalibrationData headsetCalibrationData)
{
headsetCalibrationData = null;
try
{
var str = Encoding.UTF8.GetString(payload);
headsetCalibrationData = JsonUtility.FromJson <HeadsetCalibrationData>(str);
return(true);
}
catch
{
return(false);
}
}
/// <summary>
/// Call to try and create a HeadsetCalibrationData instance from a binary reader.
/// </summary>
/// <param name="reader">Binary reader to obtain serialized data from</param>
/// <param name="headsetCalibrationData">output headset calibration data</param>
/// <returns>Returns true if the provided binary reader could be used to create headset calibration data, otherwise false.</returns>
public static bool TryDeserialize(BinaryReader reader, out HeadsetCalibrationData headsetCalibrationData)
{
headsetCalibrationData = null;
var str = reader.ReadString();
try
{
headsetCalibrationData = JsonUtility.FromJson <HeadsetCalibrationData>(str);
return(true);
}
catch (Exception e)
{
Debug.LogError($"Exception thrown: {e}");
return(false);
}
}
/// <summary>
/// Called to try and create a HeadsetCalibrationData instance from a byte array.
/// </summary>
/// <param name="payload">byte array to deserialize</param>
/// <param name="headsetCalibrationData">output headset calibration data</param>
/// <returns>Returns true if the provided payload could be converted into headset calibration data, otherwise false.</returns>
public static bool TryDeserialize(byte[] payload, out HeadsetCalibrationData headsetCalibrationData)
{
headsetCalibrationData = null;
try
{
var str = Encoding.UTF8.GetString(payload);
headsetCalibrationData = JsonUtility.FromJson <HeadsetCalibrationData>(str);
return(true);
}
catch (Exception e)
{
Debug.LogError($"Exception thrown: {e}");
return(false);
}
}
private void OnHeadsetCalibrationUpdated(HeadsetCalibrationData data)
{
if (holographicCameraBroadcaster != null)
{
using (MemoryStream memoryStream = new MemoryStream())
using (BinaryWriter writer = new BinaryWriter(memoryStream))
{
writer.Write(HeadsetCalibration.CalibrationDataReceivedCommandHeader);
data.SerializeAndWrite(writer);
writer.Flush();
Debug.Log("Sending headset calibration data payload.");
connectionManager.Broadcast(memoryStream.ToArray());
}
}
}
private void Update()
{
if (feedImage != null &&
feedImage.texture == null)
{
feedImage.texture = CompositorWrapper.Instance.GetVideoCameraFeed();
}
if (Input.GetKeyDown(KeyCode.Space))
{
RequestHeadsetData();
}
if (headsetData != null)
{
lastDetectedMarkersCount = headsetData.markers.Count;
if (headsetData.markers.Count < MinimumNumberOfDetectedMarkers)
{
Debug.Log("Data set did not contain enough markers to use.");
}
else
{
var dslrTexture = CompositorWrapper.Instance.GetVideoCameraTexture();
var fileName = CalibrationDataHelper.GetUniqueFileName();
CalibrationDataHelper.SaveDSLRArUcoImage(dslrTexture, fileName);
CalibrationDataHelper.SaveHeadsetData(headsetData, fileName);
if (ProcessArUcoData(headsetData, dslrTexture))
{
processedDatasetCount++;
CalibrationDataHelper.SaveDSLRArUcoDetectedImage(dslrTexture, fileName);
CreateVisual(headsetData, fileName);
}
}
headsetData = null;
}
if (Input.GetKeyDown(KeyCode.Return))
{
CalculateExtrinsics();
}
}
/// <summary>
/// Call to signal to the HeadsetCalibration class that it should create a new marker payload
/// </summary>
public void UpdateHeadsetCalibrationData()
{
Debug.Log("Updating headset calibration data");
var data = new HeadsetCalibrationData();
data.timestamp = Time.time;
data.headsetData.position = Camera.main.transform.position;
data.headsetData.rotation = Camera.main.transform.rotation;
data.markers = new List <MarkerPair>();
foreach (var detectedMarkerPair in markers)
{
if (markerPairs.ContainsKey(detectedMarkerPair.Key))
{
var markerPair = markerPairs[detectedMarkerPair.Key];
data.markers.Add(markerPair);
}
}
sendQueue.Enqueue(data);
}
/// <summary>
/// Creates a list of marker pairs that are corrected for the unity camera location specified in the headset calibration data.
/// </summary>
/// <param name="data">Headset calibration data</param>
/// <returns>Marker corners transformed to correct for the unity camera orientation</returns>
public static List <MarkerCorners> CalcMarkerCornersRelativeToCamera(HeadsetCalibrationData data)
{
List <MarkerCorners> markersRelativeToCamera = new List <MarkerCorners>();
var cameraTransform = Matrix4x4.TRS(data.headsetData.position, data.headsetData.rotation, Vector3.one);
var inverseCameraTransform = cameraTransform.inverse;
foreach (var markerPair in data.markers)
{
var arUcoCorners = markerPair.arucoMarkerCorners;
var cornersRelativeToCamera = new MarkerCorners();
cornersRelativeToCamera.topLeft = inverseCameraTransform.MultiplyPoint(arUcoCorners.topLeft);
cornersRelativeToCamera.topRight = inverseCameraTransform.MultiplyPoint(arUcoCorners.topRight);
cornersRelativeToCamera.bottomRight = inverseCameraTransform.MultiplyPoint(arUcoCorners.bottomRight);
cornersRelativeToCamera.bottomLeft = inverseCameraTransform.MultiplyPoint(arUcoCorners.bottomLeft);
var orientationTransform = inverseCameraTransform * Matrix4x4.TRS(Vector3.zero, arUcoCorners.orientation, Vector3.one);
cornersRelativeToCamera.orientation = Quaternion.LookRotation(orientationTransform.GetColumn(2), orientationTransform.GetColumn(1));
markersRelativeToCamera.Add(cornersRelativeToCamera);
}
return(markersRelativeToCamera);
}
private void CreateVisual(HeadsetCalibrationData data, string fileName)
{
var parent = new GameObject();
parent.name = $"Dataset {fileName}";
for (int i = 0; i < data.markers.Count; i++)
{
GameObject temp = null;
var corners = data.markers[i].arucoMarkerCorners;
float dist = Vector3.Distance(corners.topLeft, corners.topRight);
markerVisualHelper.CreateOrUpdateVisual(ref temp, corners.topLeft, corners.orientation, dist * Vector3.one);
temp.name = $"Marker {fileName}.{data.markers[i].id}";
temp.transform.parent = parent.transform;
}
GameObject camera = null;
cameraVisualHelper.CreateOrUpdateVisual(ref camera, data.headsetData.position, data.headsetData.rotation);
camera.name = $"HoloLens {fileName}";
camera.transform.parent = parent.transform;
parentVisuals.Add(parent);
}
private void Update()
{
if (feedImage != null &&
feedImage.texture == null)
{
feedImage.texture = CompositorWrapper.Instance.GetVideoCameraFeed();
}
if (Input.GetKeyDown(KeyCode.Space))
{
if (networkingService != null &&
matchMakingService != null)
{
var request = new HeadsetCalibrationDataRequest();
request.timestamp = Time.time;
var payload = request.Serialize();
if (networkingService.SendData(payload, NetworkPriority.Critical))
{
Debug.Log($"Sent headset calibration data request to HoloLens at {request.timestamp}");
}
else
{
Debug.LogWarning("Failed to send headset calibration data request to HoloLens");
}
}
if (headsetCalibration != null)
{
Debug.Log("Requesting headset calibration data from VR Headset");
headsetCalibration.UpdateHeadsetCalibrationData();
}
}
if (headsetData != null)
{
if (headsetData.markers.Count != expectedNumberOfMarkers)
{
Debug.Log("Headset has not yet detected all of the markers on the calibration board, dropping payload from headset.");
}
else
{
var dslrTexture = CompositorWrapper.Instance.GetVideoCameraTexture();
var fileName = CalibrationDataHelper.GetUniqueFileName();
CalibrationDataHelper.SaveDSLRArUcoImage(dslrTexture, fileName);
CalibrationDataHelper.SaveHeadsetData(headsetData, fileName);
if (ProcessArUcoData(headsetData, dslrTexture))
{
CalibrationDataHelper.SaveDSLRArUcoDetectedImage(dslrTexture, fileName);
CreateVisual(headsetData, fileName);
}
}
headsetData = null;
}
if (Input.GetKeyDown(KeyCode.Return))
{
Debug.Log("Starting Individual Camera Extrinsics calculations.");
cameraExtrinsics = CalibrationAPI.Instance.CalculateIndividualArUcoExtrinsics(dslrIntrinsics, parentVisuals.Count);
if (cameraExtrinsics != null)
{
foreach (var extrinsic in cameraExtrinsics)
{
Debug.Log($"Calculated extrinsics: {extrinsic}");
}
CreateExtrinsicsVisual(cameraExtrinsics);
}
Debug.Log("Starting the Global Camera Extrinsics calculation.");
globalExtrinsics = CalibrationAPI.Instance.CalculateGlobalArUcoExtrinsics(dslrIntrinsics);
if (globalExtrinsics != null)
{
var fileName = CalibrationDataHelper.SaveCameraExtrinsics(globalExtrinsics);
Debug.Log($"Saved global extrinsics: {fileName}");
Debug.Log($"Found global extrinsics: {globalExtrinsics}");
var position = globalExtrinsics.ViewFromWorld.GetColumn(3);
var rotation = Quaternion.LookRotation(globalExtrinsics.ViewFromWorld.GetColumn(2), globalExtrinsics.ViewFromWorld.GetColumn(1));
GameObject camera = null;
cameraVisualHelper.CreateOrUpdateVisual(ref camera, position, rotation);
camera.name = "Global Extrinsics";
GameObject hololens = null;
cameraVisualHelper.CreateOrUpdateVisual(ref hololens, Vector3.zero, Quaternion.identity);
hololens.name = "Global HoloLens";
}
}
}
/// <summary>
/// Processes an image containing ArUco markers cooresponding with world space coordinates provided in the headset calibraiton data.
/// </summary>
/// <param name="data">Headset calibration data</param>
/// <param name="dslrImage">RGB24 dslr image data</param>
/// <param name="imageWidth">Image width in pixels</param>
/// <param name="imageHeight">Image height in pixels</param>
/// <returns>Returns true if ArUco markers were found in the image that had valid world space coordinates in the provided headset calibration data.</returns>
public bool ProcessArUcoData(HeadsetCalibrationData data, byte[] dslrImage, int imageWidth, int imageHeight)
{
if (!initialized)
{
Debug.LogWarning("Calibration data wasn't processed. CalibrationPlugin dll failed to initialize for calibration");
return(false);
}
try
{
float[] orientation = new float[7];
orientation[0] = data.headsetData.position.x;
orientation[1] = data.headsetData.position.y;
orientation[2] = -1.0f * data.headsetData.position.z;
orientation[3] = data.headsetData.rotation.y;
orientation[4] = -1.0f * data.headsetData.rotation.z;
orientation[5] = -1.0f * data.headsetData.rotation.x;
orientation[6] = data.headsetData.rotation.w;
List <int> markerIds = new List <int>();
List <float> markerCorners = new List <float>();
foreach (var markerPair in data.markers)
{
markerIds.Add(markerPair.id);
markerCorners.Add(markerPair.qrCodeMarkerCorners.topLeft.x);
markerCorners.Add(markerPair.qrCodeMarkerCorners.topLeft.y);
markerCorners.Add(-1.0f * markerPair.qrCodeMarkerCorners.topLeft.z);
markerCorners.Add(markerPair.qrCodeMarkerCorners.topRight.x);
markerCorners.Add(markerPair.qrCodeMarkerCorners.topRight.y);
markerCorners.Add(-1.0f * markerPair.qrCodeMarkerCorners.topRight.z);
markerCorners.Add(markerPair.qrCodeMarkerCorners.bottomRight.x);
markerCorners.Add(markerPair.qrCodeMarkerCorners.bottomRight.y);
markerCorners.Add(-1.0f * markerPair.qrCodeMarkerCorners.bottomRight.z);
markerCorners.Add(markerPair.qrCodeMarkerCorners.bottomLeft.x);
markerCorners.Add(markerPair.qrCodeMarkerCorners.bottomLeft.y);
markerCorners.Add(-1.0f * markerPair.qrCodeMarkerCorners.bottomLeft.z);
}
List <MarkerCorners> markersRelativeToCamera = CalcMarkerCornersRelativeToCamera(data);
List <float> markerCornersRelativeToCamera = new List <float>();
foreach (var corners in markersRelativeToCamera)
{
markerCornersRelativeToCamera.Add(corners.topLeft.x);
markerCornersRelativeToCamera.Add(corners.topLeft.y);
markerCornersRelativeToCamera.Add(-1.0f * corners.topLeft.z);
markerCornersRelativeToCamera.Add(corners.topRight.x);
markerCornersRelativeToCamera.Add(corners.topRight.y);
markerCornersRelativeToCamera.Add(-1.0f * corners.topRight.z);
markerCornersRelativeToCamera.Add(corners.bottomRight.x);
markerCornersRelativeToCamera.Add(corners.bottomRight.y);
markerCornersRelativeToCamera.Add(-1.0f * corners.bottomRight.z);
markerCornersRelativeToCamera.Add(corners.bottomLeft.x);
markerCornersRelativeToCamera.Add(corners.bottomLeft.y);
markerCornersRelativeToCamera.Add(-1.0f * corners.bottomLeft.z);
}
if (!ProcessArUcoImageNative(
dslrImage,
imageWidth,
imageHeight,
markerIds.ToArray(),
markerIds.Count,
markerCorners.ToArray(),
markerCornersRelativeToCamera.ToArray(),
markerCorners.Count))
{
PrintLastError();
return(false);
}
return(true);
}
catch
{
PrintLastError();
}
return(false);
}
public static void SaveHeadsetData(HeadsetCalibrationData data, string filename)
{
string path = Path.Combine(GetDocumentsFolderPath(), RootDirectoryName, HeadsetDataDirectory, $"{filename}.json");
File.WriteAllBytes(path, data.Serialize());
}
private void SendHeadsetCalibrationDataPayload(HeadsetCalibrationData data)
{
byte[] payload = data.Serialize();
Updated?.Invoke(payload);
}