bbiwarg-original/bbiwarg/Images/DepthImage.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using MathNet.Numerics.LinearAlgebra.Single;

using System.Drawing;
using Emgu.CV;
using Emgu.CV.Structure;

namespace bbiwarg.Images
{
    /**
     * Represents an depth image where the depth is given in distance to the camera in millimeters. 
     */
    class DepthImage
    {
        private int width, height;
        private Image<Gray, short> image;
        private int[] histogram;

        public DepthImage(int width, int height, short[] data)
        {
            this.width = width;
            this.height = height;
            
            image = new Image<Gray, short>(width, height);
            for (int i = 0; i < data.Length; ++i)
                setDepth(i % width, i / width, data[i]);
        }

        public int getWidth()
        {
            return width;
        }

        public int getHeight()
        {
            return height;
        }

        public short getDepth(int x, int y)
        {
            return image.Data[y, x, 0];
        }

        private void setDepth(int x, int y, short depth)
        {
            image.Data[y, x, 0] = depth;
        }

        public void filterMedian(int filterSize)
        {
            image = image.SmoothMedian(filterSize);
        }

        public void thresholdDepth(int min, int max)
        {
            image = image.ThresholdToZero(new Gray(min));
            image = image.ThresholdTrunc(new Gray(max));

            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    if (getDepth(x, y) == 0)
                        setDepth(x, y, (short) max);
                }
            }
        }

        public void thresholdBinary(Int16 thresholdDepth)
        {
            Int16 maxDepth = getMaxDepth();
            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    Int16 depth = getDepth(x, y);
                    if (depth <= thresholdDepth)
                        setDepth(x, y, 0);
                    else if (depth != maxDepth)
                        setDepth(x, y, (short) (maxDepth / 2));
                }
            }
        }

        public int[] getSmoothedHistogram()
        {
            Int16 minDepth = getMinDepth();
            Int16 maxDepth = getMaxDepth();

            histogram = new int[maxDepth - minDepth];
            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    int depth = getDepth(x, y);
                    if (depth != maxDepth)
                        histogram[depth - minDepth]++;
                }
            }

            smoothHistogram();
            return histogram;
        }

        private void smoothHistogram()
        {
            histogram[0] = (histogram[0] + histogram[1]) / 2;
            histogram[histogram.Length - 1] = (histogram[histogram.Length - 2] + histogram[histogram.Length - 1]) / 2;

            for (int i = 1; i < histogram.Length - 1; ++i)
            {
                histogram[i] = (histogram[i - 1] + histogram[i] + histogram[i + 1]) / 3;
            }
        }

        public Int16[] getSegmentationDepth()
        {
            //get first peak
            Int16 firstMaxIndex = 0;

            for (Int16 i = 0; i < histogram.Length; i++) {
                if (histogram[i] > histogram[firstMaxIndex]) {
                    firstMaxIndex = i;
                }
            }

            // get second peak
            Int16 minDistance = 20; // mm
            Int16 secondMaxIndex = 0;
            for (Int16 i = 0; i < histogram.Length; i++) {
                if (Math.Abs(firstMaxIndex - i) > minDistance && histogram[i] > histogram[secondMaxIndex]) {
                    secondMaxIndex = i;
                }
            }

            if (secondMaxIndex < firstMaxIndex) {
                Int16 dummy = firstMaxIndex;
                firstMaxIndex = secondMaxIndex;
                secondMaxIndex = dummy;
            }

            // get min in between
            Int16 minIndex = firstMaxIndex;
            for (Int16 i = firstMaxIndex; i < secondMaxIndex; i++) {
                if (histogram[i] < histogram[minIndex]) {
                    minIndex = i;
                }
            }

            Int16 minDepth = getMinDepth();

            Int16[] result = new Int16[3];
            result[0] = firstMaxIndex;
            result[1] = minIndex;
            result[2] = secondMaxIndex;

            return result;
        }

        public Int16 getPeakDepth()
        {
            Int16 minDepth = getMinDepth();

            Int16 peak = -1;
            int maxValue = 0;
            for (int i = 0; i < histogram.Length; ++i)
            {
                if (histogram[i] > maxValue)
                {
                    maxValue = histogram[i];
                    peak = (Int16) (i + minDepth);
                }
            }
            return peak;
        }

        public Int16 getMinDepth()
        {
            Int16 minDepth = Int16.MaxValue;
            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    Int16 depth = getDepth(x, y);
                    if (depth < minDepth)
                        minDepth = depth;
                }
            }
            return minDepth;
        }

        public Int16 getMaxDepth()
        {
            Int16 maxDepth = Int16.MinValue;
            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    Int16 depth = getDepth(x, y);
                    if (depth > maxDepth)
                        maxDepth = depth;
                }
            }
            return maxDepth;
        }

        public void floodFill(int x, int y, int val)
        {
            Image<Gray, Byte> tmp = image.Convert<Byte>(delegate(short s) { return (byte) ((int) s * Byte.MaxValue / 2000); });

            Emgu.CV.Structure.MCvConnectedComp comp = new MCvConnectedComp();
            Emgu.CV.CvInvoke.cvFloodFill(tmp.Ptr, new Point(x, y), new MCvScalar(val), new MCvScalar(30), new MCvScalar(30), out comp, 0, IntPtr.Zero);

            for (int i = 0; i < width; ++i)
            {
                for (int j = 0; j < height; ++j)
                {
                    if (tmp.Data[j, i, 0] == 255)
                        setDepth(i, j, 2000);
                    else
                        setDepth(i, j, 0);
                }
            }
        }
    }
}