Computing dense optical flow with cv::calcOpticalFlowFarneback

OpenCV provides a complete implementation of the dense Farnebäck method; see Figure 17-2 for an example output. This functionality is entirely contained in the cv::calcOpticalFlowFarneback() method, which has the following prototype:

void cv::calcOpticalFlowFarneback(
  cv::InputArray       prevImg,    // An input image
  cv::InputArray       nextImg,    // Image immediately subsequent to 'prevImg'
  cv::InputOutputArray flow,       // Flow vectors will be recorded here
  double               pyrScale,   // Scale between pyramid levels (< '1.0')
  int                  levels,     // Number of pyramid levels
  int                  winsize,    // Size of window for pre-smoothing pass
  int                  iterations, // Iterations for each pyramid level
  int                  polyN,      // Area over which polynomial will be fit
  double               polySigma,  // Width of fit polygon, usually '1.2*polyN'
  int                  flags       // Option flags, combine with OR operator
);

Figure 17-2. Motion from the left and center frames are compared, and represented with a field of vectors on the right. The original images were 640x480, winsize=13, numIters=10, polyN=5, polySigma=1.1, and a box kernel was used for pre-smoothing. Motion vectors are shown only on a lower density subgrid, but the actual results are valid for every pixel

The first two arguments are the pair of previous and next images you want to compute a displacement between. Both should be 8-bit, single-channel images and be the same size. The next argument, flow, is the result image; it will be the same size as prevImg and nextImg but be two channels and of the 32-bit floating-point type (CV_32FC2). pyrScale and levels affect how the image pyramid is constructed. pyrScale must be less than 1, and indicates the size of each level of the pyramid relative to its predecessor (e.g., if pyrScale is 0.5, then the pyramid will be a “typical” factor-of-two scaling pyramid). levels determines how many levels the pyramid will have.

The winsize argument controls a presmoothing pass done before the fitting. If you make this an odd number greater than 5, then image noise will not cause as much trouble for the fitting and it will be possible to detect larger (faster) motions. On the other hand, it will also tend to blur the resulting motion field and make it difficult to detect motion on small objects. This smoothing pass can be either a Gaussian blurring or a simple sliding-average window (controlled by the flags argument, described shortly).

The iterations argument controls how many iterations are used at each level in the pyramid. In general, increasing the number of iterations will increase accuracy of the final result. Though in some cases, three or even one iteration is sufficient, the algorithm’s author found six to be a good number for some scenes.

The polyN argument determines the size of the area considered when fitting the polynomial around a point. This is different from winsize, which is used only for presmoothing. polyN could be thought of as analogous to the window size associated with Sobel derivatives. If this number is large, high-frequency fluctuations will not contribute to the polynomial fitting. Closely related to polyN is polySigma, which is the source of the intrinsic scale for the motion field. The derivatives computed as part of the fit use a Gaussian kernel (not the one associated with the smoothing) with variance polySigma and whose total extent is polyN. The value of polySigma should be a bit more than 20% of polyN. (The pairings polyN=5, polySigma=1.1, and polyN=7, polySigma=1.5 have been found to work well and are recommended in source code.)

The final argument to cv::calcOpticalFlowFarneback() is flags, which (as usual) supports several options that may be combined with the logical OR operator. The first option is cv::OPTFLOW_USE_INITIAL_FLOW, which indicates to the algorithm that the array flow should also be treated as an input, and should be used as an initial estimation for the motion in the scene. This is commonly used when one is analyzing sequential frames in video, on the theory that one frame is likely to contain similar motion to the next. The second option is cv::OPTFLOW_FARNEBACK_GAUSSIAN, which tells the algorithm to use a Gaussian kernel in the presmoothing (the one controlled by winsize). In general, one gets superior results with the Gaussian presmoothing kernel, but at a cost of somewhat greater compute time. The higher cost comes not just from the multiplication of the kernel weight values in the smoothing sum, but also because one tends to need a larger value for winsize with the Gaussian kernel.¹

Computing dense optical flow with cv::createOptFlow_DualTVL1

The OpenCV implementation of the Dual TV- L¹ algorithm uses a slightly different interface style than the other optical flow algorithms in this section. There is a separate factory-like function in the library called cv::createOptFlow_DualTVL1(), which constructs an object of type cv::OpticalFlow_DualTVL1 (derived from the base class cv::DenseOpticalFlow) and returns a pointer to it:

cv::Ptr<cv::DenseOpticalFlow> createOptFlow_DualTVL1();

The resulting object has member variables that you will need to override directly if you want to change them from their default values. Here is the relevant portion of the definition of cv::OpticalFlow_DualTVL1:

// Function to get a dense optical flow object
//
cv::Ptr<cv::DenseOpticalFlow> createOptFlow_DualTVL1();

