Click here to Skip to main content
15,861,172 members
Articles / General Programming / Algorithms

Image Tracking and Computer Vision Using Fourier Image Correlation

Rate me:
Please Sign up or sign in to vote.
4.98/5 (27 votes)
23 Apr 2013CPOL15 min read 71.7K   17.2K   90  
How to teach a program to recognize something within a video stream.
///////////////////////////////////////////////////////////////////////////////
//
//  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)


Written By
Technical Lead
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.

Comments and Discussions