Click here to Skip to main content
Click here to Skip to main content
 
Add your own
alternative version

Image Tracking and Computer Vision Using Fourier Image Correlation

, 23 Apr 2013
How to teach a program to recognize something within a video stream.
OpenCVDemo3-noexe.zip
OpenCVDemo
Description.odt
OpenCvSharp-2.3.1-x86-20111030
XmlDoc-English
XmlDoc-Japanese
TrackingEyes.jpg
WindowsFormsApplication1
Properties
Settings.settings
OpenCVDemo3.zip
Description.odt
OpenCv-2.3.1-x86
opencv_calib3d231.dll
opencv_contrib231.dll
opencv_core231.dll
opencv_features2d231.dll
opencv_ffmpeg.dll
opencv_flann231.dll
opencv_gpu231.dll
opencv_highgui231.dll
opencv_imgproc231.dll
opencv_legacy231.dll
opencv_ml231.dll
opencv_objdetect231.dll
opencv_ts231.dll
opencv_video231.dll
tbb.dll
OpenCvSharp.Blob.dll
OpenCvSharp.CPlusPlus.dll
OpenCvSharp.DebuggerVisualizers.dll
OpenCvSharp.dll
OpenCvSharp.Extensions.dll
OpenCvSharp.MachineLearning.dll
OpenCvSharp.UserInterface.dll
OpenCvSharpExtern.dll
TrackingEyes.jpg
Settings.settings
///////////////////////////////////////////////////////////////////////////////
//
//  PositionFilter.cs
//
//  By Philip R. Braica (HoshiKata@aol.com, VeryMadSci@gmail.com)
//
//  Distributed under the The Code Project Open License (CPOL)
//  http://www.codeproject.com/info/cpol10.aspx
///////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace OpenCVDemo
{
    /// <summary>
    /// Kalman filter, 2D state, 1D measurement.
    ///             
    /// Algorithm:
    /// X = Predicted
    /// P = Covariance
    /// Y = Residual
    /// M = Measured
    /// S = Residual Covariance
    /// R = Minimal inovative covariance.
    /// K = Kalman gain
    /// Q = Minimal update covariance of P
    /// H = Rolls actual to predicted: Measured = H*X+ + noise.
    /// I = Identity matrix.
    /// 
    /// Move prediction from old time to new time.
    ///   X = F*X + B*U
    ///   P = F*X*F^T + Q
    /// 
    /// Update:
    ///   Y = M - H*X
    ///   S = H*P*H^T  + R
    ///   K = P * H^T * S^-1
    ///   X = X + K * Y        upates to this time.
    ///   P = (I - K * H) * P  update to this time.
    ///
    /// Example:
    ///   X = x, v   position and velocity.
    /// Predict
    ///   x = = F * X + H * U
    ///   F = [1, dt], [0, 1], 
    ///   H * U = 0 'cause no controlled inputs.
    ///   Q = [min x to x cov., min vel to x cov.], [min pos to vel cov., min vel to vel cov.]
    ///   H = [1, 0]  => we measure only the 1 value.
    ///   R = Expectation of measurement noise.
    ///   Initialize P always to a diagonal large matrix.        
    /// </summary>
    public class KalmanFilter
    {
        /// <summary>
        /// Constructor.
        /// </summary>
        public KalmanFilter()
        {
            Reset();
        }

        #region Protected stuff: m_x, m_v, m_p, m_q, m_r.

        /// <summary>
        /// Position.
        /// </summary>
        protected double m_x = 0;

        /// <summary>
        /// Velocity.
        /// </summary>
        protected double m_v = 0;

        /// <summary>
        /// Covariance vector.
        /// </summary>
        protected double[] m_p = new double[4];

        /// <summary>
        /// Minimal prediction covariance.
        /// </summary>
        protected double[] m_q = new double[2];

        /// <summary>
        /// Measurement covariance.
        /// </summary>
        protected double m_r = 0;
        #endregion

        /// <summary>
        /// Variance.
        /// </summary>
        public double Variance
        { get { return System.Math.Abs(m_p[0]); } }

        /// <summary>
        /// The current value after last update.
        /// </summary>
        public double Value
        { get { return m_x; } }

        /// <summary>
        /// Setup the filter.
        /// </summary>
        public void Reset()
        {
            m_q[0] = 50;    // Predicted initial position covariance in pixels.
            m_q[1] = 30;    // Predicted initial velocity covariance in pixels.

            m_r = 5;        // Measure to measure covariance

            m_p[0] = 10000000;
            m_p[1] = 0;
            m_p[2] = 0;
            m_p[3] = 10000000;

            m_x = 0;
            m_v = 0;
        }

        /// <summary>
        /// Reset to an initial position.
        /// </summary>
        /// <param name="x"></param>
        public void Reset(int x)
        {
            Reset();
            m_x = x;
        }

        /// <summary>
        /// Update the value.
        /// </summary>
        /// <param name="m">Measurement.</param>
        public double Update(double m)
        {
            // Move prediction from old time to new time.
            //   X = F*X + B*U
            //   P = F*X*F^T + Q
            // 
            // Update:
            //   Y = M - H*X
            //   S = H*P*H^T  + R
            //   K = P * H^T * S^-1
            //   X = X + K * Y        upates to this time.
            //   P = (I - K * H) * P  update to this time.

            // X = F*X + B*U
            double old = m_x;
            m_x += m_v;

            // P = F*P*F^T + Q
            //   = [1 t] [a b] [1 0]
            //     [0 1] [c d] [t 1]
            //   = [a + ct, b + t] [1 0]
            //   = [     c,     d] [t 1]
            //   = [ac + act + bt + tt,   b + t]
            //     [            c + dt,       d]
            //   = [ac(1+t) + (b+t)t,   b + t]
            //     [          c + dt,       d]
            m_p[0] = ((m_p[0] * m_p[2]) * (2)) + (m_p[1] + 1);
            m_p[1] += 1;
            m_p[2] += 1;

            // Y = M - H*X
            double y0 = m - m_x; // y[1] is always 0 because there is no velocity measurement.

            // S = H*P*H^T  + R
            double s = m_p[0] + m_r;

            // K = P * H^T * S^-1
            double k = m_p[0] / s;

            // X = X + K * Y        upates to this time.
            m_x += y0 * k;
            m_v += k * (m - old);

            // P = (I - K * H) * P  update to this time.
            m_p[0] = (1 - k) * m_p[0]; // 'cause H = [1, 0].

            return m_x;
        }
    }

    /// <summary>
    /// Kalman filter of a rectangle.
    /// </summary>
    public class PositionFilter 
    {
        /// <summary>
        /// X position.
        /// </summary>
        KalmanFilter m_X = new KalmanFilter();

        /// <summary>
        /// Y position.
        /// </summary>
        KalmanFilter m_Y = new KalmanFilter();

        /// <summary>
        /// Just use last scalar for width.
        /// </summary>
        public int Width { get; set; }
        
        /// <summary>
        /// Just use last scalar for height.
        /// </summary>
        public int Height { get; set; }

        /// <summary>
        /// X position.
        /// </summary>
        public int X { get { return (int)m_X.Value; } }

        /// <summary>
        /// Y position.
        /// </summary>
        public int Y { get { return (int)m_Y.Value; } }

        /// <summary>
        /// The location.
        /// </summary>
        public Point Location { get { return new Point(X, Y); } }

        /// <summary>
        /// Current bounds value.
        /// </summary>
        public Rectangle Bounds
        {
            get
            {
                return new Rectangle(
                    (int)m_X.Value,
                    (int)m_Y.Value,
                    Width,
                    Height);
            }
        }

        /// <summary>
        /// Get estimated search region.
        /// </summary>
        public Rectangle SearchBounds
        {
            get
            {
                int dx = (int)(m_X.Variance*4 + 10);
                int dy = (int)(m_Y.Variance*4 + 10);
                return new Rectangle(
                    (int)(m_X.Value - dx),
                    (int)(m_Y.Value - dy),
                    Width + dx + dx,
                    Height + dy + dy);
            }
        }

        /// <summary>
        /// Variance.
        /// </summary>
        public double Variance
        { get { return (m_X.Variance + m_Y.Variance) / 2; } }

        /// <summary>
        /// Setup the filter.
        /// </summary>
        public void Reset()
        {
            m_X.Reset();
            m_Y.Reset();
        }

        public void Reset(int x, int y)
        {
            m_X.Reset(x);
            m_X.Reset(y);
        }

        /// <summary>
        /// Update the value w/ constant time between 
        /// measurements.
        /// </summary>
        /// <param name="m"></param>
        /// <returns></returns>
        public Rectangle Update(Rectangle m)
        {
            m_X.Update(m.X);
            m_Y.Update(m.Y);
            Width = m.Width;
            Height = m.Height;
            return Bounds;
        }
    }
}

By viewing downloads associated with this article you agree to the Terms of Service and the article's licence.

If a file you wish to view isn't highlighted, and is a text file (not binary), please let us know and we'll add colourisation support for it.

License

This article, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

Share

About the Author

HoshiKata
Software Developer (Senior) KMC Systems
United States United States
Phil is a Principal Software developer focusing on weird yet practical algorithms that run the gamut of embedded and desktop (PID loops, Kalman filters, FFTs, client-server SOAP bindings, ASIC design, communication protocols, game engines, robotics).
 
In his personal life he is a part time mad scientist, full time dad, and studies small circle jujitsu, plays guitar and piano.

| Advertise | Privacy | Mobile
Web01 | 2.8.140814.1 | Last Updated 23 Apr 2013
Article Copyright 2012 by HoshiKata
Everything else Copyright © CodeProject, 1999-2014
Terms of Service
Layout: fixed | fluid