Kalman Filter Implementation (Tracking mouse position)

KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F)

Parameters:	dynamParams – Dimensionality of the state. measureParams – Dimensionality of the measurement. controlParams – Dimensionality of the control vector. type – Type of the created matrices that should be CV_32F or CV_64F.

const Mat& KalmanFilter::predict(const Mat& control=Mat()) 

   - Computes a predicted state

const Mat& KalmanFilter::correct(const Mat& measurement)

  - Updates the predicted state from the measurement.

Example:

-----------

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include <Windows.h>

#define drawCross( center, color, d )                                 \
line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

using namespace cv;
using namespace std;
 
int main( )
{ 

KalmanFilter KF(4, 2, 0);
POINT mousePos;
GetCursorPos(&mousePos);

// intialization of KF...
KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
Mat_<float> measurement(2,1); measurement.setTo(Scalar(0));

KF.statePre.at<float>(0) = mousePos.x;
KF.statePre.at<float>(1) = mousePos.y;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(10));
setIdentity(KF.errorCovPost, Scalar::all(.1));
// Image to show mouse tracking
Mat img(600, 800, CV_8UC3);
vector<Point> mousev,kalmanv;
mousev.clear();
kalmanv.clear();

while(1)
{
 // First predict, to update the internal statePre variable
 Mat prediction = KF.predict();
 Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
             
 // Get mouse point
 GetCursorPos(&mousePos);
 measurement(0) = mousePos.x;
 measurement(1) = mousePos.y; 
 
 // The update phase 
 Mat estimated = KF.correct(measurement);

 Point statePt(estimated.at<float>(0),estimated.at<float>(1));
 Point measPt(measurement(0),measurement(1));
    // plot points
    imshow("mouse kalman", img);
    img = Scalar::all(0);

    mousev.push_back(measPt);
    kalmanv.push_back(statePt);
    drawCross( statePt, Scalar(255,255,255), 5 );
    drawCross( measPt, Scalar(0,0,255), 5 );

    for (int i = 0; i < mousev.size()-1; i++) 
     line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
    
    for (int i = 0; i < kalmanv.size()-1; i++) 
     line(img, kalmanv[i], kalmanv[i+1], Scalar(0,155,255), 1);
    
 waitKey(10);  
}
                                          
    return 0;
}

-----------

Result:

2D Convolution / Creating new filter

OpenCV function filter2D is used to create new linear filters.

void filter2D(InputArray src, OutputArray dst, int ddepth, InputArray kernel, Point anchor=Point(-1,-1), double delta=0, int borderType=BORDER_DEFAULT )
Parameters:

• src – input image.
• dst – output image of the same size and the same number of channels as src.
• ddepth – desired depth of the destination image; if it is negative, it will be the same as src.depth(); the following combinations of src.depth() and ddepth are supported:

  • src.depth() = CV_8U, ddepth = -1/CV_16S/CV_32F/CV_64F
  • src.depth() = CV_16U/CV_16S, ddepth = -1/CV_32F/CV_64F
  • src.depth() = CV_32F, ddepth = -1/CV_32F/CV_64F
  • src.depth() = CV_64F, ddepth = -1/CV_64F

  when ddepth=-1, the output image will have the same depth as the source.
  
• kernel – convolution kernel (or rather a correlation kernel), a single-channel floating point matrix; if you want to apply different kernels to different channels, process them individually.
• anchor – anchor of the kernel that indicates the relative position of a filtered point within the kernel; the anchor should lie within the kernel; default value (-1,-1) means that the anchor is at the kernel center.
• delta – optional value added to the filtered pixels before storing them in dst.
• borderType – pixel extrapolation method (see borderInterpolate() for details).

A kernel is a fixed size array of numerical coefficients along with an anchor point in that array.

The code provided below is slight modification of code provided in OpenCV documentation.

Steps:

1. Load image
2. Create a kernel to convolve with the input matrix ( here all elements of kernel is equal; so performs a low pass filter operation)
3. Apply convolution (filter2D)
4. Draw contours

Functions:

filter2D, imshow, imread, waitKey.

Example:

-------------

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <stdlib.h>
#include <stdio.h>

using namespace cv;

void conv2(Mat src, int kernel_size)
{
    Mat dst,kernel;
    kernel = Mat::ones( kernel_size, kernel_size, CV_32F )/ (float)(kernel_size*kernel_size);

    /// Apply filter
    filter2D(src, dst, -1 , kernel, Point( -1, -1 ), 0, BORDER_DEFAULT );
    namedWindow( "filter2D Demo", CV_WINDOW_AUTOSIZE );imshow( "filter2D Demo", dst );
}

int main ( int argc, char** argv )
{
    Mat src;

    /// Load an image
    src = imread( "1.jpg" );
    if( !src.data )  { return -1; }

    conv2(src,3);

    waitKey(0);
    return 0;
}

-------------

Applying Normalized Block Filter

void blur(InputArray src, OutputArray dst, Size ksize, Point anchor=Point(-1,-1), int borderType=BORDER_DEFAULT )

Parameters:

