TransformImage.cs

Go to the documentation of this file.

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AForge;
using AForge.Imaging;
using AForge.Imaging.Filters;
using Configuration;
using SharedObjects;
using Image = System.Drawing.Image;
using Line = SharedObjects.Line;

namespace Simple3DScan
{
    internal static class TransformImage
    {
        internal static Bitmap CreateGrayImageOfLaser(Bitmap image, Filter filter)
        {
            Bitmap filteredImage = (Bitmap) image.Clone();

            if (filter.UseHslFilter)
            {
                HSLFiltering hslFilter = new HSLFiltering
                {
                    Saturation = new Range(filter.SaturationMin, filter.SaturationMax),
                    Luminance = new Range(filter.LuminanceMin, filter.LuminanceMax),
                    Hue = new IntRange(filter.HueMin, filter.HueMax)
                };
                hslFilter.ApplyInPlace(filteredImage);
            }

            if (filter.UseColorFilter)
            {
                ColorFiltering colorFilter = new ColorFiltering
                {
                    Red = new IntRange(filter.FilterRedMin, filter.FilterRedMax),
                    Blue = new IntRange(filter.FilterBlueMin, filter.FilterBlueMax),
                    Green = new IntRange(filter.FilterGreenMin, filter.FilterGreenMax)
                };

                colorFilter.ApplyInPlace(filteredImage);
            }

            IFilter grayFilter;

            switch (filter.GrayFilter)
            {
                case GrayFilter.BT709:
                    //RGB image to 8bpp gray scale 
                    grayFilter = Grayscale.CommonAlgorithms.BT709;
                    break;
                case GrayFilter.RMY:
                    grayFilter = Grayscale.CommonAlgorithms.RMY;
                    break;
                case GrayFilter.Y:
                    grayFilter = Grayscale.CommonAlgorithms.Y;
                    break;
                default:
                    //RGB image to 8bpp gray scale 
                    grayFilter = Grayscale.CommonAlgorithms.BT709;
                    break;
            }

            Bitmap grayImage = grayFilter.Apply(filteredImage);

            if (filter.UseThresholdFilter)
            {
                Threshold thresholdFilter = new Threshold(filter.Threshold);
                thresholdFilter.ApplyInPlace(grayImage);
            }

            if (filter.UseErosionFilter)
            {
                //erosion filter to filter out small unwanted pixels 
                Erosion3x3 errossionFilter = new Erosion3x3();
                errossionFilter.ApplyInPlace(grayImage);
            }

            if (filter.UseDiletationFilter)
            {
                //dilation filter
                Dilatation3x3 dilatationFilter = new Dilatation3x3();
                dilatationFilter.ApplyInPlace(grayImage);
            }

            return grayImage;
        }

        //[Log]
        //[LogException]
        //public static Bitmap CreateGrayImageOfLaser(Bitmap image)
        //{
        //    HSLFiltering hslFilter = new HSLFiltering
        //    {
        //        Saturation = new Range(0.05f, 1f),
        //        Luminance = new Range(0.05f, 1f),
        //        Hue = new IntRange(280, 340)
        //    };

        //    ColorFiltering colorFilter = new ColorFiltering
        //    {
        //        Red = new IntRange(252, 255),
        //        Blue = new IntRange(0, 255),
        //        Green = new IntRange(0, 255)
        //    };

        //    //RGB image to 8bpp gray scale 
        //    IFilter grayFilter = Grayscale.CommonAlgorithms.BT709;
        //    Bitmap grayImage = grayFilter.Apply(colorFilter.Apply(hslFilter.Apply(image)));

        //    //thresholding a image
        //    Threshold thresholdFilter = new Threshold(40);
        //    thresholdFilter.ApplyInPlace(grayImage);

        //    //erosion filter to filter out small unwanted pixels 
        //    Erosion3x3 errossionFilter = new Erosion3x3();
        //    errossionFilter.ApplyInPlace(grayImage);


        //    //dilation filter
        //    Dilatation3x3 dilatationFilter = new Dilatation3x3();
        //    dilatationFilter.ApplyInPlace(grayImage);
        //    return grayImage;
        //}

        [Log]
        [LogException]
        public static Blob[] GetBlobsFromGrayScaleBitmap(Bitmap bitmap)
        {
            BlobCounter blobCounter = new BlobCounter { ObjectsOrder = ObjectsOrder.YX };//MaxWidth = 10, ObjectsOrder = ObjectsOrder.YX };
            blobCounter.ProcessImage(bitmap);
            Blob[] blobs = blobCounter.GetObjectsInformation();
            return blobs;
        }

        [Log]
        [LogException]
        public static Blob GetLargestBlob(IEnumerable<Blob> blobs)
        {
            int maxSize = 0;
            Blob maxObject = new Blob(0, new Rectangle(0, 0, 0, 0));
            foreach (Blob blob in blobs)
            {
                int blobSize = blob.Rectangle.Width * blob.Rectangle.Height;
                if (blobSize <= maxSize)
                    continue;

                maxSize = blobSize;
                maxObject = blob;
            }

            return maxObject;
        }

