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

using Emgu.CV;
using Emgu.CV.Structure;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Security.Permissions;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace BBIWARG.Input.InputProviding
{
    class IntelCameraWrapper : MarshalByRefObject
    {
        private Assembly assembly;
        public InputProviderIntel inputProviderIntel;
        private dynamic senseManager;
        private dynamic session;
        private dynamic AccessRead;
        private dynamic PixelFormat;
        private dynamic EverythingFine;
        private MethodInfo acquireAccessMethod;
        private object[] methodParams;
        //public bool errorstate = false;
        public bool terminated = false;

        public IntelCameraWrapper()
        {
           
        }

        [SecurityPermissionAttribute(SecurityAction.Demand, Flags = SecurityPermissionFlag.Infrastructure)]
        public override object InitializeLifetimeService()
        {
            return null;
        }

        public void init(InputProviderIntel intelInputProvider) {
            inputProviderIntel = intelInputProvider;

            var domain = System.AppDomain.CreateDomain("IntelCamDomain");
            assembly = Assembly.Load("libpxcclr.cs");

            AccessRead = (dynamic)Enum.Parse(assembly.GetType("PXCMImage+Access"), "ACCESS_READ");
            PixelFormat = (dynamic)Enum.Parse(assembly.GetType("PXCMImage+PixelFormat"), "PIXEL_FORMAT_DEPTH");

            EverythingFine = (dynamic)Enum.Parse(assembly.GetType("pxcmStatus"), "PXCM_STATUS_NO_ERROR");

            // here be dragons
            Type imageDataType = assembly.GetType("PXCMImage+ImageData");
            Type refType = imageDataType.MakeByRefType();
            Type[] types = new Type[] { AccessRead.GetType(), PixelFormat.GetType(), refType };
            acquireAccessMethod = assembly.GetType("PXCMImage").GetMethod("AcquireAccess", types);

            //depthImage.AcquireAccess(AccessRead, PixelFormat, out imageData);
            methodParams = new object[] { AccessRead, PixelFormat, null };

            session = assembly.GetType("PXCMSession").GetMethod("CreateInstance").Invoke(null, null);
            senseManager = session.CreateSenseManager();

            dynamic ddesc = Activator.CreateInstance(assembly.GetType("PXCMVideoModule+DataDesc"));
            ddesc.deviceInfo.streams = (dynamic)Enum.Parse(assembly.GetType("PXCMCapture+StreamType"), "STREAM_TYPE_DEPTH");

            senseManager.EnableStreams(ddesc);
            var result = senseManager.Init();

            dynamic fov = senseManager.captureManager.device.QueryDepthFieldOfView();
            inputProviderIntel.FieldOfViewHorizontal = (float) (fov.x * Math.PI / 180.0f);
            inputProviderIntel.FieldOfViewVertical = (float) (fov.y * Math.PI / 180.0f);
            inputProviderIntel.lowConfidenceValue = senseManager.captureManager.device.QueryDepthLowConfidenceValue();
            senseManager.captureManager.device.SetDepthConfidenceThreshold((UInt16)Parameters.ConfidenceImageMinThreshold);

            Console.WriteLine("Started camera. Delivering frames...");
        }

        internal void run()
        {
            while (inputProviderIntel.IsActive)
            {
                if (terminated)
                    return;

                //errorstate = false;

                var status = senseManager.AcquireFrame(true);

                if (status != EverythingFine)
                {
                    Console.WriteLine("Found crash inside CameraHandler...");
                    //errorstate = true;
                    return;
                }

                if (terminated)
                    return;

                if (inputProviderIntel.hasNewFrameEvent())
                {

                    dynamic sample = senseManager.QuerySample();
                    dynamic depthImage = sample.depth;
                    dynamic info = depthImage.info;



                    acquireAccessMethod.Invoke(depthImage, methodParams);

                    dynamic imageData = methodParams[2];

                    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(inputProviderIntel.lowConfidenceValue), new Gray(inputProviderIntel.lowConfidenceValue));
                    dImg.SetValue(new Gray(Int16.MaxValue), mask);

                    inputProviderIntel.newFrame(inputProviderIntel.CurrentFrameID, dImg);
                }

                inputProviderIntel.CurrentFrameID += 1;

                senseManager.ReleaseFrame();
            }

            senseManager.Dispose();
        }

        private void closeCamera()
        {
            senseManager.Close();
            session.Dispose();
        }
    }
}