class OpticalFlowDual_TVL1 : public DenseOpticalFlow {

public:

  OpticalFlowDual_TVL1();

  void calc( InputArray I0, InputArray I1, InputOutputArray flow );
  void collectGarbage();

  double tau;            // timestep for solver          (default = 0.25)
  double lambda;         // weight of smoothness term    (default = 0.15)
  double theta;          // tightness parameter          (default = 0.3)
  int    nscales;        // scales in pyramid            (default = 5)
  int    warps;          // warpings per scale           (default = 5)
  double epsilon;        // stopping criterion           (default = 0.01)
  int    iterations;     // max iterations               (default = 300)
  bool   useInitialFlow; // use 'flow' as starting guess (default = false)

};

The variables you can set to configure the algorithm are the arguments to the create function. tau is the timestep used by the numerical solver. It can be set to any value less than 0.125 and convergence is guaranteed, but empirically it can be set as high as 0.25 for faster convergence (in fact, this is the default value). lambda is the most important parameter; it sets the weight of the smoothness term in the energy. The ideal value of lambda will vary depending on the image sequence, with smaller values corresponding to smoother solutions. The default value for lambda is 0.15. The parameter theta is called (by the authors of the original paper) the “tightness parameter.” This is the parameter that couples the two stages of the overall solver. In principle it should be very small, but the algorithm is stable for a wide range of values. The default tightness parameter is 0.30.

The number of scales in the image pyramid is set by nscales. For each scale, the number of warps is the number of times and are computed per scale. This parameter allows a trade-off between speed (fewer warps) and accuracy (more warps). By default there are five scales and five warps per scale.

epsilon is the stopping criterion used by the numerical solver. Additionally there is an iterations criterion, which sets the maximum number of iterations allowed. The default for epsilon is 0.01, while the default for iterations is 300. Making epsilon smaller will give more accurate solutions, but at the cost of more computational time.

The final parameter that you might want to set is useInitialFlow. If this parameter is set to true, then when you call the calc() method for the cv::OpticalFlow_DualTVL1 object, the flow parameter you pass will be used as a starting point for the computation. In many cases it makes sense to use the previous result when you are running on sequential video.

The method cv::OpticalFlow_DualTVL1::calc() is what you use when you want to actually compute optical flow. The calc() method expects two 8-bit, single-channel images as input and will compute the flow output for you from those. As just described, flow is an input-output parameter, and if you set the useInitialFlow member variable of the cv::OpticalFlow_DualTVL1 object to true, then any data currently in flow will be used as a starting point for the next calculation. In any case, flow will be the same size as the input images and will be of type CV_32FC2.

The final method of cv::OpticalFlow_DualTVL1 is collectGarbage(). This method takes no arguments and simply releases any internally allocated memory inside of the cv::OpticalFlow_DualTVL1 object.

Computing Simple Flow with cv::optflow::calcOpticalFlowSF()

We are now ready to look at the OpenCV function that implements the Simple Flow algorithm.⁹ It is called cv::optflow::calcOpticalFlowSF().

void cv::optflow::calcOpticalFlowSF(
  InputArray  from,                     // Initial image (input)
  InputArray  to,                       // Subsequent image (input)
  OutputArray flow,                     // Output flow field, CV_32FC2
  int         layers,                   // Number of layers in pyramid
  int         averaging_block_size,     // Size of neighborhoods (odd)
  int         max_flow                  // Velocities search region sz (odd)
);

void cv::calcOpticalFlowSF(
  InputArray  from,                     // Initial image (input)
  InputArray  to,                       // Subsequent image (input)
  OutputArray flow,                     // Output flow field, CV_32FC2
  int         layers,                   // Number of layers in pyramid
  int         averaging_block_size,     // Size of neighborhoods (odd)
  int         max_flow                  // Velocities search region sz (odd)
  double      sigma_dist,               // sigma_d
  double      sigma_color,              // sigma_c
  int         postprocess_window,       // Velocity filter window sz (odd)
  double      sigma_dist_fix,           // Sigma_d^fix
  double      sigma_color_fix,          // Sigma_c^fix
  double      occ_thr,                  // Threshold used in occlusion detection
  int         upscale_averaging_radius, // Window for joint bilateral upsampling
  double      upscale_sigma_dist,       // Sigma_d^up
  double      upscale_sigma_color,      // Sigma_c^up
  double      speed_up_thr              // Tao
);

The first form of the cv::calcOpticalFlowSF() function does not require you to have a particularly deep understanding of the Simple Flow algorithm. It requires the previous and current images, from and to, and returns the velocity field to you in the array flow. The input images should be 8-bit, three-channel images (CV_8UC3); the result array, flow, will be a 32-bit, two-channel image (CV_32FC2). The only parameters you need to specify for this version of the function are the number of layers, the neighborhood size, and the largest flow velocity that you would like the algorithm to consider when running the initial velocity solver (at each level). These three parameters—layers, averaging_block_size, and max_flow (respectively)—can reasonably be set to 5, 11, and 20.

