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 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()
        {
            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;
            
            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);
                    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;

                senseManager.ReleaseFrame();
            }

            senseManager.Dispose();
        }
    }
}