Renamed KalmanFilter to Kalman2DPositionFilter and made it easy to use.

Added KalmanDemo.

bbiwarg/Graphics/KalmanDemo.cs

@@ -0,0 +1,66 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+using System.Drawing;
+
+using Emgu.CV;
+using Emgu.CV.Structure;
+
+using bbiwarg.Utility;
+using bbiwarg.Detectors.Touch;
+
+namespace bbiwarg.Graphics
+{
+    class KalmanDemo
+    {
+        private List<Point> points, kalmanPoints;
+        private Kalman2DPositionFilter kalman;
+
+        public KalmanDemo()
+        {
+            points = new List<Point>();
+            kalmanPoints = new List<Point>();
+        }
+
+        public void addPoint(Point p)
+        {
+            if (points.Count == 0)
+                kalman = new Kalman2DPositionFilter(new Vector2D(p));
+
+            Vector2D v = kalman.getCorrectedPosition(new Vector2D(p));
+
+            points.Add(p);
+            kalmanPoints.Add(new Point(v.IntX, v.IntY));
+        }
+
+        public void reset()
+        {
+            points.Clear();
+            kalmanPoints.Clear();
+        }
+
+        public Int16[] getTextureData(int width, int height)
+        {
+            Image<Bgr, short> image = new Image<Bgr, short>(width, height);
+            image.DrawPolyline(points.ToArray(), false, new Bgr(0, Int16.MaxValue, 0), 1);
+            image.DrawPolyline(kalmanPoints.ToArray(), false, new Bgr(0, 0, Int16.MaxValue), 1);
+
+            Int16[] textureData = new Int16[3 * width * height];
+            int index = 0;
+            for (int y = 0; y < height; ++y) {
+                for (int x = 0; x < width; ++x) {
+                    textureData[index + 0] = image.Data[y, x, 2];
+                    textureData[index + 1] = image.Data[y, x, 1];
+                    textureData[index + 2] = image.Data[y, x, 0];
+
+                    index += 3;
+                }
+            }
+
+            return textureData;
+        }
+    }
+}

bbiwarg/Graphics/OutputWindow.cs

@@ -10,6 +10,8 @@ using OpenTK.Graphics.OpenGL;
 using bbiwarg.Images;
 using bbiwarg.Detectors.Touch;
 
+using Emgu.CV;
+
 namespace bbiwarg.Graphics
 {
     class OutputWindow : GameWindow
@@ -17,16 +19,19 @@ namespace bbiwarg.Graphics
         private VideoHandle videoHandle;
         private uint depthTextureID;
         private uint edgeTextureID;
+        private uint kalmanDemoTextureID;
         private bool paused = false;
         private long timeSpacePressed, timeLeftPressed, timeRightPressed;
         private Stopwatch watch;
+        private KalmanDemo kalmanDemo;
 
         public OutputWindow(VideoHandle videoHandle)
-            : base(3 * videoHandle.Width, 2 * videoHandle.Height)
+            : base(4 * videoHandle.Width, 2 * videoHandle.Height)
         {
             this.videoHandle = videoHandle;
             watch = new Stopwatch();
             watch.Start();
+            kalmanDemo = new KalmanDemo();
         }
 
         protected override void OnLoad(EventArgs e)
@@ -56,6 +61,13 @@ namespace bbiwarg.Graphics
             GL.BindTexture(TextureTarget.Texture2D, edgeTextureID);
             GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear);
             GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear);
+
+            //kalman demo texture
+            GL.GenTextures(1, out kalmanDemoTextureID);
+            GL.BindTexture(TextureTarget.Texture2D, kalmanDemoTextureID);
+            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear);
+            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear);
+
         }
 
         protected override void OnResize(EventArgs e)
@@ -131,9 +143,19 @@ namespace bbiwarg.Graphics
                 foreach (PalmTouchEvent ev in videoHandle.getTouchEvents())
                 {
                     Console.WriteLine("touch at " + ev.Position + " -> " + ev.RelativePalmPosition);
+                    //touchVisualizer.addTouchEvent(ev);
                 }
             }
 