The second form of cv::calcOpticalFlowSF() allows you to really get in there and tune every parameter of the algorithm. In addition to the arguments used by the short form, the long form allows you to set the bilateral filter parameters used in the energy minimization (sigma_dist and sigma_color, the same as σ_d and σ_c in the previous discussion), the window size for the velocity field cross-bilateral filters (postprocess_window), the parameters for the velocity field cross-bilateral filter (sigma_dist_fix and sigma_color_fix, the same as and in the previous discussion), the threshold used for occlusion detection, the window size for the upsampling joint bilateral filter (upscale_averaging_radius), the parameters for the upsampling joint bilateral filter (upscale_sigma_dist and upscale_sigma_color, the same as and in the previous discussion), and the threshold used to determine when the irregularity map dictates that flows must be recalculated at a finer pyramid level (speed_up_thr, the same as τ in the previous discussion).

Detailed tuning of these parameters is clearly a topic for experts, and you should read the original paper if you want to understand them deeply. For our needs here, however, it is helpful to know the nominal values—used by default by the short argument version of cv::calcOpticalFlowSF()—listed in Table 17-1.

What goes in and what comes out

The Kalman filter is an estimator. This means that it helps us integrate information we have about the state of a system, information we have about the dynamics of the system, and new information we might learn by observation while the system is operating. In practice, there are important limitations to how we can express each of these things. The first important limitation is that the Kalman filter will help us only with systems whose state can be expressed in terms of a vector, representing what we think the current value is for each of its degrees of freedom, and a matrix, expressing our uncertainty about those same degrees of freedom. (It is a matrix because such an uncertainty includes not only the variance of individual degrees of freedom, but also possible covariance between them.)

The state vector can contain any variables we think are relevant to the system we are working with. In the example of a person walking across an image, the components of this state vector might be their current position and their velocity: . In some cases, it is appropriate to have two dimensions each for position and velocity: . In complex systems there may be many more components to this vector. State vectors are normally accompanied by a subscript indicating the time step; the little “star” indicates that we are talking about our best estimate at that particular time.¹⁹ The state vector is accompanied by a covariance matrix, normally written as . This matrix will be an n × n matrix if has n elements.

The Kalman filter will allow us to start with a state , and to estimate subsequent states at later times. Two very important observations are in order about this statement. The first is that every one of these states has the same form; that is, it is expressed as a mean, which represents what we think the most likely state of the system is, and the covariance, which expresses our uncertainty about that mean. This means that only certain kinds of states can be handled by a Kalman filter. Consider, for example, a car that is driving down the road. It is perfectly reasonable to model such a system with a Kalman filter. Consider, however, the case of the car coming to a fork in the road. It could proceed to the right, or to the left, but not straight ahead. If we do not yet know which way the car has turned, the Kalman filter will not work for us. This is because the states represented by the filter are Gaussian distributions—basically, blobs with a peak in the middle. In the fork-in-the-road case, the car is likely to have gone right, and likely to have gone left, but unlikely to have gone straight and crashed. Unfortunately, any Gaussian distribution that gives equal weight to the right and left sides must give yet greater weight to the middle. This is because the Gaussian distribution is unimodal.²⁰

The second important assumption is that the system must  have started out in a state we can describe by such a Gaussian distribution.  This state, written , is called a prior distribution (note the zero subscript). We will see that it is always possible to start a system with what is called an uninformative prior, in which the mean is arbitrary and the covariance is taken to have huge values in it—which will basically express the idea that we “don’t know anything.” But what is important is that the prior must be Gaussian and that we must always provide one.

Assumptions required by the Kalman filter

Before we go into the details of how to work with the Kalman filter, let’s take a moment to look at the “strong but reasonable” set of assumptions we mentioned earlier. There are three important assumptions required in the theoretical construction of the Kalman filter: (1) the system being modeled is linear, (2) the noise that measurements are subject to is “white,” and (3) this noise is also Gaussian in nature. The first assumption means (in effect) that the state of the system at time k can be expressed as a vector, and that the state of the system a moment later, at time k + 1 (also a vector), can be expressed as the state at time k multiplied by some matrix F. The additional assumptions that the noise is both white and Gaussian means that any noise in the system is not correlated in time and that its amplitude can be accurately modeled with only an average and a covariance (i.e., the noise is completely described by its first and second moments). Although these assumptions may seem restrictive, they actually apply to a surprisingly general set of circumstances.

