bbiwarg-original/bbiwarg/DataSource/IisuDataSource.cs

using System;
using System.Drawing;
using System.Diagnostics;
using System.Runtime.InteropServices;
using Iisu;
using MathNet.Numerics.LinearAlgebra.Single;
using bbiwarg.Graphics;

namespace bbiwarg.DataSource
{
    class IIsuDataSource: IVideoDataSource
    {
        private IHandle handle;
        private IDevice device;
        private string moviePath;
        bool active;

        // parameters
        private IParameterHandle<float> frameRate;

        // data
        private IDataHandle<bool>[] handOpen = new IDataHandle<bool>[2];
        private IDataHandle<Iisu.Data.Vector3>[] palmPositions3D = new IDataHandle<Iisu.Data.Vector3>[2];
        private IDataHandle<Iisu.Data.Vector2>[] palmPositions2D = new IDataHandle<Iisu.Data.Vector2>[2];
        private IDataHandle<Iisu.Data.Vector3>[] tipPositions3D = new IDataHandle<Iisu.Data.Vector3>[2];
        private IDataHandle<Iisu.Data.Vector3>[] forearmPositions3D = new IDataHandle<Iisu.Data.Vector3>[2];
        private IDataHandle<Iisu.Data.Vector3>[] palmNormals3D = new IDataHandle<Iisu.Data.Vector3>[2];
        private IDataHandle<int[]>[] fingerStatus = new IDataHandle<int[]>[2];
        private IDataHandle<int>[] handStatus = new IDataHandle<int>[2];
        private IDataHandle<Iisu.Data.Vector3[]>[] fingerTipPositions3D = new IDataHandle<Iisu.Data.Vector3[]>[2];
        private IDataHandle<Iisu.Data.Vector2[]>[] fingerTipPositions2D = new IDataHandle<Iisu.Data.Vector2[]>[2];
        private IDataHandle<int>[] handSides = new IDataHandle<int>[2];
        private IDataHandle<Iisu.Data.IImageData> depthImage;
        private IDataHandle<Iisu.Data.IImageData> colorImage;
        private IDataHandle<Iisu.Data.IImageData> confidenceImage;
        private IDataHandle<Iisu.Data.IImageData> uvImage;
        private IParameterHandle<float> hfov;
        private IParameterHandle<float> vfov;

        private ImageData currentImage;
        private float maxU;
        private float maxV;
        private int width;
        private int height;

        //TODO
        private IntPtr vertexBuffer;

        /*
         * Creates an Iisu data source.
         * params:
         *  moviePath: path to movie to be used as source
         *             if empty the camera is used
         */
        public IIsuDataSource(string moviePath = "")
        {
            this.moviePath = moviePath;
            active = false;
        }

        public void init()
        {
            handle = Iisu.Iisu.Context.CreateHandle();
            
            IDeviceConfiguration conf = handle.CreateDeviceConfiguration();
            if (moviePath.Length != 0)
                conf.MoviePath = moviePath;

            device = handle.InitializeDevice(conf);

            // parameters
            if (moviePath.Length != 0)
                device.RegisterParameterHandle<int>("SOURCE.MOVIE.PlayMode").Value = 0; // playMode = once
            else
                device.RegisterParameterHandle<int>("SOURCE.DEPTHSENSE.AmplitudeThreshold").Value = 100; // confidence-threshhold

            frameRate = device.RegisterParameterHandle<float>("SOURCE.FrameRate");
            hfov = device.RegisterParameterHandle<float>("SOURCE.CAMERA.DEPTH.HFOV");
            vfov = device.RegisterParameterHandle<float>("SOURCE.CAMERA.DEPTH.VFOV");
            maxU = (float)Math.Tan(hfov.Value / 2f);
            maxV = (float)Math.Tan(vfov.Value / 2f);

            // events
            device.EventManager.RegisterEventListener("DEVICE.Status", new Iisu.EventDelegates.Device.Status(onDeviceStatusChanged));
            
            // data
            depthImage = device.RegisterDataHandle<Iisu.Data.IImageData>("SOURCE.CAMERA.DEPTH.Image");
            colorImage = device.RegisterDataHandle<Iisu.Data.IImageData>("SOURCE.CAMERA.COLOR.Image");
            confidenceImage = device.RegisterDataHandle<Iisu.Data.IImageData>("SOURCE.CAMERA.CONFIDENCE.Image");
            uvImage = device.RegisterDataHandle<Iisu.Data.IImageData>("SOURCE.CAMERA.COLOR.REGISTRATION.UV.Image");

            handOpen[0] = device.RegisterDataHandle<bool>("CI.HAND1.IsOpen");
            handOpen[1] = device.RegisterDataHandle<bool>("CI.HAND2.IsOpen");

            palmPositions3D[0] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND1.PalmPosition3D");
            palmPositions3D[1] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND2.PalmPosition3D");

            palmPositions2D[0] = device.RegisterDataHandle<Iisu.Data.Vector2>("CI.HAND1.PalmPosition2D");
            palmPositions2D[1] = device.RegisterDataHandle<Iisu.Data.Vector2>("CI.HAND2.PalmPosition2D");

            tipPositions3D[0] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND1.TipPosition3D");
            tipPositions3D[1] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND2.TipPosition3D");

            forearmPositions3D[0] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND1.ForearmPosition3D");
            forearmPositions3D[1] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND2.ForearmPosition3D");

            palmNormals3D[0] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND1.PalmNormal3D");
            palmNormals3D[1] = device.RegisterDataHandle<Iisu.Data.Vector3>("CI.HAND2.PalmNormal3D");

            fingerStatus[0] = device.RegisterDataHandle<int[]>("CI.HAND1.FingerStatus");
            fingerStatus[1] = device.RegisterDataHandle<int[]>("CI.HAND2.FingerStatus");

            handStatus[0] = device.RegisterDataHandle<int>("CI.HAND1.Status");
            handStatus[1] = device.RegisterDataHandle<int>("CI.HAND2.Status");

            fingerTipPositions3D[0] = device.RegisterDataHandle<Iisu.Data.Vector3[]>("CI.HAND1.FingerTipPositions3D");
            fingerTipPositions3D[1] = device.RegisterDataHandle<Iisu.Data.Vector3[]>("CI.HAND2.FingerTipPositions3D");
            
            fingerTipPositions2D[0] = device.RegisterDataHandle<Iisu.Data.Vector2[]>("CI.HAND1.FingerTipPositions2D");
            fingerTipPositions2D[1] = device.RegisterDataHandle<Iisu.Data.Vector2[]>("CI.HAND2.FingerTipPositions2D");

            handSides[0] = device.RegisterDataHandle<int>("CI.HAND1.Side");
            handSides[1] = device.RegisterDataHandle<int>("CI.HAND1.Side");
        }