+            // kalman demo
+            int mouseX = Mouse.X - (2 * Width) / 3 - 30;
+            int mouseY = Mouse.Y - 85;
+            if (mouseX >= 0 && mouseX <= videoHandle.Width && mouseY >= 0 && mouseY <= Height)
+                kalmanDemo.addPoint(new Point(mouseX, mouseY));
+            // reset with right click
+            if (OpenTK.Input.Mouse.GetState().IsButtonDown(OpenTK.Input.MouseButton.Right))
+                kalmanDemo.reset();
+
             //draw textures
             Int16[] depthTextureData = new Int16[3 * videoHandle.Width * videoHandle.Height];
             Int16[] edgeTextureData = new Int16[3 * videoHandle.Width * videoHandle.Height];
@@ -204,8 +226,6 @@ namespace bbiwarg.Graphics
                             green = (Int16)(green / 2);
                             blue = (Int16)(blue / 2);
                             break;
-                        
-                        
                     }
 
                     depthTextureData[index] = red;
@@ -229,31 +249,45 @@ namespace bbiwarg.Graphics
                 }
             }
 
+            float size = 0.5f;
+            float size_2 = size / 2.0f;
+            float size_1_5 = 1.5f * size;
+            float gap = 0.0f;
+
+            GL.Enable(EnableCap.Texture2D);
+            GL.BindTexture(TextureTarget.Texture2D, edgeTextureID);
+            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgb, videoHandle.Width, videoHandle.Height, 0, PixelFormat.Rgb, PixelType.Short, edgeTextureData);
+            GL.Begin(PrimitiveType.Quads);
+            GL.Color3(1.0, 1.0, 1.0);
+            GL.TexCoord2(0.0, 0.0); GL.Vertex3(-size_1_5 - gap,  size_2, -0.5);
+            GL.TexCoord2(1.0, 0.0); GL.Vertex3(-size_2   - gap,  size_2, -0.5);
+            GL.TexCoord2(1.0, 1.0); GL.Vertex3(-size_2   - gap, -size_2, -0.5);
+            GL.TexCoord2(0.0, 1.0); GL.Vertex3(-size_1_5 - gap, -size_2, -0.5);
+            GL.End();
+
             GL.Enable(EnableCap.Texture2D);
             GL.BindTexture(TextureTarget.Texture2D, depthTextureID);
             GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgb, videoHandle.Width, videoHandle.Height, 0, PixelFormat.Rgb, PixelType.Short, depthTextureData);
-            float size = 0.5f;
-            float size_2 = (float)(size / 2.0f);
             GL.Begin(PrimitiveType.Quads);
             GL.Color3(1.0, 1.0, 1.0);
-            GL.TexCoord2(0.0, 0.0); GL.Vertex3(0, size_2, -0.5);
-            GL.TexCoord2(1.0, 0.0); GL.Vertex3(size, size_2, -0.5);
-            GL.TexCoord2(1.0, 1.0); GL.Vertex3(size, -size_2, -0.5);
-            GL.TexCoord2(0.0, 1.0); GL.Vertex3(0, -size_2, -0.5);
+            GL.TexCoord2(0.0, 0.0); GL.Vertex3(-size_2,  size_2, -0.5);
+            GL.TexCoord2(1.0, 0.0); GL.Vertex3( size_2,  size_2, -0.5);
+            GL.TexCoord2(1.0, 1.0); GL.Vertex3( size_2, -size_2, -0.5);
+            GL.TexCoord2(0.0, 1.0); GL.Vertex3(-size_2, -size_2, -0.5);
             GL.End();
 
             GL.Enable(EnableCap.Texture2D);
-            GL.BindTexture(TextureTarget.Texture2D, edgeTextureID);
-            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgb, videoHandle.Width, videoHandle.Height, 0, PixelFormat.Rgb, PixelType.Short, edgeTextureData);
+            GL.BindTexture(TextureTarget.Texture2D, kalmanDemoTextureID);
+            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgb, videoHandle.Width, videoHandle.Height, 0, PixelFormat.Rgb, PixelType.Short, 
+                kalmanDemo.getTextureData(videoHandle.Width, videoHandle.Height));
             GL.Begin(PrimitiveType.Quads);
             GL.Color3(1.0, 1.0, 1.0);