What does it mean to “maximize the a posteriori probability given the previous measurements”? It means that the new model we construct after making a measurement—taking into account both our previous model with its uncertainty and the new measurement with its uncertainty—is the model that has the highest probability of being correct. For our purposes, this means that the Kalman filter is, given the three assumptions, the best way to combine data from different sources or from the same source at different times. We start with what we know, we obtain new information, and then we decide to change what we know based on how certain we are about the old and new information using a weighted combination of the old and the new.

Let’s work all this out with a little math for the case of one-dimensional motion. You can skip the next section if you want, but linear systems and Gaussians are so friendly that Dr. Kalman might be upset if you didn’t at least give it a try.

Information fusion

So what’s the gist of the Kalman filter? Information fusion. Suppose you want to know where some point is on a line—a simple one-dimensional scenario.²¹ As a result of noise, suppose you have two unreliable (in a Gaussian sense) reports about where the object is: locations x₁ and x₂. Because there is Gaussian uncertainty in these measurements, they have means of and together with standard deviations σ₁ and σ₂. The standard deviations are, in fact, expressions of our uncertainty regarding how good our measurements are. For each measurement, the implied probability distribution as a function of location is the Gaussian distribution:²²

Given two such measurements, each with a Gaussian probability distribution, we would expect that the probability density for some value of x given both measurements would be proportional to . It turns out that this product is another Gaussian distribution, and we can compute the mean and standard deviation of this new distribution as follows. Given that:

Given also that a Gaussian distribution is maximal at the average value, we can find that average value simply by computing the derivative of p(x) with respect to x. Where a function is maximal its derivative is 0, so:

Since the probability distribution function p(x) is never 0, it follows that the term in brackets must be 0. Solving that equation for x gives us this very important relation:

Thus, the new mean value is just a weighted combination of the two measured means. What is critically important, however, is that the weighting is determined by (and only by) the relative uncertainties of the two measurements. Observe, for example, that if the uncertainty σ₂ of the second measurement is particularly large, then the new mean will be essentially the same as the mean x₁, the more certain previous measurement.

With the new mean in hand, we can substitute this value into our expression for p₁₂(x) and, after substantial rearranging,²³ identify the uncertainty as:

At this point, you are probably wondering what this tells us. Actually, it tells us a lot. It says that when we make a new measurement with a new mean and uncertainty, we can combine that measurement with the mean and uncertainty we already have to obtain a new state characterized by a still newer mean and uncertainty. (We also now have numerical expressions for these things, which will come in handy momentarily.)

This property that two Gaussian measurements, when combined, are equivalent to a single Gaussian measurement (with a computable mean and uncertainty) is the most important feature for us. It means that when we have M measurements, we can combine the first two, then the third with the combination of the first two, then the fourth with the combination of the first three, and so on. This is what happens with tracking in computer vision: we obtain one measurement followed by another followed by another.

Thinking of our measurements (x_i, σ_i) as time steps, we can compute the current state of our estimation as follows. First, assume we have some initial notion of where we think our object is; this is the prior probability. This initial notion will be characterized by a mean and an uncertainty and . (Recall that we introduced the little stars or asterisks to denote that this is a current estimate, as opposed to a measurement.)

Next, we get our first measurement (x₁, σ₁) at time step 1. All we have to go on is this new measurement and the prior, but we can substitute these into our optimal estimation equation. Our optimal estimate will be the result of combining our previous state estimate—in this case the prior —with our new measurement (x₁, σ₁).

Rearranging this equation gives us the following useful iterable form:

Before we worry about just what this is useful for, we should also compute the analogous equation for .

A rearrangement similar to what we did for yields an iterable equation for estimating variance given a new measurement:

In their current form, these equations allow us to separate clearly the “old” information (what we knew before a new measurement was made—the part with the stars) from the “new” information (what our latest measurement told us—no stars). The new information , seen at time step 1, is called the innovation. We can also see that our optimal iterative update factor is now:

This factor is known as the update gain. Using this definition for K, we obtain a convenient recursion form. This is because there is nothing special about the derivation up to this point or about the prior distribution or the new measurement. In effect, after our first measurement, we have a new state estimate , which is going to function relative to the new measurement in exactly the same way as did relative to . In effect, is a new prior (i.e., what we “previously believed” before the next measurement arrived). Putting this realization into equations:

and

In the Kalman filter literature, if the discussion is about a general series of measurements, then it is traditional to refer to the “current” time step as k, and the previous time step is thus k – 1 (as opposed to k + 1 and k, respectively). Figure 17-11 shows this update process sharpening and converging on the true distribution.

Figure 17-11. Combining our prior belief [left] with our measurement observation [right], the result is our new estimate: [center]

Systems with dynamics

In our simple one-dimensional example, we considered the case of an object being located at some point x, and a series of successive measurements of that point. In that case we did not specifically consider the case in which the object might actually be moving in between measurements. In this new case we will have what is called the prediction phase. During the prediction phase, we use what we know to figure out where we expect the system to be before we attempt to integrate a new measurement.