        private void onDeviceStatusChanged(string eventName, DeviceStatus status)
        {
            active = status.HasFlag(Iisu.DeviceStatus.Playing);
        }

        public void start()
        {
            device.Start();
            updateFrame();
         }

        public void stop()
        {
            device.Stop(true);
        }

        public void updateFrame()
        {
            device.UpdateFrame(true);
            createImageData();
        }

        public void releaseFrame()
        {
            device.ReleaseFrame();
        }

        public bool isActive()
        {
            return active;
        }

        public int getFrameRate()
        {
            return (int) frameRate.Value;
        }

        public float getHFOV() {
            return hfov.Value;
        }

        public float getVFOV() {
            return vfov.Value;
        }

        public DepthImage getDepthImage()
        {
            Iisu.Data.IImageInfos imageInfos = depthImage.Value.ImageInfos;
            int width = (int) imageInfos.Width;
            int height = (int) imageInfos.Height;
            int numBytes = (int) imageInfos.BytesRaw;

            IntPtr imageData = depthImage.Value.Raw;
            
            short[] depthData = new short[width * height];
            Marshal.Copy(imageData, depthData, 0, width * height);

            return new DepthImage(width, height, depthData);
        }

        public ColorImage getColorImage()
        {
            Iisu.Data.IImageInfos imageInfos = colorImage.Value.ImageInfos;
            int width = (int)imageInfos.Width;
            int height = (int)imageInfos.Height;
            int numBytes = (int)imageInfos.BytesRaw;

            IntPtr imageData = colorImage.Value.Raw;

            byte[] colorData = new byte[numBytes];
            Marshal.Copy(imageData, colorData, 0, numBytes);

            return new ColorImage(width, height, colorData);
        }

        public ConfidenceImage getConfidenceImage()
        {
            Iisu.Data.IImageInfos imageInfos = confidenceImage.Value.ImageInfos;
            int width = (int)imageInfos.Width;
            int height = (int)imageInfos.Height;
            int numBytes = (int)imageInfos.BytesRaw;

            IntPtr imageData = confidenceImage.Value.Raw;

            short[] confidenceData = new short[width * height];
            Marshal.Copy(imageData, confidenceData, 0, width * height);

            return new ConfidenceImage(width, height, confidenceData);
        }

        public UVImage getUVImage()
        {
            Iisu.Data.IImageInfos imageInfos = uvImage.Value.ImageInfos;
            int width = (int)imageInfos.Width;
            int height = (int)imageInfos.Height;
            int numBytes = (int)imageInfos.BytesRaw;

            IntPtr imageData = uvImage.Value.Raw;

            float[] uvData = new float[2 * width * height];
            Marshal.Copy(imageData, uvData, 0, 2 * width * height);

            return new UVImage(width, height, uvData);
        }

        public ImageData getImageData()
        {
            return currentImage;
        }

        private void createImageData() {
            currentImage = new ImageData(getDepthImage(), getConfidenceImage(), getColorImage(), getUVImage());
            width = currentImage.getWidth();
            height = currentImage.getHeight();
        }

        private void checkHandIndex(uint handIndex) 
        {
            if (handIndex < 1 || handIndex > 2)
                throw new ArgumentOutOfRangeException("handIndex is out of range [0,1]");
        }

