bbiwarg-original/bbiwarg/Images/DepthImage.cs

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

namespace BBIWARG.Images
{
    /// <summary>
    /// DepthImage stores a processed version of the depth image read by the camera as an <see cref="Image"/>
    /// </summary>
    public class DepthImage
    {
        /// <summary>
        /// the processed depth image
        /// </summary>
        public Image<Gray, byte> Image { get; private set; }

        /// <summary>
        /// the maximum depth which is considered important
        /// </summary>
        public Int16 MaxDepth { get; private set; }

        /// <summary>
        /// the minimum depth in the raw depth image
        /// </summary>
        public Int16 MinDepth { get; private set; }

        /// <summary>
        /// image size of the depth image
        /// </summary>
        public ImageSize Size { get; private set; }

        /// <summary>
        /// Construct a DepthImage from the raw depth data, the image size and a confidenceImage.
        /// Filters the image using confidenceImage and also applies thresholding and smoothing.
        /// </summary>
        /// <param name="rawDepthData">the raw depth data</param>
        /// <param name="size">the image size</param>
        /// <param name="confidenceImage">the confidence image</param>
        public DepthImage(IntPtr rawDepthData, ImageSize size, ConfidenceImage confidenceImage)
        {
            Size = size;
            Image<Gray, Int16> rawDepthImage = new Image<Gray, Int16>(Size.Width, Size.Height, Size.Width * 2, rawDepthData);

            // filter with confidenceImage mask
            rawDepthImage = rawDepthImage.Or((1 - confidenceImage.Mask).Convert<Gray, Int16>().Mul(Int16.MaxValue));

            // smooth with median filter
            rawDepthImage = rawDepthImage.SmoothMedian(Parameters.DepthImageMedianSize);

            // threshold min&maxDepth
            MinDepth = findMinDepth(rawDepthImage);
            MaxDepth = (Int16)(MinDepth + Parameters.DepthImageDepthRange);

            // threshold (dst = (src > (MaxDepth - MinDepth)) ? MaxDepth - MinDepth : src)
            Image = (rawDepthImage - MinDepth).ThresholdTrunc(new Gray(MaxDepth - MinDepth)).Convert<Gray, byte>();

            // smooth with median filter
            Image = Image.SmoothMedian(Parameters.DepthImageMedianSize);
        }

        /// <summary>
        /// Returns the depth in the processed image at a given point.
        /// </summary>
        /// <param name="point">the point</param>
        /// <returns>depth at the point</returns>
        public Int16 getDepthAt(Point point)
        {
            return getDepthAt(point.X, point.Y);
        }

        /// <summary>
        /// Returns the depth in the processed image at a given position.
        /// </summary>
        /// <param name="x">x coordinate of the position</param>
        /// <param name="y">y coordinate of the position</param>
        /// <returns>the depth at the position</returns>
        public Int16 getDepthAt(int x, int y)
        {
            try
            {
                return (Int16)(MinDepth + Image.Data[y, x, 0]);
            }
            catch (IndexOutOfRangeException e)
            {
                return (Int16)(MinDepth + Image.Data[0, 0, 0]);
            }
        }

        /// <summary>
        /// Sets the depth in the processed image.
        /// </summary>
        /// <param name="point">point where the depth is set</param>
        /// <param name="depth">new depth value</param>
        public void setDepthAt(Point point, Int16 depth)
        {
            setDepthAt(point.X, point.Y, depth);
        }

        /// <summary>
        /// Sets the depth in the processed image.
        /// </summary>
        /// <param name="x">x coordinate of position to set depth</param>
        /// <param name="y">y coordinate of position to set depth</param>
        /// <param name="depth">new depth value</param>
        public void setDepthAt(int x, int y, Int16 depth)
        {
            Image.Data[y, x, 0] = (byte)(depth - MinDepth);
        }

        /// <summary>
        /// Returns the minimum depth in an <see cref="Image"/>
        /// </summary>
        /// <param name="image">the image</param>
        /// <returns>the minimum depth</returns>
        private Int16 findMinDepth(Image<Gray, Int16> image)
        {
            // min and max values
            double[] min, max;

            // min and max locations
            Point[] minLoc, maxLoc;

            image.MinMax(out min, out max, out minLoc, out maxLoc);

            return (Int16)min[0];
        }
    }
}