private void OnStartStreaming(PipelineProfile obj)
{
// var ds = obj.Streams.FirstOrDefault(s => s.Stream == Stream.Depth) as VideoStreamProfile;
var ds = obj.GetStream <VideoStreamProfile>(Stream.Depth, -1);
if (ds != null)
{
depthIntrinsics = ds.GetIntrinsics();
OnDepthCalibrationInit.Invoke(depthIntrinsics);
}
// var cs = obj.Streams.FirstOrDefault(s => s.Stream == Stream.Color) as VideoStreamProfile;
var cs = obj.GetStream <VideoStreamProfile>(Stream.Color, -1);
if (cs != null)
{
colorIntrinsics = cs.GetIntrinsics();
OnColorCalibrationInit.Invoke(colorIntrinsics);
}
if (ds != null && cs != null)
{
depthToColorExtrinsics = ds.GetExtrinsicsTo(cs);
OnDepthToColorCalibrationInit.Invoke(depthToColorExtrinsics);
}
// Unity camera FOV alignment with color intrinsics
if (targetCamera)
{
targetCamera.fieldOfView = Mathf.Rad2Deg * 2 * Mathf.Atan2(colorIntrinsics.height / 2.0f, colorIntrinsics.fy);
}
}
// gets the given camera extrinsics
private void GetCameraExtrinsics(Intel.RealSense.Extrinsics camExtr, ref KinectInterop.CameraExtrinsics extr)
{
extr = new KinectInterop.CameraExtrinsics();
extr.rotation = new float[camExtr.rotation.Length];
camExtr.rotation.CopyTo(extr.rotation, 0);
extr.translation = new float[camExtr.translation.Length];
camExtr.translation.CopyTo(extr.translation, 0);
}
internal static extern void rs2_get_extrinsics(IntPtr from, IntPtr to, out Extrinsics extrin, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(Helpers.ErrorMarshaler))] out object error);
public void RegisterExtrinsicsTo(StreamProfile other, Extrinsics extrinsics)
{
object error;
NativeMethods.rs2_register_extrinsics(Handle, other.Handle, extrinsics, out error);
}
public override bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager, bool isPlayMode)
{
base.UpdateSensorData(sensorData, kinectManager, isPlayMode);
if (sensorData.colorCamIntr == null && colorStreamProfile != null)
{
lock (colorFrameLock)
{
Intel.RealSense.Intrinsics colorCamIntr = colorStreamProfile.GetIntrinsics();
if (colorCamIntr.model != Distortion.None)
{
GetCameraIntrinsics(colorCamIntr, ref sensorData.colorCamIntr, 1);
}
}
}
if (sensorData.depthCamIntr == null && depthStreamProfile != null)
{
lock (depthFrameLock)
{
Intel.RealSense.Intrinsics depthCamIntr = depthStreamProfile.GetIntrinsics();
//Debug.Log("RS distType: " + depthCamIntr.model);
if (depthCamIntr.model != Distortion.None || deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
{
GetCameraIntrinsics(depthCamIntr, ref sensorData.depthCamIntr, 0);
if (depthCamIntr.model == Distortion.None && deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
{
// workaround for playback mode (model & coeffs are missing there)
sensorData.depthCamIntr.distType = KinectInterop.DistortionType.BrownConrady;
}
// body & body-index data
if ((frameSourceFlags & KinectInterop.FrameSource.TypeBody) != 0)
{
Debug.LogWarning("Body tracking is not supported for RealSense sensors!");
//if(depthCamIntr.width == 640 && depthCamIntr.height == 480)
//{
// Calibration cal = GetBodyTrackerCalibration(sensorData.depthCamIntr);
// InitBodyTracking(frameSourceFlags, sensorData, cal, true);
// if (isSyncBodyAndDepth && bodyTracker == null)
// {
// // don't sync body and depth if the body tracker can't be created
// isSyncBodyAndDepth = false;
// }
//}
}
}
}
}
if (sensorData.depth2ColorExtr == null && depthStreamProfile != null && colorStreamProfile != null)
{
lock (depthFrameLock)
{
lock (colorFrameLock)
{
Intel.RealSense.Extrinsics depth2ColorExtr = depthStreamProfile.GetExtrinsicsTo(colorStreamProfile);
GetCameraExtrinsics(depth2ColorExtr, ref sensorData.depth2ColorExtr);
}
}
}
if (sensorData.color2DepthExtr == null && colorStreamProfile != null && depthStreamProfile != null)
{
lock (colorFrameLock)
{
lock (depthFrameLock)
{
Intel.RealSense.Extrinsics color2DepthExtr = colorStreamProfile.GetExtrinsicsTo(depthStreamProfile);
GetCameraExtrinsics(color2DepthExtr, ref sensorData.color2DepthExtr);
}
}
}
return(true);
}