        public bool isHandOpen(uint handIndex)
        {
            checkHandIndex(handIndex);
            return handOpen[handIndex - 1].Value;
        }

        public Vector getPalmPosition3D(uint handIndex)
        {
            checkHandIndex(handIndex);
            return new DenseVector(palmPositions3D[handIndex - 1].Value.ToArray());
        }

        public Vector getPalmPosition2D(uint handIndex)
        {
            checkHandIndex(handIndex);
            return new DenseVector(palmPositions2D[handIndex - 1].Value.ToArray());
        }

        public Vector getTipPosition3D(uint handIndex)
        {
            checkHandIndex(handIndex);
            return new DenseVector(tipPositions3D[handIndex - 1].Value.ToArray());
        }

        public Vector getForearmPosition3D(uint handIndex)
        {
            checkHandIndex(handIndex);
            return new DenseVector(forearmPositions3D[handIndex - 1].Value.ToArray());
        }

        public Vector getPalmNormal3D(uint handIndex)
        {
            checkHandIndex(handIndex);
            return new DenseVector(palmNormals3D[handIndex - 1].Value.ToArray());
        }

        public DetectionStatus[] getFingerStatus(uint handIndex)
        {
            checkHandIndex(handIndex);
            int[] status = fingerStatus[handIndex - 1].Value;
            DetectionStatus[] result = new DetectionStatus[status.Length];
            for (int i = 0; i < status.Length; ++i)
            {
                switch (status[i])
                {
                    case 0:
                        result[i] = DetectionStatus.Inactive;
                        break;
                    case 1:
                        result[i] = DetectionStatus.Detected;
                        break;
                    case 2:
                        result[i] = DetectionStatus.Tracked;
                        break;
                }
            }
            return result;
        }

        public DetectionStatus getHandStatus(uint handIndex)
        {
            checkHandIndex(handIndex);
            int status = handStatus[handIndex - 1].Value;
            DetectionStatus result = new DetectionStatus();
            switch (status)
            {
                case 0:
                    result = DetectionStatus.Inactive;
                    break;
                case 1:
                    result = DetectionStatus.Detected;
                    break;
                case 2:
                    result = DetectionStatus.Tracked;
                    break;
            }
            return result;
        }

        public Vector[] getFingerTipPositions3D(uint handIndex)
        {
            checkHandIndex(handIndex);
            Iisu.Data.Vector3[] positions = fingerTipPositions3D[handIndex - 1].Value;
            Vector[] results = new DenseVector[positions.Length];
            for (int i = 0; i < positions.Length; ++i)
                results[i] = new DenseVector(positions[i].ToArray());
            return results;
        }

        public Vector[] getFingerTipPositions2D(uint handIndex)
        {
            checkHandIndex(handIndex);
            Iisu.Data.Vector2[] positions = fingerTipPositions2D[handIndex - 1].Value;
            Vector[] results = new DenseVector[positions.Length];
            for (int i = 0; i < positions.Length; ++i)
                results[i] = new DenseVector(positions[i].ToArray());
            return results;
        }

        public HandSide getHandSide(uint handIndex)
        {
            checkHandIndex(handIndex);
            int side = handSides[handIndex - 1].Value;
            switch (side) 
            {
                case 1:
                    return HandSide.Left;
                case 2:
                    return HandSide.Right;
                default:
                    return HandSide.Unknown;
            }
        }

        public void createVertexArray()
        {
            int index = 0;
            for (int x = 0; x < width; x++)
            {
                for (int y = 0; y < height; y++)
                {
                    int depth = currentImage.getDepth(x, y);
                    create3DVertexFrom2D(x, y, depth, index, currentImage.getColor(x, y));
                    index++;
                }
            }
        }

        private void create3DVertexFrom2D(float pixelX, float pixelY, int depth, int index, Color c)
        {
            float convertedDepth = depth / 1000f; // mm into m

            float u = (pixelX / (float)width - 0.5f) * 2f;
            float v = ((1 - pixelY / (float)height) - 0.5f) * 2f;

            float relX = (u * maxU);
            float relY = (v * maxV);

            float z = convertedDepth / (float)Math.Sqrt(1 + relX * relX + relY * relY);
            float x = relX * z;
            float y = relY * z;

            int i4 = (3 * sizeof(float) + 4 * sizeof(byte)) / sizeof(float) * index;
            int i16 = (3 * sizeof(float) + 4 * sizeof(byte)) * index;
            unsafe
            {
                byte* vertexArrayB = (byte*)vertexBuffer.ToPointer();
                float* vertexArrayF = (float*)vertexBuffer.ToPointer();

                vertexArrayF[i4 + 0] = x;
                vertexArrayF[i4 + 1] = y;
                vertexArrayF[i4 + 2] = -z;

                vertexArrayB[i16 + 12] = c.R;
                vertexArrayB[i16 + 13] = c.G;
                vertexArrayB[i16 + 14] = c.B; 
                vertexArrayB[i16 + 15] = c.A; 
            }
        }

        public void setVertexBuffer(IntPtr vertexBuffer)
        {
            this.vertexBuffer = vertexBuffer;
        }
    }
}