Added very basic Kalman Filter example.

bbiwarg/Preprocessing/BackgroundSubtractor.cs

@@ -1,44 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
-
-using bbiwarg.Images;
-
-using Emgu.CV;
-using Emgu.CV.Structure;
-using Emgu.CV.VideoSurveillance;
-
-namespace bbiwarg.Preprocessing
-{
-    class BackgroundSubtractor
-    {
-        private BackgroundSubtractorMOG subtractor;
-
-        public BackgroundSubtractor()
-        {
-            subtractor = new BackgroundSubtractorMOG(0, 0, 0.0, 0.0); // defaults
-        }
-
-        public void update(Image<Bgr, byte> currentColorImage)
-        {
-            subtractor.Update(currentColorImage);
-        }
-
-        public Image<Gray, short> getForeground(Image<Gray, short> image)
-        {
-            Image<Gray, short> foreground = image.Clone();
-            foreground = foreground.And(subtractor.ForegroundMask.Convert<short>(delegate(byte b) { if (b == 0) return 0; else return short.MaxValue; }));
-
-            int numNonZero = 0;
-            for (int i = 0; i < image.Width; ++i)
-                for (int j = 0; j < image.Height; ++j)
-                    if (subtractor.ForegroundMask.Data[j, i, 0] != 0)
-                        ++numNonZero;
-            Console.WriteLine("non zero: " + numNonZero);
-
-            return subtractor.ForegroundMask.Convert<short>(delegate(byte b) { return (b == 0) ? (short) 0 : (short) 100; });
-        }
-    }
-}

bbiwarg/Utility/KalmanFilter.cs

@@ -0,0 +1,110 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+using Emgu.CV;
+using Emgu.CV.Structure;
+
+namespace bbiwarg.Utility
+{
+    class KalmanFilter
+    {
+        public KalmanFilter()
+        {
+            Kalman kalman = new Kalman(4, 2, 0);
+
+            // initial state
+            Matrix<float> initialState = new Matrix<float>(new float[] { 0.0f, 0.0f, 0.0f, 0.0f});
+            kalman.CorrectedState = initialState;
+
+            // transition matrix
+            Matrix<float> transitionMatrix = new Matrix<float>(new float[,] 
+            { {1.0f, 0.0f, 1.0f, 0.0f},
+              {0.0f, 1.0f, 0.0f, 1.0f},
+              {0.0f, 0.0f, 1.0f, 0.0f},
+              {0.0f, 0.0f, 0.0f, 1.0f}});
+            kalman.TransitionMatrix = transitionMatrix;
+
+            // measurement matrix
+            Matrix<float> measurementMatrix = new Matrix<float>(new float[,]
+            { {1.0f, 0.0f, 0.0f, 0.0f},
+              {0.0f, 1.0f, 0.0f, 0.0f}
+            });
+            kalman.MeasurementMatrix = measurementMatrix;
+
+            // measurement noise covariance matrix
+            Matrix<float> measurementNoiseCovarianceMatrix = new Matrix<float>(2, 2);
+            measurementNoiseCovarianceMatrix.SetIdentity(new MCvScalar(1.0e-1));
+            kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;
+
+            // process noise covariance matrix
+            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(4, 4);
+            processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(1.0e-4));
+            kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;
+
+            // error covariance post matrix (initial value)
+            Matrix<float> errorCovariancePostMatrix = new Matrix<float>(4, 4);
+            errorCovariancePostMatrix.SetIdentity();
+            kalman.ErrorCovariancePost = errorCovariancePostMatrix;
+
+
+            Random random = new Random();
+            int[] randomData = new int[70];
+
+            for (int i = 0; i < 70; ++i)
+            {
+                int y = random.Next() % 21 - 10;
+                randomData[i] = y;
+            }
+
+            int[] filteredData = new int[70];
+            for (int i = 0; i < 70; ++i)
+            {
+                // random data (could be touch position)
+                Matrix<float> data = new Matrix<float>(new float[,]
+                { {0.0f},
+                  {randomData[i]}});
+
+                // prediction according to model
+                Matrix<float> prediction = kalman.Predict();
+            
+                // estimated point
+                Matrix<float> estimate = kalman.Correct(data);
+                filteredData[i] = (int) estimate[1, 0];
+            }
+
+
+            Console.WriteLine("unfiltered: ");
+            for (int i = 0; i < 21; ++i)
+            {
+                int val = 10 - i;
+                for (int j = 0; j < 70; ++j)
+                {
+                    if (randomData[j] >= val)
+                        Console.Write("#");
+                    else
+                        Console.Write(' ');
+                }
+                Console.WriteLine();
+            }
+
+            Console.WriteLine("filtered: ");
+            for (int i = 0; i < 21; ++i)
+            {
+                int val = 10 - i;
+                for (int j = 0; j < 70; ++j)
+                {
+                    if (filteredData[j] >= val)
+                        Console.Write("#");
+                    else
+                        Console.Write(' ');
+                }
+                Console.WriteLine();
+            }
+
+            Console.Read();
+        }
+    }
+}

bbiwarg/bbiwarg.csproj

@@ -89,6 +89,7 @@
     <Compile Include="MainBBWIWARG.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
     <Compile Include="Utility\HelperFunctions.cs" />
+    <Compile Include="Utility\KalmanFilter.cs" />
     <Compile Include="Utility\Line2D.cs" />
     <Compile Include="Utility\Quadrangle.cs" />
     <Compile Include="Utility\Vector.cs" />