private CalculatedCameraExtrinsics CreateExtrinsicsFromArray(float[] extrinsicsArray, int offset = 0)
{
bool succeeded = (extrinsicsArray[offset + 0] > 0.01f);
Vector3 rightHandedPosition = new Vector3(extrinsicsArray[offset + 1], extrinsicsArray[offset + 2], extrinsicsArray[offset + 3]);
Vector3 rodriguesVector = new Vector3(extrinsicsArray[offset + 4], extrinsicsArray[offset + 5], extrinsicsArray[offset + 6]);
var angle = Mathf.Rad2Deg * rodriguesVector.magnitude;
var axis = rodriguesVector.normalized;
Quaternion rightHandedRotation = Quaternion.AngleAxis(angle, axis);
var rightHandedMatrix = Matrix4x4.TRS(rightHandedPosition, rightHandedRotation, Vector3.one);
var leftHandedMatrix = rightHandedMatrix;
leftHandedMatrix.m02 *= -1.0f;
leftHandedMatrix.m12 *= -1.0f;
leftHandedMatrix.m22 *= -1.0f;
leftHandedMatrix.m32 *= -1.0f;
var inverse = leftHandedMatrix.inverse;
var inversePosition = inverse.GetColumn(3);
var inverseRotation = Quaternion.LookRotation(inverse.GetColumn(2), inverse.GetColumn(1)) * Quaternion.Euler(0, 0, 180);
var calcExtrinsics = new CalculatedCameraExtrinsics();
calcExtrinsics.ViewFromWorld = Matrix4x4.TRS(inversePosition, inverseRotation, Vector3.one);
calcExtrinsics.Succeeded = succeeded;
return(calcExtrinsics);
}
public static CalculatedCameraExtrinsics LoadCameraExtrinsics(string path)
{
if (File.Exists(path))
{
var fileData = File.ReadAllBytes(path);
if (CalculatedCameraExtrinsics.TryDeserialize(fileData, out var extrinsics))
{
return(extrinsics);
}
}
Debug.LogError($"Failed to load camera extrinsics file {path}");
return(null);
}
public static string SaveCameraExtrinsics(CalculatedCameraExtrinsics extrinsics)
{
string path = Path.Combine(GetDocumentsFolderPath(), RootDirectoryName, $"CameraExtrinsics.json");
int i = 0;
while (File.Exists(path))
{
path = Path.Combine(GetDocumentsFolderPath(), RootDirectoryName, $"CameraExtrinsics_{i}.json");
i++;
}
var payload = extrinsics.Serialize();
File.WriteAllBytes(path, payload);
return(path);
}
public static bool TryDeserialize(byte[] payload, out CalculatedCameraExtrinsics extrinsics)
{
extrinsics = null;
try
{
var str = Encoding.UTF8.GetString(payload);
extrinsics = JsonUtility.FromJson <CalculatedCameraExtrinsics>(str);
return(true);
}
catch (Exception e)
{
Debug.LogWarning($"Exception thrown deserializing camera extrinsics: {e.ToString()}");
return(false);
}
}
public CalibrationData(string cameraIntrinsicsPath, string cameraExtrinsicsPath)
{
if (File.Exists(cameraIntrinsicsPath) &&
File.Exists(cameraExtrinsicsPath))
{
cameraIntrinsics = CalibrationDataHelper.LoadCameraIntrinsics(cameraIntrinsicsPath);
cameraExtrinsics = CalibrationDataHelper.LoadCameraExtrinsics(cameraExtrinsicsPath);
if (cameraIntrinsics == null ||
cameraExtrinsics == null)
{
Debug.LogError($"Failed to load cameara intrinsics/extrinscs: {cameraIntrinsicsPath}, {cameraExtrinsicsPath}");
}
}
else
{
Debug.LogError($"Invalid paths provided for camera intrinsics/extrinsics: {cameraIntrinsicsPath}, {cameraExtrinsicsPath}");
}
}
/// <summary>
/// Call to calculate camera extrinsics based on all obtained and usable datasets.
/// </summary>
public void CalculateExtrinsics()
{
if (processedDatasetCount > 0)
{
Debug.Log("Starting Individual Camera Extrinsics calculations.");
cameraExtrinsics = CalibrationAPI.Instance.CalculateIndividualArUcoExtrinsics(dslrIntrinsics, parentVisuals.Count);
if (cameraExtrinsics != null)
{
CreateExtrinsicsVisual(cameraExtrinsics);
}
Debug.Log("Completed Individual Camera Extrinsics calculations.");
Debug.Log("Starting the Global Camera Extrinsics calculation.");
globalExtrinsics = CalibrationAPI.Instance.CalculateGlobalArUcoExtrinsics(dslrIntrinsics);
if (globalExtrinsics != null)
{
globalExtrinsicsFileName = CalibrationDataHelper.SaveCameraExtrinsics(globalExtrinsics);
Debug.Log($"Saved global extrinsics: {globalExtrinsicsFileName}");
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";
lastCalibration = new CalculatedCameraCalibration(dslrIntrinsics, globalExtrinsics);
calibrationFileName = CalibrationDataHelper.SaveCameraCalibration(lastCalibration);
}
}
else
{
Debug.LogWarning("No usable marker datasets have been processed, unable to calculate camera extrinsics.");
}
}
public CalibrationData(CalculatedCameraIntrinsics intrinsics, CalculatedCameraExtrinsics extrinsics)
{
cameraIntrinsics = intrinsics;
cameraExtrinsics = extrinsics;
}
public CalculatedCameraCalibration(CalculatedCameraIntrinsics intrinsics, CalculatedCameraExtrinsics extrinsics)
{
this.intrinsics = intrinsics;
this.extrinsics = extrinsics;
}
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";
}
}
}