In practice, the prediction phase is done immediately after a new measurement is made, but before the new measurement is incorporated into our estimation of the state of the system. An example of this might be when we measure the position of a car at time t, then again at time t + dt. If the car has some velocity v, then we do not just incorporate the second measurement directly. We first fast-forward our model based on what we knew at time t so that we have a model not only of the system at time t but also of the system at time t + dt, the instant before the new information is incorporated. In this way, the new information, acquired at time t + dt, is fused not with the old model of the system, but with the old model of the system projected forward to time t + dt. This is the meaning of the cycle depicted in Figure 17-10. In the context of Kalman filters, there are three kinds of motion that we would like to consider.

The first is dynamical motion. This is motion that we expect as a direct result of the state of the system when last we measured it. If we measured the system to be at position x with some velocity v at time t, then at time t + dt we would expect the system to be located at position x + v · dt, possibly still with velocity v.

The second form of motion is called control motion. Control motion is motion that we expect because of some external influence applied to the system of which, for whatever reason, we happen to be aware. As the name implies, the most common example of control motion is when we are estimating the state of a system that we ourselves have some control over, and we know what we did to bring about the motion. This situation arises often in robotic systems where the control is the system telling the robot, for example, to accelerate or to go forward. Clearly, in this case, if the robot was at x and moving with velocity v at time t, then at time t + dt we expect it to have moved not only to x + v · dt (as it would have done without the control), but also a little farther, since we know we told it to accelerate.

The final important class of motion is random motion. Even in our simple one-dimensional example, if whatever we were looking at had a possibility of moving on its own for whatever reason, we would want to include random motion in our prediction step. The effect of such random motion will be to simply increase the uncertainty of our state estimate with the passage of time. Random motion includes any motions that are not known or under our control. As with everything else in the Kalman filter framework, however, there is an assumption that this random motion is either Gaussian (i.e., a kind of random walk) or that it can at least be effectively modeled as Gaussian.

These dynamical elements are input into our simulation model by the update step before a new measurement can be fused. This update step first applies any knowledge we have about the motion of the object according to its prior state, then any additional information resulting from actions that we ourselves have taken or that we know have been taken on the system from another outside agent, and then finally our estimation of random events that might have changed the state of the system along the way. Once those factors have been applied, we can incorporate our next new measurement.

In practice, the dynamical motion is particularly important when the state of the system is more complex than our simulation model. Often when an object is moving, there are multiple components to the state vector, such as the position as well as the velocity. In this case, of course, the state is evolved according to the velocity that we believe it to have. Handling systems with multiple components to the state is the topic of the next section. In order to handle these new aspects of the situation, however, we will need to develop our mathematical machinery a little further as well.

Kalman equations

We are now ready to put everything together and understand how to handle a more general model. Such a model will include a multidimensional state vector, dynamical motion, control motion, random motion, measurement, measurement uncertainty, and the techniques necessary to fuse together our estimates of where the system has gone with what we subsequently measure.

The first thing we need to do is to generalize our discussion to states that contain many state variables. The simplest example of this might be in the context of video tracking, where objects can move in two or three dimensions. In general, the state might contain additional elements, such as the velocity of an object being tracked. We will generalize the description of the state at time step k to be the following function of the state at time step k – 1:

Here is now the n-dimensional vector of state components and F is an n × n matrix, sometimes called the transfer matrix or transition matrix, which multiplies . The vector is new. It’s there to allow external controls on the system, and it consists of a c-dimensional vector referred to as the control inputs; B is an n × c matrix that relates these control inputs to the state change.²⁴ The variable is a random variable (usually called the process noise) associated with random events or forces that directly affect the actual state of the system. We assume that the components of have a zero mean Gaussian distribution, for some n × n covariance matrix Q_k (which could, in principle, vary with time, but typically it does not).

In general, we make measurements that may or may not be direct measurements of the state variable . (For example, if you want to know how fast a car is moving then you could either measure its speed with a radar gun or measure the sound coming from its tailpipe; in the former case, will be with some added measurement noise, but in the latter case, the relationship is not direct in this way.) We can summarize this situation by saying that we measure the m-dimensional vector of measurements given by:

Here H is an m × n matrix and is the measurement error, which is also assumed to have Gaussian distributions N(0, R_k) for some m × m covariance matrix R_k.²⁵

Before we get totally lost, let’s consider a particular realistic situation of taking measurements on a car driving in a parking lot. We might imagine that the state of the car could be summarized by two position variables, x and y, and two velocities, V_x and V_y. These four variables would be the elements of the state vector . This suggests that the correct form for F is:

However, when using a camera to make measurements of the car’s state, we probably measure only the position variables:

This implies that the structure of H is something like:

