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;

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 ImageHeight
        {
            get;
            private set;
        }

        public int ImageWidth
        {
            get;
            private set;
        }

        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()
        {
            senseManager = PXCMSenseManager.CreateInstance();
            PXCMVideoModule.DataDesc ddesc = new PXCMVideoModule.DataDesc();
            ddesc.deviceInfo.streams = PXCMCapture.StreamType.STREAM_TYPE_DEPTH;
            senseManager.EnableStreams(ddesc);

            senseManager.Init();

            PXCMPointF32 fov = senseManager.captureManager.device.QueryDepthFieldOfView();
            FieldOfViewHorizontal = fov.x;
            FieldOfViewVertical = fov.y;

            while (senseManager.AcquireFrame(true).IsError()) { }
            PXCMImage.ImageInfo info = senseManager.QuerySample().depth.info;
            ImageWidth = info.width;
            ImageHeight = info.height;
            senseManager.ReleaseFrame();

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

            IsActive = true;

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

            run();
        }

        public void stop()
        {
            IsActive = false;
        }

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

                if (NewFrameEvent != null)
                {

                    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);
                    Bitmap dBmp = imageData.ToBitmap(0, info.width, info.height);
                    depthImage.ReleaseAccess(imageData);
                    depthImage.Dispose();

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

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

                CurrentFrameID += 1;

                senseManager.ReleaseFrame();
            }

            senseManager.Dispose();
        }
    }
}