Added image warpperspective

Engine/Emgu.CV.External/Extensions/EmguExtensions.cs

@@ -131,7 +131,7 @@ public static BitmapSource ToGradientBitmapSource(this Image<Gray, float> image,
                     {
                         var index = (int)((Gradient.Length - 1) * (255.0 - depth) / 255.0);
                         if (index < 0) index = 0;
-                        if (index > Gradient.Length) index = Gradient.Length - 1;
+                        if (index >= Gradient.Length) index = Gradient.Length - 1;
                         color = Gradient[index];
                     }
                     gradientImage[y, x] = color;

Engine/Huddle.Engine/App.config

@@ -1,17 +1,29 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8"?>
 <configuration>
   <configSections>
-    <sectionGroup name="userSettings" type="System.Configuration.UserSettingsGroup, System, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" >
+    <sectionGroup name="userSettings" type="System.Configuration.UserSettingsGroup, System, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
       <section name="Huddle.Engine.Properties.Settings" type="System.Configuration.ClientSettingsSection, System, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" allowExeDefinition="MachineToLocalUser" requirePermission="false" />
     </sectionGroup>
   </configSections>
   <runtime>
     <assemblyBinding xmlns="urn:schemas-microsoft-com:asm.v1">
-      <probing privatePath="External"/>
+      <probing privatePath="External" />
+      <dependentAssembly>
+        <assemblyIdentity name="AForge.Math" publicKeyToken="abba2e25397ee8c9" culture="neutral" />
+        <bindingRedirect oldVersion="0.0.0.0-2.2.5.0" newVersion="2.2.5.0" />
+      </dependentAssembly>
+      <dependentAssembly>
+        <assemblyIdentity name="AForge" publicKeyToken="c1db6ff4eaa06aeb" culture="neutral" />
+        <bindingRedirect oldVersion="0.0.0.0-2.2.5.0" newVersion="2.2.5.0" />
+      </dependentAssembly>
+      <dependentAssembly>
+        <assemblyIdentity name="AForge.Imaging" publicKeyToken="ba8ddea9676ca48b" culture="neutral" />
+        <bindingRedirect oldVersion="0.0.0.0-2.2.5.0" newVersion="2.2.5.0" />
+      </dependentAssembly>
     </assemblyBinding>
   </runtime>
   <startup>
-    <supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.5.1"/>
+    <supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.5.1" />
   </startup>
   <userSettings>
     <Huddle.Engine.Properties.Settings>

Engine/Huddle.Engine/Data/RotationMatrixData.cs

@@ -0,0 +1,72 @@
+using Emgu.CV;
+using Huddle.Engine.Processor;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace Huddle.Engine.Data
+{
+    public class RotationMatrixData : BaseData
+    {
+        #region properties
+
+        #region RotationMatrix
+
+        /// <summary>
+        /// The <see cref="RotationMatrix" /> property's name.
+        /// </summary>
+        public const string RotationMatrixPropertyName = "RotationMatrix";
+
+        private Matrix<double> _rotationMatrix;
+
+        /// <summary>
+        /// Sets and gets the RotationMatrix property.
+        /// Changes to that property's value raise the PropertyChanged event. 
+        /// </summary>
+        public Matrix<double> RotationMatrix
+        {
+            get
+            {
+                return _rotationMatrix;
+            }
+
+            set
+            {
+                if (_rotationMatrix == value)
+                {
+                    return;
+                }
+
+                RaisePropertyChanging(RotationMatrixPropertyName);
+                _rotationMatrix = value;
+                RaisePropertyChanged(RotationMatrixPropertyName);
+            }
+        }
+
+        #endregion
+
+        #endregion
+
+        #region ctor
+
+        public RotationMatrixData(IProcessor source, string key, Matrix<double> rotationMatrix)
+            : base(source, key)
+        {
+            RotationMatrix = rotationMatrix;
+        }
+
+        #endregion
+
+        public override IData Copy()
+        {
+            return new RotationMatrixData(Source, Key, RotationMatrix);
+        }
+
+        public override void Dispose()
+        {
+
+        }
+    }
+}

Engine/Huddle.Engine/Huddle.Engine.csproj

@@ -162,17 +162,26 @@
     <Prefer32Bit>true</Prefer32Bit>
   </PropertyGroup>
   <ItemGroup>
-    <Reference Include="AForge, Version=2.2.5.0, Culture=neutral, PublicKeyToken=c1db6ff4eaa06aeb, processorArchitecture=MSIL">
+    <Reference Include="Accord, Version=2.14.0.0, Culture=neutral, PublicKeyToken=fa1a88e29555ccf7, processorArchitecture=MSIL">
       <SpecificVersion>False</SpecificVersion>
-      <HintPath>..\..\lib\Aforge.NET\AForge.dll</HintPath>
+      <HintPath>..\..\packages\Accord.2.14.0\lib\net40\Accord.dll</HintPath>
     </Reference>
-    <Reference Include="AForge.Imaging, Version=2.2.5.0, Culture=neutral, PublicKeyToken=ba8ddea9676ca48b, processorArchitecture=MSIL">
+    <Reference Include="Accord.Math, Version=2.14.0.0, Culture=neutral, PublicKeyToken=fa1a88e29555ccf7, processorArchitecture=MSIL">
       <SpecificVersion>False</SpecificVersion>
-      <HintPath>..\..\lib\Aforge.NET\AForge.Imaging.dll</HintPath>
+      <HintPath>..\..\packages\Accord.Math.2.14.0\lib\net40\Accord.Math.dll</HintPath>
     </Reference>
-    <Reference Include="AForge.Math, Version=2.2.5.0, Culture=neutral, PublicKeyToken=abba2e25397ee8c9, processorArchitecture=MSIL">
+    <Reference Include="Accord.Statistics, Version=2.14.0.0, Culture=neutral, PublicKeyToken=fa1a88e29555ccf7, processorArchitecture=MSIL">
       <SpecificVersion>False</SpecificVersion>
-      <HintPath>..\..\lib\Aforge.NET\AForge.Math.dll</HintPath>
+      <HintPath>..\..\packages\Accord.Statistics.2.14.0\lib\net40\Accord.Statistics.dll</HintPath>
+    </Reference>
+    <Reference Include="AForge, Version=2.2.5.0, Culture=neutral, PublicKeyToken=c1db6ff4eaa06aeb, processorArchitecture=MSIL">
+      <HintPath>..\..\packages\AForge.2.2.5\lib\AForge.dll</HintPath>
+    </Reference>
+    <Reference Include="AForge.Imaging, Version=2.2.5.0, Culture=neutral, PublicKeyToken=ba8ddea9676ca48b, processorArchitecture=MSIL">
+      <HintPath>..\..\packages\AForge.Imaging.2.2.5\lib\AForge.Imaging.dll</HintPath>
+    </Reference>
+    <Reference Include="AForge.Math, Version=2.2.5.0, Culture=neutral, PublicKeyToken=abba2e25397ee8c9, processorArchitecture=MSIL">
+      <HintPath>..\..\packages\AForge.Math.2.2.5\lib\AForge.Math.dll</HintPath>
     </Reference>
     <Reference Include="AForge.Vision.GlyphRecognition, Version=2.0.1.0, Culture=neutral, PublicKeyToken=e0a55246fcae183a, processorArchitecture=MSIL">
       <SpecificVersion>False</SpecificVersion>
@@ -309,6 +318,7 @@
       <DependentUpon>ExceptionMessageBox.xaml</DependentUpon>
     </Compile>
     <Compile Include="Data\Device.cs" />
+    <Compile Include="Data\RotationMatrixData.cs" />
     <Compile Include="Extensions\BitmapSourceExtensions.cs" />
     <Compile Include="Extensions\CollectionExtensions.cs" />
     <Compile Include="Extensions\DispatcherExtensions.cs" />
@@ -321,7 +331,10 @@
     <Compile Include="Filter\NoSmoothing.cs" />
     <Compile Include="Filter\OneEuroSmoothing.cs" />
     <Compile Include="Filter\SmoothingFilterFactory.cs" />
+    <Compile Include="Processor\Complex\CameraRotation.cs" />
+    <Compile Include="Processor\Complex\ImageWarpPerspective.cs" />
     <Compile Include="Processor\Complex\PolygonIntersection\PolygonCollisionUtils.cs" />
+    <Compile Include="Processor\Complex\RansacFitPlane.cs" />
     <Compile Include="Processor\MergeRgbImageAndDevice.cs" />
     <Compile Include="Processor\Network\Huddle\Client.cs" />
     <Compile Include="Processor\Network\Huddle\ClientState.cs" />

Engine/Huddle.Engine/Processor/Complex/CameraRotation.cs

@@ -0,0 +1,158 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Diagnostics;
+using System.Linq;
+using System.Runtime.Serialization;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+using GalaSoft.MvvmLight;
+using GalaSoft.MvvmLight.Threading;
+using Huddle.Engine.Data;
+using Huddle.Engine.Util;
+using Emgu.CV;
+using Emgu.CV.CvEnum;
+using Emgu.CV.Structure;
+using Accord.Math;
+
+namespace Huddle.Engine.Processor.Complex
+{
+    /// <summary>
+    /// calculate the table plane and the camera rotation
+    /// established on 27/4/2015 by Yunlong
+    /// </summary>
+    [ViewTemplate("Camera Rotation", "CameraRotation")]
+    public class CameraRotation : BaseProcessor
+    {
+        Image<Gray,float> depthImage1 = null;
+        Image<Gray, float> depthImage2 = null;
+        Image<Gray, float> depthImage3 = null;
+        Image<Gray, float> depthImage4 = null;
+        Image<Gray, float> depthImage0 = null;
+        double[] eula0 = { 0.0, 0.0, 0.0 };
+        bool dataReady = false;
+        int shakeNum = 0;
+        bool init = false;
+        int cnt = 0;
+        Matrix<double> cameraRotation = new Matrix<double>(3, 3);
+        Matrix<double> rotationMatrix;
+
+        public override IDataContainer PreProcess(IDataContainer dataContainer)
+        {         
+            // EmguCv depth image
+            foreach (GrayFloatImage imageData in dataContainer.OfType<GrayFloatImage>())
+            {
+                //initialize Matrix homoGraph
+                if (!init)
+                {
+                    Init();
+                    init = true;
+                }
+                cnt++;
+                if (cnt == 5)
+                {
+                    Stopwatch sw = Stopwatch.StartNew();
+                    rotationMatrix = GetCameraRotation(imageData.Image);
+                    Console.WriteLine("time Rotation: " + sw.ElapsedMilliseconds);
+                    cnt = 0;
+                }
+                Stage(new RotationMatrixData(this, "cameraRotation", rotationMatrix));
+            }
+
+            Push();
+            return null;
+        }
+
+        public override IData Process(IData data)
+        {
+            return null;
+        }
+
+        private void Init(){
+            cameraRotation.SetZero();
+            cameraRotation.Data[0, 0] = 1; cameraRotation.Data[1, 1] = 1; cameraRotation.Data[2, 2] = 1;
+        }
+
+        private Matrix<double> GetCameraRotation(Image<Gray, float> depthImage)
+        {
+            //time filter
+            if (!dataReady)
+            {
+                depthImage1 = depthImage2 = depthImage3 = depthImage4 = depthImage;
+                dataReady = true;
+            }
+            depthImage0 = depthImage;
+            depthImage = (depthImage0 + depthImage1 + depthImage2 + depthImage3 + depthImage4) / 5.0;
+            depthImage4 = depthImage3; depthImage3 = depthImage2; depthImage2 = depthImage1; depthImage1 = depthImage0;
+
+            //median filter
+            depthImage = depthImage.SmoothMedian(3);
+
+            //subImage  !! depthImage.Data [240,320,1] !!                      
+            int padding = 8;
+            int widthSubImage = (int)Math.Ceiling(depthImage.Width * (1.0 - (2.0 / padding)));
+            int heightSubImage = (int)Math.Ceiling(depthImage.Height * (1.0 - (2.0 / padding)));
+            int originalX = (int)Math.Ceiling(depthImage.Width * (1.0 / padding));
+            int originalY = (int)Math.Ceiling(depthImage.Height * (1.0 / padding));
+            Image<Gray, float> depthImageSub = new Image<Gray, float>(widthSubImage, heightSubImage);
+            for (int j = 0; j < heightSubImage; j++)
+            {
+                for (int i = 0; i < widthSubImage; i++)
+                {
+                    depthImageSub.Data[j, i, 0] = depthImage.Data[originalY + j, originalX + i, 0];
+                }
+            }
+
+            //sampling 
+            //convent to world coordinates
+            //move coordinate original to center
+            int samplingSpace = 3;
+            int widthSampling = widthSubImage / samplingSpace;
+            int heightSampling = heightSubImage / samplingSpace;
+            double focalLengthDepth = depthImage.Width / (2 * Math.Tan((double)(74 * 3.14 / 180) / 2));
+            float[, ,] depthData = depthImageSub.Data;
+            Point3[] depthDataSamples = new Point3[widthSampling * heightSampling];
+            for (int j = 0; j < heightSampling; j++)
+            {
+                for (int i = 0; i < widthSampling; i++)
+                {
+                    float z = Math.Abs(depthData[samplingSpace * j, samplingSpace * i, 0]);
+                    float x = (float)((originalX + i * samplingSpace - depthImage.Width / 2.0) * z / focalLengthDepth);
+                    float y = (float)((originalY + j * samplingSpace - depthImage.Height / 2.0) * z / focalLengthDepth);
+                    depthDataSamples[j * widthSampling + i] = new Point3(x, y, z);
+                }
+            }
+            //Ransac                       
+            Stopwatch sw = Stopwatch.StartNew();
+            RansacFitPlane ransacFitPlane = new RansacFitPlane(depthDataSamples, 5, 20);
+            Plane plane = ransacFitPlane.FitPlane();
+            if (plane != null)
+            {
+                Console.WriteLine(plane.A / plane.Offset + " " + plane.B / plane.Offset + " " + plane.C / plane.Offset + " " + plane.Offset);
+
+                //calc rotation matrix
+                double[] eula = { Math.Atan((plane.B) / (plane.C)), -Math.Atan((plane.A) / (plane.C)), 0 };
+                Console.WriteLine("Xaxis: " + Math.Atan((plane.B) / (plane.C)) * 180 / Math.PI + " Yaxis: " + Math.Atan((plane.A) / (plane.C)) * 180 / Math.PI);
+                //avoid shaking
+                if (Math.Abs(eula0[0] - eula[0]) > 2 * Math.PI / 180 || Math.Abs(eula0[1] - eula[1]) > 2 * Math.PI / 180)
+                {
+                    shakeNum++;
+                    if (shakeNum == 5)
+                    {
+                        shakeNum = 0;
+                        eula0[0] = eula[0]; eula0[1] = eula[1];
+                    }
+                }
+                else
+                {
+                    shakeNum = 0;
+                }
+                cameraRotation = RansacFitPlane.EulaArray2Matrix(eula0);
+                Console.WriteLine("time: " + sw.ElapsedMilliseconds);
+
+            }
+            return cameraRotation;
+        }    
+    }
+}