In this case, we might not really believe that the velocity of the car is constant and so would assign a value of Q_k to reflect this. The actions of a driver not under our control are, in a sense, random from our point of view. We would choose R_k based on our estimate of how accurately we have measured the car’s position using (for example) our image-analysis techniques on a video stream.

All that remains now is to plug these expressions into the generalized forms of the update equations. The basic idea is the same, however. First we compute the a priori estimate of the state. It is relatively common (though not universal) in the literature to use the superscript minus sign to mean “at the time immediately prior to the new measurement”; we’ll adopt that convention here as well. Thus the a priori estimate is given by:

Using to denote the error covariance, the a priori estimate for this covariance at time k is obtained from the value at time k – 1 by:

This equation forms the basis of the predictive part of the estimator, and it tells us “what we expect” based on what we’ve already seen. From here we’ll state (without derivation) what is often called the Kalman gain or the blending factor, which tells us how to weight new information against what we think we already know:

Though this equation looks intimidating, it’s really not so bad. We can understand it more easily by considering various simple cases. For our one-dimensional example in which we measured one position variable directly, H_k is just a 1 × 1 matrix containing only a 1! Thus, if our measurement error is , then R_k is also a 1 × 1 matrix containing that value. Similarly, Σ_k is just the variance . So that big equation boils down to just this:

Note that this is exactly what we thought it would be. The gain, which we first saw in the previous section, allows us to optimally compute the updated values for and σ_k when a new measurement is available:

and:

Once again, these equations look intimidating at first, but in the context of our simple one-dimensional discussion, it’s really not as bad as it looks. The optimal weights and gains are obtained by the same methodology as for the one-dimensional case, except this time we minimize the uncertainty of our position state x by setting to 0 the partial derivatives with respect to x before solving. We can show the relationship with the simpler one-dimensional case by first setting F = I (where I is the identity matrix), B = 0, and Q = 0. The similarity to our one-dimensional filter derivation is then revealed as we make the following substitutions in our more general equations:

Tracking in OpenCV with cv::KalmanFilter

With all of this at our disposal, you probably feel either that you don’t need OpenCV to do anything for you, or that you desperately need OpenCV to do all of this for you. Fortunately, the library and its authors are amenable to either interpretation. The Kalman filter is (not surprisingly) represented in OpenCV by an object called cv::KalmanFilter. The  declaration for the cv::KalmanFilter object has the following form:

class cv::KalmanFilter {

public:
  cv::KalmanFilter();

  cv::KalmanFilter(
    int dynamParams,            // Dimensionality of state vector
    int measureParams,          // Dimensionality of measurement vector
    int controlParams = 0,      // Dimensionality of control vector
    int type          = CV_32F  // Type for matrices (CV_32F or F64)
  );

  //! re-initializes Kalman filter. The previous content is destroyed.
  void init(
    int dynamParams,            // Dimensionality of state vector
    int measureParams,          // Dimensionality of measurement vector
    int controlParams = 0,      // Dimensionality of control vector
    int type          = CV_32F  // Type for matrices (CV_32F or F64)
  );

  //! computes predicted state
  const cv::Mat& predict(
    const cv::Mat& control = cv::Mat() // External applied control vector (u_k)
  );

  //! updates the predicted state from the measurement
  const cv::Mat & correct(
    const cv::Mat& measurement  // Measurement vector (z_k)
  );

  cv::Mat transitionMatrix;     // state transition matrix (F)
  cv::Mat controlMatrix;        // control matrix (B) (w/o control)
  cv::Mat measurementMatrix;    // measurement matrix (H)
  cv::Mat processNoiseCov;      // process noise covariance matrix (Q)
  cv::Mat measurementNoiseCov;  // measurement noise covariance matrix (R)

  cv::Mat statePre;         // predicted state (x'_k):
                            //   x_k       = F * x_(k-1) + B * u_k

  cv::Mat statePost;        // corrected state (x_k):
                            //   x_k       = x'_k + K_k * ( z_k – H * x'_k )

  cv::Mat errorCovPre;      // a-priori error estimate covariance matrix (P'_k):
                            //   Sigma'_k = F * Sigma_(k-1) * F^t + Q

  cv::Mat gain;             // Kalman gain matrix (K_k):
                            //   K_k = Sigma'_k * H^t * inv(H*Sigma'_k*H^t+R)

  cv::Mat errorCovPost;     // a posteriori error covariance matrix (Sigma_k):
                            //   Sigma_k  = ( I - K_k * H ) * Sigma'_k

  ...
};

You can either create your filter object with the default constructor and then configure it with the cv::KalmanFilter::init() method, or just call the constructor that shares the same argument list with cv::KalmanFilter::init(). In either case, there are four arguments you will need.

