bbiwarg-original/bbiwarg/Input/InputProviding/InputProviderIntel.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Emgu.CV;
using Emgu.CV.Structure;
using System.Drawing;
using System.Threading;

namespace BBIWARG.Input.InputProviding
{
    class InputProviderIntel : IInputProvider
    {
        PXCMSenseManager senseManager;

        public int CurrentFrameID
        {
            get;
            private set;
        }

        public float FieldOfViewHorizontal
        {
            get;
            private set;
        }

        public float FieldOfViewVertical
        {
            get;
            private set;
        }

        public int ImageWidth
        {
            get { return 640; }
        }

        public int ImageHeight
        {
            get { return 480; }
        }
        
        public bool IsActive
        {
            get;
            private set;
        }

        public event DeviceStartedEventHandler DeviceStartedEvent;

        public event NewFrameEventHandler NewFrameEvent;

        protected UInt16 lowConfidenceValue;

        public void initialize()
        {
            CurrentFrameID = 0;
        }

        public void start()
        {
            initCameraDevice();

            IsActive = true;

            if (DeviceStartedEvent != null && CurrentFrameID == 0)
                DeviceStartedEvent(this, new EventArgs());

            keepActive();
        }

        private bool initCameraDevice()
        {
            senseManager = PXCMSenseManager.CreateInstance();
            PXCMVideoModule.DataDesc ddesc = new PXCMVideoModule.DataDesc();
            ddesc.deviceInfo.streams = PXCMCapture.StreamType.STREAM_TYPE_DEPTH;
            senseManager.EnableStreams(ddesc);

            var initStatus = senseManager.Init();

            if (initStatus != pxcmStatus.PXCM_STATUS_NO_ERROR) {
                Console.WriteLine("Could not init camera. Result: " + initStatus);
                return false;
            }

            PXCMPointF32 fov = senseManager.captureManager.device.QueryDepthFieldOfView();
            FieldOfViewHorizontal = (float) (fov.x * Math.PI / 180.0f);
            FieldOfViewVertical = (float) (fov.y * Math.PI / 180.0f);

            lowConfidenceValue = senseManager.captureManager.device.QueryDepthLowConfidenceValue();
            senseManager.captureManager.device.SetDepthConfidenceThreshold((UInt16)Parameters.ConfidenceImageMinThreshold);

            return true;
        }

        public void stop()
        {
            IsActive = false;
        }

        bool crashed = false;

        protected void keepActive() {
            

            while (IsActive) {

                if (crashed)
                {
                    Console.WriteLine("Trying to reinitialize the camera...");
                    Task<bool> initCameraTask = Task.Run(() => restartCamera());
                    initCameraTask.Wait(TimeSpan.FromSeconds(5));

                    crashed = !initCameraTask.IsCompleted || !initCameraTask.Result;

                    if (crashed) {
                        Console.WriteLine("Something went wrong during init. Waiting a few seconds to cool down...");
                        Thread.Sleep(2000);
                    }
                }
                else {
                    crashed = !tryAcquireAndHandleFrame();
                }
            }
        }

        private bool tryAcquireAndHandleFrame()
        {
            try
            {
                //try to acquire new frame...
                Task<bool> acquireFrameTask = Task.Run(() => acquireNewFrame());
                //wait a second...
                acquireFrameTask.Wait(TimeSpan.FromSeconds(4));

                if (acquireFrameTask.IsCompleted && acquireFrameTask.Result)
                {
                    //that worked, we should have a new frame by now...
                    handleNewFrame();
                    senseManager.ReleaseFrame();
                    return true;
                }
                else {
                    return false;
                }
            }
            catch (Exception e)
            {
                Console.WriteLine("Exception while acquiring frame: " + e);
                return false;
            }
        }

        private bool restartCamera()
        {
            try
            {
                crashed = false;
                senseManager.Dispose();
                Thread.Sleep(1000);
                return initCameraDevice();
            }
            catch (Exception)
            {
                return false;
            }
        }

        public bool IsCrashed()
        {
            return crashed;
        }

        protected bool acquireNewFrame() {
            var status = senseManager.AcquireFrame(true);

            if (status != pxcmStatus.PXCM_STATUS_NO_ERROR) {
                Console.WriteLine("Cam crashed with status" + status);
                return false;
            }

            return true;
        }

        protected void run()
        {
            while (IsActive) {
                if (senseManager.AcquireFrame(true).IsError())
                    continue;

                if (NewFrameEvent != null)
                {
                    handleNewFrame();
                }


                senseManager.ReleaseFrame();
            }

            senseManager.Dispose();
        }

        


        private void handleNewFrame()
        {
            PXCMCapture.Sample sample = senseManager.QuerySample();
            PXCMImage depthImage = sample.depth;
            PXCMImage.ImageInfo info = depthImage.info;
            PXCMImage.ImageData imageData;

            depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out imageData);
            ushort[] data = imageData.ToUShortArray(0, info.width * info.height);

            Image<Gray, UInt16> dImg;

            unsafe
            {
                fixed (ushort* d = data)
                {
                    IntPtr ptr = (IntPtr)d;
                    dImg = new Image<Gray, UInt16>(info.width, info.height, info.width * sizeof(ushort), ptr).Copy();
                }
            }

            depthImage.ReleaseAccess(imageData);
            depthImage.Dispose();

            // confidence filter
            Image<Gray, byte> mask = dImg.InRange(new Gray(lowConfidenceValue), new Gray(lowConfidenceValue));
            dImg.SetValue(new Gray(Int16.MaxValue), mask);

            NewFrameEvent(this, new NewFrameEventArgs(CurrentFrameID, dImg));

            CurrentFrameID += 1;
        }
    }
}