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);
}
}
// Callback for calibration params
public void OnColorCalibrationInit(Intel.RealSense.Intrinsics intrinsic)
{
depthIntrinsics = intrinsic;
depthIntrinsics.fx = intrinsic.fx / intrinsic.width;
depthIntrinsics.fy = intrinsic.fy / intrinsic.height;
depthIntrinsics.ppx = intrinsic.ppx / intrinsic.width;
depthIntrinsics.ppy = intrinsic.ppy / intrinsic.height;
}
// gets the given camera intrinsics
private void GetCameraIntrinsics(Intel.RealSense.Intrinsics camIntr, ref KinectInterop.CameraIntrinsics intr, int camType)
{
intr = new KinectInterop.CameraIntrinsics();
intr.cameraType = camType;
intr.width = camIntr.width;
intr.height = camIntr.height;
intr.ppx = camIntr.ppx;
intr.ppy = camIntr.ppy;
intr.fx = camIntr.fx;
intr.fy = camIntr.fy;
intr.distCoeffs = new float[camIntr.coeffs.Length];
camIntr.coeffs.CopyTo(intr.distCoeffs, 0);
intr.distType = (KinectInterop.DistortionType)camIntr.model;
EstimateFOV(intr);
}
internal static extern void rs2_get_video_stream_intrinsics(IntPtr from, out Intrinsics intrinsics, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(Helpers.ErrorMarshaler))] out object error);
/// <summary>
/// Clone current profile and change the type, index and format to input parameters
/// </summary>
/// <param name="type">will change the stream type from the cloned profile.</param>
/// <param name="index">will change the stream index from the cloned profile.</param>
/// <param name="format">will change the stream format from the cloned profile.</param>
/// <param name="width">will change the width of the profile.</param>
/// <param name="height">will change the height of the profile.</param>
/// <param name="intr">will change the intrinsics of the profile.</param>
/// <returns>the cloned stream profile.</returns>
public StreamProfile Clone(Stream type, int index, Format format, int width, int height, Intrinsics intr)
{
object error;
var ptr = NativeMethods.rs2_clone_video_stream_profile(Handle, type, index, format, width, height, intr, out error);
var p = StreamProfile.Create <VideoStreamProfile>(ptr);
p.clone = new Base.DeleterHandle(ptr, StreamProfileReleaser);
return(p);
}
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);
}