The first is dynamParams; this is the number of dimensions of the state vector . It does not matter what dynamical parameters you will have—only the number of them matters. Remember that their interpretation will be set by the various other components of the filter (notably the state transition matrix F). The next parameter is measureParams; this is the number of dimensions that are present in a measurement, the dimension of z_k. As with x_k, it is the other components of the filter that ultimately give its meaning, so all we care about right now is the total dimension of the vector (in this case the meaning of is primarily coming from the way we define the measurement matrix H, and its relationship to ). If there are to be external controls to the system, then you must also specify the dimension of the control vector . By default, all of the internal components of the filter will be created as 32-bit floating-point type numbers. If you would like the filter to run in higher precision, you can set the final argument, type, to cv::F64.

The next two functions implement the Kalman filter process (Figure 17-10). Once the data is in the structure (we will talk about this in a moment), we can compute the prediction for the next time step by calling cv::KalmanFilter::predict() and then integrate our new measurements by calling cv::KalmanFilter::correct().²⁶ The prediction method (optionally) accepts a control vector , while the correction method requires the measurement vector . After running each of these routines, we can read the state of the system being tracked. The result of cv::KalmanFilter::correct() is placed in statePost, while the result of cv::KalmanFilter::predict() is placed in statePre. You can read these values out of the member variables of the filter, or you can just use the return values of the two methods.

You will notice that, unlike many objects in OpenCV, all of the member variables are public. There are not “get/set” routines for them, so you just access them directly when you need to do so. For example, the state-transition matrix F, which is called cv::KalmanFilter::transitionMatrix in this context, is something you will just set up yourself for your system. This will all make a little more sense with an example.

Kalman filter example code

Clearly it is time for a good example. Let’s take a relatively simple one and implement it explicitly. Imagine that we have a point moving around in a circle, like a car on a racetrack. The car moves with mostly a constant velocity around the track, but there is some variation (i.e., process noise). We measure the location of the car using a method such as tracking it via our vision algorithms. This generates some (unrelated and probably different) noise as well (i.e., measurement noise).

So our model is quite simple: the car has a position and an angular velocity at any moment in time. Together these factors form a two-dimensional state vector . However, our measurements are only of the car’s position and so form a one-dimensional “vector” .

We’ll write a program (Example 17-1) whose output will show the car circling around (in red) as well as the measurements we make (in yellow) and the location predicted by the Kalman filter (in white).

We begin with the usual calls to include the library header files. We also define a macro that will prove useful when we want to transform the car’s location from angular to Cartesian coordinates so we can draw on the screen.

#include "opencv2/opencv.hpp"
#include <iostream>

using namespace std;

#define phi2xy( mat )                                                        
  cv::Point( cv::cvRound( img.cols/2 + img.cols/3*cos(mat.at<float>(0)) ),   
             cv::cvRound( img.rows/2 - img.cols/3*sin(mat.at<float>(0)) ))

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

...

Next, we will create a random-number generator, an image to draw to, and the Kalman filter structure. Notice that we need to tell the Kalman filter how many dimensions the state variables are (2) and how many dimensions the measurement variables are (1).

...

    // Initialize, create Kalman filter object, window, random number
    // generator etc.
    //
    cv::Mat img( 500, 500, CV_8UC3 );
    cv::KalmanFilter kalman(2, 1, 0);

...

Once we have these building blocks in place, we create a matrix (really a vector, but in OpenCV we call everything a matrix) for the state x_k (), the process noise w_k (), the measurements z_k (), and the all-important transition matrix transitionMatrix (F). The state needs to be initialized to something, so we fill it with some reasonable random numbers that are narrowly distributed around zero.

The transition matrix is crucial because it relates the state of the system at time k to the state at time k + 1. In this case, the transition matrix will be 2 × 2 (since the state vector is two-dimensional). It is, in fact, the transition matrix that gives meaning to the components of the state vector. We view x_k as representing the angular position of the car (φ) and the car’s angular velocity (ω). In this case, the transition matrix has the components [[1, dt], [0, 1]]. Hence, after we multiply by F, the state becomes —that is, the angular velocity is unchanged but the angular position increases by an amount equal to the angular velocity multiplied by the time step. In our example we choose dt = 1.0 for convenience, but in practice we’d need to use something like the time between sequential video frames.

...

  // state is (phi, delta_phi) - angle and angular velocity
  // Initialize with random guess.
  //
  cv::Mat x_k( 2, 1, CV_32F );
  randn( x_k, 0., 0.1 );

  // process noise
  //
  cv::Mat w_k( 2, 1, CV_32F );

  // measurements, only one parameter for angle
  //
  cv::Mat z_k = cv::Mat::zeros( 1, 1, CV_32F );

  // Transition matrix 'F' describes relationship between
  // model parameters at step k and at step k+1 (this is
  // the "dynamics" in our model.
  //
  float F[] = { 1, 1, 0, 1 };
  kalman.transitionMatrix = Mat( 2, 2, CV_32F, F ).clone();