-            GL.TexCoord2(0.0, 0.0); GL.Vertex3(0, -size / 2.0, -0.5);
-            GL.TexCoord2(-1.0, 0.0); GL.Vertex3(-size, -size / 2.0, -0.5);
-            GL.TexCoord2(-1.0, -1.0); GL.Vertex3(-size, size / 2.0, -0.5);
-            GL.TexCoord2(0.0, -1.0); GL.Vertex3(0, size / 2.0, -0.5);
+            GL.TexCoord2(0.0, 0.0); GL.Vertex3(size_2   + gap,  size_2, -0.5);
+            GL.TexCoord2(1.0, 0.0); GL.Vertex3(size_1_5 + gap,  size_2, -0.5);
+            GL.TexCoord2(1.0, 1.0); GL.Vertex3(size_1_5 + gap, -size_2, -0.5);
+            GL.TexCoord2(0.0, 1.0); GL.Vertex3(size_2   + gap, -size_2, -0.5);
             GL.End();
 
-
             SwapBuffers();
         }
 

bbiwarg/Utility/Kalman2DPositionFilter.cs

@@ -0,0 +1,76 @@
+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 Kalman2DPositionFilter
+    {
+        private Kalman kalman;
+
+        // state: x, y, v_x, v_y
+        //   x: current x position
+        //   y: current y position
+        // v_x: velocity in x direction
+        // v_y: velocity in y direction
+        public Kalman2DPositionFilter(Vector2D initialPosition, float measurementNoiseFactor = 1.0e-1f, float processNoiseFactor = 1.0e-4f)
+        {
+            // 4 state variables and 2 measurements (0 controls)
+            kalman = new Kalman(4, 2, 0);
+
+            // initial state (x, y, v_x, v_y)
+            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y, 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},   // x = x + v_x
+              {0.0f, 1.0f, 0.0f, 1.0f},   // y = y + v_y
+              {0.0f, 0.0f, 1.0f, 0.0f},   // v_x = v_x
+              {0.0f, 0.0f, 0.0f, 1.0f}}); // v_y = v_y
+            kalman.TransitionMatrix = transitionMatrix;
+
+            // measurement matrix
+            Matrix<float> measurementMatrix = new Matrix<float>(new float[,]
+            { {1.0f, 0.0f, 0.0f, 0.0f}, // first measurement = x
+              {0.0f, 1.0f, 0.0f, 0.0f}  // second measurement = y
+            });
+            kalman.MeasurementMatrix = measurementMatrix;
+
+            // measurement noise covariance matrix
+            Matrix<float> measurementNoiseCovarianceMatrix = new Matrix<float>(2, 2);
+            measurementNoiseCovarianceMatrix.SetIdentity(new MCvScalar(measurementNoiseFactor));
+            kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;
+
+            // process noise covariance matrix
+            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(4, 4);
+            processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
+            kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;
+
+            // error covariance post matrix (initial value)
+            Matrix<float> errorCovariancePostMatrix = new Matrix<float>(4, 4);
+            errorCovariancePostMatrix.SetIdentity();
+            kalman.ErrorCovariancePost = errorCovariancePostMatrix;
+        }
+
+        // updates the filter and returns the corrected position
+        public Vector2D getCorrectedPosition(Vector2D rawPosition)
+        {
+            Matrix<float> rawPositionMatrix = new Matrix<float>(new float[,] 
+            { {rawPosition.X},
+              {rawPosition.Y}});
+
+            // prediction according to model
+            Matrix<float> prediction = kalman.Predict();
+
+            // corrected point
+            Matrix<float> estimate = kalman.Correct(rawPositionMatrix);
+            return new Vector2D(estimate[0, 0], estimate[1, 0]);
+        }
+    }
+}

bbiwarg/Utility/KalmanFilter.cs

@@ -1,110 +0,0 @@
-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

@@ -78,6 +78,7 @@
     <Compile Include="Detectors\Touch\TouchEvent.cs" />
     <Compile Include="Detectors\Touch\TouchTracker.cs" />
     <Compile Include="Graphics\OutputWindow.cs" />
+    <Compile Include="Graphics\KalmanDemo.cs" />
     <Compile Include="Images\FingerImage.cs" />
     <Compile Include="Images\DepthImage.cs" />
     <Compile Include="Images\EdgeImage.cs" />
@@ -89,7 +90,7 @@
     <Compile Include="MainBBWIWARG.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
     <Compile Include="Utility\HelperFunctions.cs" />
-    <Compile Include="Utility\KalmanFilter.cs" />
+    <Compile Include="Utility\Kalman2DPositionFilter.cs" />
     <Compile Include="Utility\Line2D.cs" />
     <Compile Include="Utility\LineSegment2D.cs" />
     <Compile Include="Utility\Quadrangle.cs" />