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);
        }
Ejemplo n.º 3
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);
Ejemplo n.º 4
0
        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);
        }