...

The Kalman filter has other internal parameters that must be initialized. In particular, the 1 × 2 measurement matrix H is initialized to [1, 0] by a somewhat unintuitive use of the identity function. The covariances of the process noise and the measurement noise are set to reasonable but interesting values (you can play with these yourself), and we initialize the posterior error covariance to the identity as well (this is required to guarantee the meaningfulness of the first iteration; it will subsequently be overwritten).

Similarly, we initialize the posterior state (of the hypothetical step previous to the first one!) to a random value since we have no information at this time.

...

  // Initialize other Kalman filter parameters.
  //
  cv::setIdentity( kalman.measurementMatrix,   cv::Scalar(1)    );
  cv::setIdentity( kalman.processNoiseCov,     cv::Scalar(1e-5) );
  cv::setIdentity( kalman.measurementNoiseCov, cv::Scalar(1e-1) );
  cv::setIdentity( kalman.errorCovPost,        cv::Scalar(1)    );

  // choose random initial state
  //
  randn(kalman.statePost, 0., 0.1);

  for(;;) {
...

Finally we are ready to start up on the actual dynamics. First we ask the Kalman filter to predict what it thinks this step will yield (i.e., before giving it any new information); we call this y_k (). Then we proceed to generate the new value of z_k (, the measurement) for this iteration. By definition, this value is the “real” value x_k () multiplied by the measurement matrix H with the random measurement noise added. We must remark here that, in anything but a toy application such as this, you would not generate z_k from x_k; instead, a generating function would arise from the state of the world or your sensors. In this simulated case, we generate the measurements from an underlying “real” data model by adding random noise ourselves; this way, we can see the effect of the Kalman filter.

...

    // predict point position
    //
    cv::Mat y_k = kalman.predict();

    // generate measurement (z_k)
    //
    cv::randn(z_k, 0., sqrt((double)kalman.measurementNoiseCov.at<float>(0,0)));
    z_k = kalman.measurementMatrix*x_k + z_k;

...

Draw the three points corresponding to the observation we synthesized previously, the location predicted by the Kalman filter, and the underlying state (which we happen to know in this simulated case).

...

    // plot points (e.g., convert to planar co-ordinates and draw)
    //
    img = Scalar::all(0);
    cv::circle( img, phi2xy(z_k), 4, cv::Scalar(128,255,255) );    // observed
    cv::circle( img, phi2xy(y_k), 4, cv::Scalar(255,255,255), 2 ); // predicted
    cv::circle( img, phi2xy(x_k), 4, cv::Scalar(0,0,255) );        // actual

    cv::imshow( "Kalman", img );

...

At this point we are ready to begin working toward the next iteration. The first thing to do is again call the Kalman filter and inform it of our newest measurement. Next we will generate the process noise. We then use the transition matrix F to time-step x_k forward one iteration, and then add the process noise we generated; now we are ready for another trip around.

...

    // adjust Kalman filter state
    //
    kalman.correct( z_k );

    // Apply the transition matrix 'F' (e.g., step time forward)
    // and also apply the "process" noise w_k
    //
    cv::randn(w_k, 0., sqrt((double)kalman.processNoiseCov.at<float>(0,0)));
    x_k = kalman.transitionMatrix*x_k + w_k;

    // exit if user hits 'Esc'
    if( (cv::waitKey( 100 ) & 255) == 27 ) break;
  }

  return 0;
}

As you can see, the Kalman filter part was not that complicated; half of the required code was just generating some information to push into it. In any case, we should summarize everything we’ve done, just to be sure it all makes sense.

We started out by creating matrices to represent the state of the system and the measurements we would make. We defined both the transition and measurement matrices, and then initialized the noise covariances and other parameters of the filter.

After initializing the state vector to a random value, we called the Kalman filter and asked it to make its first prediction. Once we read out that prediction (which was not very meaningful this first time through), we drew to the screen what was predicted. We also synthesized a new observation and drew that on the screen for comparison with the filter’s prediction. Next we passed the filter new information in the form of that new measurement, which it integrated into its internal model. Finally, we synthesized a new “real” state for the model so that we could iterate through the loop again.

When we run the code, the little red ball orbits around and around. The little yellow ball appears and disappears about the red ball, representing the noise that the Kalman filter is trying to “see through.” The white ball rapidly converges down to moving in a small space around the red ball, showing that the Kalman filter has given a reasonable estimate of the motion of the particle (the car) within the framework of our model.

One topic that we did not address in our example is the use of control inputs. For example, if this were a radio-controlled car and we had some knowledge of what the person with the controller was doing, we could include that information into our model. In that case it might be that the velocity is being set by the controller. We’d then need to supply the matrix B (kalman.controlMatrix) and also to provide a second argument for cv::KalmanFilter::predict() to accommodate the control vector .