        [Log]
        [LogException]
        public static float GetLaserLinePosition(Image laserImage, Filter filter)
        {
            Image grayImage = CreateGrayImageOfLaser((Bitmap)laserImage, filter);
            Image skeletonImage = SimpleSkeletonizationImage((Bitmap)grayImage);
            Blob[] blobs = GetBlobsFromGrayScaleBitmap((Bitmap)skeletonImage);
            return GetLaserLinePosition(blobs);
        }

        [Log]
        [LogException]
        public static float GetLaserLinePosition(Image grayImage)
        {
            Image skeletonImage = SimpleSkeletonizationImage((Bitmap)grayImage);
            Blob[] blobs = GetBlobsFromGrayScaleBitmap((Bitmap)skeletonImage);
            return GetLaserLinePosition(blobs);
        }

        [Log]
        [LogException]
        public static float GetLaserLinePosition(Blob[] blobs)
        {
            // take laserlight from top 100 pixels and ignore lasers outside a valid range.
            List<Blob> blobList =
                blobs.Where(
                            blob => blob.CenterOfGravity.Y < 100)// && blob.CenterOfGravity.X > 100 && blob.CenterOfGravity.X < 800)
                     .ToList(); // also var blobList = ... would be possible. .ToList() prevents lazy evaluation and evaluates linq directly.
            if (blobList.Count < 1)
                blobList =
                    blobs.Where(
                                blob => blob.Rectangle.Y < 100)// && blob.CenterOfGravity.X > 100 && blob.CenterOfGravity.X < 800)
                         .ToList();
            return blobList.Sum(blob => blob.CenterOfGravity.X) / blobList.Count;
        }

        [Log]
        [LogException]
        internal static List<Blob> ClearBlobs(Blob[] blobs, float laserLineX)
        {
            const float safetyFrame = 10f; // pixels directly near laser are seen as laserline

            return
                blobs.Where(
                            blob =>
                                blob.CenterOfGravity.X > laserLineX + safetyFrame &&
                                   // blob.CenterOfGravity.X > 100 && blob.CenterOfGravity.X < 950 &&
                                    blob.CenterOfGravity.Y > 100 && blob.CenterOfGravity.Y < 860)
                     .ToList();
        }

        [Log]
        [LogException]
        public static Bitmap SimpleSkeletonizationImage(Bitmap bitmap)
        {
            SimpleSkeletonization filter = new SimpleSkeletonization();
            return ReturnFilteredImage(bitmap, filter);
        }

        [Log]
        [LogException]
        public static Bitmap CannyEdgeImage(Bitmap bitmap)
        {
            CannyEdgeDetector filter = new CannyEdgeDetector();
            return ReturnFilteredImage(bitmap, filter);
        }

        [Log]
        [LogException]
        public static Bitmap HomogenityEdgeDetectorImage(Bitmap bitmap)
        {
            HomogenityEdgeDetector filter = new HomogenityEdgeDetector();
            return ReturnFilteredImage(bitmap, filter);
        }

        [Log]
        [LogException]
        public static Bitmap SobelEdgeDetectorImage(Bitmap bitmap)
        {
            SobelEdgeDetector filter = new SobelEdgeDetector();
            return ReturnFilteredImage(bitmap, filter);
        }

        [LogException]
        private static Bitmap ReturnFilteredImage(Bitmap bitmap, IInPlaceFilter filter)
        {
            Bitmap returnImage = (Bitmap)bitmap.Clone();
            filter.ApplyInPlace(returnImage);
            return returnImage;
        }

        [Log]
        [LogException]
        public static List<ColorPoint3D> TransformStepImage(Image skeletonImage, Image colorImage, int step, bool microSteps, Information configuration)
        {

            List<ColorPoint3D> points = new List<ColorPoint3D>();
            Blob[] blobs = GetBlobsFromGrayScaleBitmap((Bitmap)skeletonImage);
            float laserLineX = GetLaserLinePosition(blobs);

            ColorPoint3D worldCameraSourcePosition = new ColorPoint3D(configuration.Camera.WorldCoordinates.X,
                                                            configuration.Camera.WorldCoordinates.Y,
                                                            configuration.Camera.WorldCoordinates.Z);

            ColorPoint3D worldLaserSourcePosition = new ColorPoint3D(configuration.Laser.WorldCoordinates.X,
                                                           configuration.Laser.WorldCoordinates.Y,
                                                           configuration.Laser.WorldCoordinates.Z);

            ColorPoint3D worldLaserLinePosition = ConvertCameraPixelToWorldPoint(new ColorPoint3D(laserLineX, 0, 0), configuration);

            Line laserline = new Line(worldLaserSourcePosition, worldLaserLinePosition); // should stay the same

            // filter irrelevant blobs
            List<Blob> relevantBlobs = ClearBlobs(blobs, laserLineX);

            foreach (Blob blob in relevantBlobs)
            {
                ColorPoint3D worldPoint = ComputeDepthByIntersection(blob, worldCameraSourcePosition, laserline, configuration);
                worldPoint.Y = ScaleY(worldPoint, worldCameraSourcePosition);

                if (!AdjustPointToTurnTable(step, ref worldPoint, microSteps, configuration))  // adjust &  test wether point is over turntable
                    continue;
                worldPoint.Color = colorImage != null 
                    ? ((Bitmap) colorImage).GetPixel((int)blob.CenterOfGravity.X, (int)blob.CenterOfGravity.Y) 
                    : Color.White;
                points.Add(worldPoint);
            }

            return points;
        }