• src – input image; it can have any number of channels, which are processed independently, but the depth should be CV_8U, CV_16U, CV_16S, CV_32F or CV_64F.
• dst – output image of the same size and type as src.
• ksize – blurring kernel size.
• anchor – anchor point; default value Point(-1,-1) means that the anchor is at the kernel center.
• borderType – border mode used to extrapolate pixels outside of the image.

You can find a nice example in OpenCV Documentation.

 Example:

-------------

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

using namespace std;
using namespace cv;

int main( int argc, char** argv )
 {
   
     Mat src = imread( "salt.tif", 1 );
     Mat dst;

     //Apply median filter
     blur( src, dst, Size( 5, 5 ), Point(-1,-1) );
     imshow("source", src);
     imshow("result", dst);   

     waitKey(0);
     return 0;
 }

-------------

Applying Gaussian Filter

void GaussianBlur(InputArray src, OutputArray dst, Size ksize, double sigmaX, double sigmaY=0, int borderType=BORDER_DEFAULT )

Parameters:

• src – input image; the image can have any number of channels, which are processed independently, but the depth should be CV_8U, CV_16U, CV_16S, CV_32F or CV_64F.
• dst – output image of the same size and type as src.
• ksize – Gaussian kernel size. ksize.width and ksize.height can differ but they both must be positive and odd. Or, they can be zero’s and then they are computed from sigma* .
• sigmaX – Gaussian kernel standard deviation in X direction.
• sigmaY – Gaussian kernel standard deviation in Y direction; if sigmaY is zero, it is set to be equal to sigmaX, if both sigmas are zeros, they are computed from ksize.width and ksize.height , respectively (see getGaussianKernel() for details); to fully control the result regardless of possible future modifications of all this semantics, it is recommended to specify all of ksize, sigmaX, and sigmaY.
• borderType – pixel extrapolation method (see borderInterpolate() for details).

You can find a nice example in OpenCV Documentation. 

 Example:

-------------

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

using namespace std;
using namespace cv;

int main( int argc, char** argv )
 {
  
     Mat src = imread( "salt.tif", 1 );
     Mat dst;

     //Apply median filter
     GaussianBlur( src, dst, Size( 5, 5 ), 0, 0 );
     imshow("source", src);
     imshow("result", dst);  

     waitKey(0);
     return 0;
 }

-------------

Applying Median Filter

void medianBlur(InputArray src, OutputArray dst, int ksize)

Parameters:

• src – input 1-, 3-, or 4-channel image; when ksize is 3 or 5, the image depth should be CV_8U, CV_16U, or CV_32F, for larger aperture sizes, it can only be CV_8U.
• dst – destination array of the same size and type as src.
• ksize – aperture linear size; it must be odd and greater than 1, for example: 3, 5, 7 ...

You can find a nice example in OpenCV Documentation. 

 Example:

-------------

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

using namespace std;
using namespace cv;

int main( int argc, char** argv )
 {

     Mat src = imread( "salt.tif", 1 );
     Mat dst;

     //Apply median filter
     medianBlur ( src, dst, 15 );
     imshow("source", src);
     imshow("result", dst);  

     waitKey(0);
     return 0;
 }

-------------

Applying Bilateral Filter

void bilateralFilter(InputArray src, OutputArray dst, int d, double sigmaColor, double sigmaSpace, int borderType=BORDER_DEFAULT )

Parameters:

• src – Source 8-bit or floating-point, 1-channel or 3-channel image.
• dst – Destination image of the same size and type as src .
• d – Diameter of each pixel neighborhood that is used during filtering. If it is non-positive, it is computed from sigmaSpace .
• sigmaColor – Filter sigma in the color space. A larger value of the parameter means that farther colors within the pixel neighborhood (see sigmaSpace ) will be mixed together, resulting in larger areas of semi-equal color.
• sigmaSpace – Filter sigma in the coordinate space. A larger value of the parameter means that farther pixels will influence each other as long as their colors are close enough (see sigmaColor ). When d>0 , it specifies the neighborhood size regardless of sigmaSpace . Otherwise, d is proportional to sigmaSpace .

You can find a nice example in OpenCV Documentation.

A bilateral filter is non-linear, edge-preserving and noise-reducing smoothing filter. The intensity value at each pixel in an image is replaced by a weighted average of intensity values from nearby pixels. This weight can be based on a Gaussian distribution. Crucially, the weights depend not only on Euclidean distance of pixels, but also on the radiometric differences. For example, the range difference such as color intensity, depth distance, etc. This preserves sharp edges by systematically looping through each pixel and adjusting weights to the adjacent pixels accordingly.

Example:

 ------------

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

using namespace std;
using namespace cv;

int main( int argc, char** argv )
 {
  
     Mat src = imread( "Fig1039.tif", 1 );
     Mat dst;

     //Apply bilateral filter
     bilateralFilter ( src, dst, 15, 80, 80 );
     imshow("source", src);
     imshow("result", dst);  

     waitKey(0);
     return 0;
 }

------------

Result:

Source:
http://en.wikipedia.org/wiki/Bilateral_filter