Changed Kalman filter to conform to the equations of motion.

bbiwarg/Constants.cs

@@ -50,13 +50,13 @@ namespace bbiwarg
         // palm detection
         public static readonly float PalmMinDefectMidFingerLineDistance = 20; // defects with mid point ((start + end) / 2) closer than this to a finger line are removed 
         public static readonly float PalmMaxThumbDefectAngle = 110; // degree
-        public static readonly float PalmMaxThumbDefectStartEndLengthQuotient = 2.2f;
+        public static readonly float PalmMaxThumbDefectStartEndLengthQuotient = 2.3f;
         public static readonly float PalmMinThumbDefectStartEndLengthQuotient = 1.2f;
         public static readonly float PalmMinTumbDefectDepthThumbLengthQuotient = 0.8f;
         public static readonly float PalmMaxTumbDefectDepthThumbLengthQuotient = 1.2f;
         
         //palm Grid
-        public static readonly int PalmGridRows = 4;
+        public static readonly int PalmGridRows = 3;
         public static readonly int PalmGridColumns = 3;
 
         // touch detection

bbiwarg/Detectors/PalmDetection/PalmDetector.cs

@@ -33,9 +33,9 @@ namespace bbiwarg.Detectors.PalmDetection
 
         public PalmDetector()
         {
-            thumbDefectDepthFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
-            thumbDefectStartFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
-            thumbDefectEndFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
+            thumbDefectDepthFilter = new Kalman2DPositionFilter(5.0f, 1.0e+1f);
+            thumbDefectStartFilter = new Kalman2DPositionFilter(5.0f, 1.0e+1f);
+            thumbDefectEndFilter = new Kalman2DPositionFilter(5.0f, 1.0e+1f);
         }
 
         public void findPalmQuad(OutputImage outputImage, List<Hand> hands)
@@ -232,7 +232,7 @@ namespace bbiwarg.Detectors.PalmDetection
                 topLeft = longestLineEndpoint + 0.15f * handLength;
                 bottomLeft = thumbDefectDepth - 0.4f * handLength;
                 bottomRight = bottomLeft + handWidth;
-                topRight = bottomRight + 1.15f * handLength - 0.35f * handWidth;
+                topRight = bottomRight + 1.2f * handLength - 0.3f * handWidth;
 
                 PalmQuad = new Quadrangle(bottomLeft, topLeft, topRight, bottomRight);
             }

bbiwarg/Graphics/TouchEventVisualizer.cs

@@ -35,7 +35,7 @@ namespace bbiwarg.Graphics
             {
                 lastTouchPositions = currentTouchPositions;
                 currentTouchPositions = new List<Vector2D>();
-                kalmanFilter = new Kalman2DPositionFilter();
+                kalmanFilter = new Kalman2DPositionFilter(1.0e-4f, 1.0e-5f);
                 kalmanFilter.setInitialPosition(e.RelativePalmPosition);
                 pos = e.RelativePalmPosition;
             }

bbiwarg/Images/DepthImage.cs

@@ -70,15 +70,5 @@ namespace bbiwarg.Images
 
             return (Int16)min[0];
         }
-
-        public void removeBackground(List<Hand> hands) {
-            Image<Gray, byte> mask = Image.CopyBlank();
-            foreach (Hand hand in hands) {
-                mask = mask.Or(hand.Mask);
-            }
-
-
-            Image = Image.Or(255-mask);
-        }
     }
 }

bbiwarg/Utility/Kalman2DPositionFilter.cs

@@ -13,6 +13,7 @@ namespace bbiwarg.Utility
     {
         private Kalman kalman;
         private float measurementNoiseFactor, processNoiseFactor;
+        private float fps;
 
         public bool Initialized { get; private set; }
 
@@ -21,30 +22,38 @@ namespace bbiwarg.Utility
         //   y: current y position
         // v_x: velocity in x direction
         // v_y: velocity in y direction
-        public Kalman2DPositionFilter(float measurementNoiseFactor = 1.0e-1f, float processNoiseFactor = 1.0e-4f)
+        // a_x: acceleration in x direction
+        // a_y: acceleration in y direction
+        public Kalman2DPositionFilter(float measurementNoiseFactor = 1.0e-1f, float processNoiseFactor = 1.0e-4f, int fps = 30)
         {
             this.measurementNoiseFactor = measurementNoiseFactor;
             this.processNoiseFactor = processNoiseFactor;
+            this.fps = fps;
             reset();
         }
 
         public void reset()
         {
-            // 4 state variables and 2 measurements (0 controls)
-            kalman = new Kalman(4, 2, 0);
+            // 6 state variables and 2 measurements (0 controls)
+            kalman = new Kalman(6, 2, 0);
+
+            // time step (s)
+            float t = 1 / fps;
 
             // 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
+            { {1.0f, 0.0f, 1.0f * t, 0.0f, 0.5f * t * t, 0.0f},  
+              {0.0f, 1.0f, 0.0f, 1.0f * t, 0.0f, 0.5f * t * t},  
+              {0.0f, 0.0f, 1.0f, 0.0f, 1.0f * t, 0.0f},  
+              {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f * t},  
+              {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f},  
+              {0.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}, // first measurement = x
-              {0.0f, 1.0f, 0.0f, 0.0f}  // second measurement = y
+            { {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // first measurement = x
+              {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}  // second measurement = y
             });
             kalman.MeasurementMatrix = measurementMatrix;
 
@@ -54,13 +63,13 @@ namespace bbiwarg.Utility
             kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;
 
             // process noise covariance matrix
-            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(4, 4);
+            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(6, 6);
             processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
             kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;
 
             // error covariance post matrix (initial value)
-            Matrix<float> errorCovariancePostMatrix = new Matrix<float>(4, 4);
-            errorCovariancePostMatrix.SetIdentity();
+            Matrix<float> errorCovariancePostMatrix = new Matrix<float>(6, 6);
+            errorCovariancePostMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
             kalman.ErrorCovariancePost = errorCovariancePostMatrix;
 
             Initialized = false;
@@ -69,7 +78,7 @@ namespace bbiwarg.Utility
         public void setInitialPosition(Vector2D initialPosition)
         {
             // initial state (x, y, v_x, v_y)
-            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y, 0.0f, 0.0f });
+            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y, 0.0f, 0.0f, 0.0f, 0.0f });
             kalman.CorrectedState = initialState;
 
             Initialized = true;
@@ -89,7 +98,7 @@ namespace bbiwarg.Utility
               {rawPosition.Y}});
 
             // prediction according to model
-            Matrix<float> prediction = kalman.Predict();
+            kalman.Predict();
 
             // corrected point
             Matrix<float> estimate = kalman.Correct(rawPositionMatrix);