        [Log]
        [LogException]
        internal static ColorPoint3D ConvertWorldPointToCameraPixel(this ColorPoint3D worldPoint, Information configuration)
        {
            ColorPoint3D origin = new ColorPoint3D(configuration.Camera.Width / 2.0f,
                                         configuration.Camera.Height * configuration.VisibleLimit.PercentFromTop / 100,
                                         0);

            return new ColorPoint3D(worldPoint.X * configuration.Camera.Width / configuration.Camera.WorldWidth + origin.X,
                               -worldPoint.Y * configuration.Camera.Height / configuration.Camera.WorldHeight + origin.Y,
                               0);
        }

        [Log]
        [LogException]
        internal static ColorPoint3D ConvertCameraPixelToWorldPoint(this ColorPoint3D cameraPoint, Information configuration)
        {
            ColorPoint3D worldPoint = new ColorPoint3D(cameraPoint.X - configuration.Camera.Width / 2.0f,
                                                 cameraPoint.Y - configuration.Camera.Height * configuration.VisibleLimit.PercentFromTop / 100,
                                                 0);

            return new ColorPoint3D(worldPoint.X * configuration.Camera.WorldWidth / configuration.Camera.Width,
                               -worldPoint.Y * configuration.Camera.WorldHeight / configuration.Camera.Height,
                               0);
        }

        [LogException]
        private static ColorPoint3D ComputeDepthByIntersection(Blob blob, ColorPoint3D worldCameraSourcePosition, Line laserline, Information configuration)
        {
            // convert point to world
            ColorPoint3D worldPoint2D = ConvertCameraPixelToWorldPoint(new ColorPoint3D(blob.CenterOfGravity.X, blob.CenterOfGravity.Y, 0), configuration);
            Line viewLine = new Line(worldCameraSourcePosition, worldPoint2D);
            // compute intersection
            ColorPoint3D worldPoint = viewLine.Intersection(laserline);
            worldPoint.Y = worldPoint2D.Y;
            return worldPoint;
        }
        
        [LogException]
        private static double ScaleY(ColorPoint3D worldPoint, ColorPoint3D worldCameraSourcePosition)
        {
            double y = worldPoint.Y - worldCameraSourcePosition.Y;
            y *= (worldCameraSourcePosition.Z - worldPoint.Z) / worldCameraSourcePosition.Z;
            y += worldCameraSourcePosition.Y;
            return y;
        }

        [LogException]
        private static bool AdjustPointToTurnTable(int step, ref ColorPoint3D worldPoint, bool microSteps, Information configuration)
        {
            double turntableDegree = GetTurntableDegree(step, microSteps, configuration);

            worldPoint.Z -= configuration.Turntable.WorldCoordinates.Z;

            double alpha = Math.Atan(worldPoint.Z/worldPoint.X) + turntableDegree*(Math.PI/180.0f);
            double hypo = Math.Sqrt(worldPoint.X*worldPoint.X + worldPoint.Z*worldPoint.Z);

            if (worldPoint.X < 0)
                if (worldPoint.Z < 0)
                    alpha += Math.PI;
                else 
                    alpha -= Math.PI;

            worldPoint.Z = Math.Sin(alpha) * hypo;
            worldPoint.X = Math.Cos(alpha) * hypo;

            double turntableSize = configuration.Turntable.Radius - (float) configuration.Turntable.SafetyDistance/10f;
            double tablePos = Math.Sqrt(worldPoint.Z*worldPoint.Z + worldPoint.X*worldPoint.X);

            return worldPoint.Y > 0.5 && hypo < 7 && tablePos < turntableSize;
        }

        [LogException]
        private static double GetTurntableDegree(int step, bool microSteps, Information configuration)
        {
            int numberOfTotalSteps = microSteps
                ? configuration.Motor.MicroSteps *
                    configuration.Motor.Steps
                : configuration.Motor.Steps;

            double turntableDegree = 360.0f - (double)step * 360.0f / (double)numberOfTotalSteps;
            return turntableDegree;
        }
    }
}