bbiwarg-original/bbiwarg/DataSource/VideoHandle.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra.Single;
using bbiwarg.Graphics;

namespace bbiwarg.DataSource
{
    class VideoHandle : IVideoHandle
    {
        private IInputProvider inputProvider;
        private DepthImage depthImage;
        private ConfidenceImage confidenceImage;
        private UVImage uvImage;
        private ColorImage colorImage;

        private float maxU;
        private float maxV;

        public VideoHandle(IInputProvider inputProvider) {
            this.inputProvider = inputProvider;
            inputProvider.init();
            inputProvider.start();
            inputProvider.updateFrame();
            createImageData();

            maxU = (float)(Math.Tan(inputProvider.getHFOV() / 2f));
            maxV = (float)(Math.Tan(inputProvider.getVFOV() / 2f));
        }

        ~VideoHandle() 
        {
            //inputProvider.stop();
        }

        public void nextFrame() 
        {
            if (inputProvider.isActive())
            {
                inputProvider.releaseFrame();
                inputProvider.updateFrame();
                createImageData();
            }
            else {
                inputProvider.stop();
            }
        }

        public int getWidth()
        {
            return depthImage.getWidth();
        }
        public int getHeight()
        {
            return depthImage.getHeight();
        }

        private void createImageData() 
        {
            depthImage = inputProvider.getDepthImage();
            confidenceImage = inputProvider.getConfidenceImage();
            uvImage = inputProvider.getUVImage();
            colorImage = inputProvider.getColorImage();
        }

        public short getDepth(int x, int y)
        {
            return depthImage.getDepth(x, y);
        }

        public short getConfidence(int x, int y)
        {
            return confidenceImage.getConfidence(x, y);
        }

        public Color getColor(int x, int y)
        {
            float u = uvImage.getU(x, y);
            float v = uvImage.getV(x, y);

            if (u < 0 || v < 0)
                return Color.Black;

            int colorImageWidth = colorImage.getWidth();
            int colorImageHeight = colorImage.getHeight();

            int xInColorImage = (int)(u * colorImageWidth) % colorImageWidth;
            int yInColorImage = (int)(v * colorImageHeight) % colorImageHeight;
            return colorImage.getColor(xInColorImage, yInColorImage);
        }

        public DetectionStatus[] getFingerStatus(uint handIndex)
        {
            return inputProvider.getFingerStatus(handIndex);
        }

        public Vector[] getFingerTipPositions3D(uint handIndex)
        {
            return inputProvider.getFingerTipPositions3D(handIndex);
        }

        public Vector getPalmPosition2D(uint handIndex)
        {
            return inputProvider.getPalmPosition2D(handIndex);
        }

        public Vector getPalmPosition3D(uint handIndex)
        {
            return inputProvider.getPalmPosition3D(handIndex);
        }

        public Vector getForearmPosition3D(uint handIndex)
        {
            return inputProvider.getForearmPosition3D(handIndex);
        }

        // TODO
        public void createVertexArray(IntPtr vertexBuffer)
        {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();
            int confidenceThreshold = inputProvider.getConfidenceThreshold();

            int index = 0;
            for (int x = 0; x < width; x++)
            {
                for (int y = 0; y < height; y++)
                {
                    if (confidenceImage.getConfidence(x, y) > confidenceThreshold)
                    {
                        int depth = depthImage.getDepth(x, y);
                        Color c = getColor(x, y);
                        create3DVertexFrom2D(x, y, depth, c, index, vertexBuffer);

                        index++;
                    }
                }
            }
        }

        public Vector pixel2VertexPosition(Vector pixelPosition)
        {
            int width = depthImage.getWidth();
            int height = depthImage.getHeight();

            float convertedDepth = pixelPosition[2] / 1000f; // mm into m

            float u = (pixelPosition[0] / (float)width - 0.5f) * 2f;
            float v = ((1 - pixelPosition[1] / (float)height) - 0.5f) * 2f;

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

            Vector result = new DenseVector(3);
            result[2] = -convertedDepth / (float)Math.Sqrt(1 + relX * relX + relY * relY);
            result[0] = -relX * result[2];
            result[1] = -relY * result[2];

            return result;
        }
        private void create3DVertexFrom2D(float pixelX, float pixelY, int depth, Color c, int index, IntPtr vertexBuffer)
        {
            Vector pixelPosition = new DenseVector(3);
            pixelPosition[0] = pixelX;
            pixelPosition[1] = pixelY;
            pixelPosition[2] = depth;
            Vector vertexPosition = pixel2VertexPosition(pixelPosition);


            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] = vertexPosition[0];
                vertexArrayF[i4 + 1] = vertexPosition[1];
                vertexArrayF[i4 + 2] = vertexPosition[2];

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

        }
    }
}