1 基本概念

和处理静态图片不同，处理视频数据的时候，通常想追踪其中的一个多种多个目标在视频帧之间的移动。在前面的章节中，已经介绍了很多方法(如颜色直方图和矩)来分离特定形状，如一个人或者汽车。同样也介绍了如何提取这些对象的关键点集合，以及如何匹配关键点。

实践中计算机视觉领域内的追踪相关任务主要有两个，分别是追踪已知的对象，和追踪未知对象。对于第二个任务，在很多时候，需要根据这些未知对象的运动对其进行鉴别。尽管通过前面章节介绍的知识，如颜色直方图和矩可以识别视频帧内的对象，但是很多时候我们仍需要分析这些运动本身，从而推断出这些对象的存在以及我们感兴趣的特征。

前一章学习了特征点和描述符，我们已经对计算稀疏光流有了一定的基础。而本章会进一步介绍一些能用于计算稠密光流的技术，稠密光流和稀疏光流的区别在于前者计算了指定区域内所有像素的移动情况。

除了追踪外，另外一个问题是建模(Modeling)。建模不仅能够帮助处理追踪问题，甚至在最理想的情况下还能近似计算不同视频帧内被追踪目标的位置。很多强大的通过这种近似方式计算目标轨迹的数学技术以及被提出。这些技术在处理二维或者三维模型和它们位置时很受欢迎。在本章的后半部分将会探索OpenCV提供的用于实现这些技术的相关接口，并讨论它们背后的一些原理。

本章开始会讨论稠密光流的细节，并介绍几个OpenCV提供的不同算法。每个算法对稠密光流的定义都有一些微小的差异，并且得到的结果也会有些微弱的区别，显然它们都有自己最适合的使用场景。接下来会讲到追踪，第一组追踪算法用于追踪区域，它们本质上是计算这些区域的稠密光流。这些算法包括均值移动和camshift算法，以及运动模版。在本章的最后，我们会讨论Kalman滤镜，它可以对被追踪的目标的运动进行建模，得到的模型可以帮助我们，将很多近似的观测现象和已有的关于目标行为的高级知识进行融合，从而得到对目标在真实世界行为的最佳估计。

最后在附录中的opencv_contrib部分中的光流和追踪函数还引用了更多的光流和追踪算法。

2 稠密光流

到目前为止，通过学习到的知识我们已经能够在某张图片中分离在另外一张图片中识别到的关键点。通过对这些关键点应用前文学到的稀疏光流，必然得到目标对象在场景中的全局运动的稀疏表示。在这种情况下，如果视频中有一个车在移动，我们可以知道车的某一个特定部分运动到了什么位置，也可能得到合理的关于整个车移动的结论，但是并不一定能知道场景的整体运动情况。特别是在包含很多车辆的复杂场景中，判断哪些关键点和哪一辆车关联并不总是容易的。稀疏光流的另一个替代方案是稠密光流，稠密光流会计算图像中每个像素的运动向量，最终计算的结果是一个速度向量场，我们可以用很多新的方法来分析这些数据。

实际上计算稠密光流并不容易，想象场景中有一张白纸移动，大多数位于白纸上的像素点在连续的一系列视频帧内都是白色。只有边缘的像素点，甚至是和移动方向垂直的边缘上的像素点颜色才会在不同帧之间改变。因此稠密光流必须包含在易于追踪的像素点之间的插值方法，从而能够计算那些不容易追踪的像素。稠密光流算法的高计算成本显然已经反映出其计算很困难。

早期的Horn-Schunk算法尝试计算速度向量场并处理插值问题，这个算法是对块匹配(Block Matching)算法的改进，后者尽管简单但是存在问题，它在处理每个像素时都简单的匹配两个视频帧内以该像素为中心的窗口。这些算法在早期版本的OpenCV中都有实现，但是它们不仅速度很慢，结果还不可靠。然后随后该领域的发展使得一些尽管速度没有稀疏光流算法快，但是已经足够快和准确，满足实用的条件。目前OpenCV提供了这些新算法的其中两个算法的实现，分别是多项式展开(Polynomial Expansion)和Dual TV-L算法。

这里只介绍项式展开算法和一些更现代的算法，通过对Horn和Schunk提出算法的改进，它们比原始版本的算法性能更好。

2.1 Farneback多项式展开算法

Farneback多项式展开(Polynomial Expansion)算法最早由G Farneback提出，该算法的基本思想是将图像近似看作是连续曲面。当然真实的图像是离散的数据，因此算法包含一些复杂特性允许对图像运用这一基本思想。

算法的第一步是在图像的每个点拟合出一个二次多项式函数来表示该点附近很小区域内的像素变化，当然函数拟合使用的数据是以对应像素点为中心的一个窗口，窗口内每个像素都会分配不同的权重，使得越靠近中心的像素对计算的结果贡献越高。而窗口的尺寸也决定了算法敏感的特征尺寸。

在理想情况下图像可以看作是光滑的连续函数，如在下图中，折线表示的是图像内的像素强度，而虚线是某个特定的像素点，而抛物线则是使用该点附近的数据拟合出的二次函数图像。从左图到右图，图像向右移动了一小段距离，因此我们在同一个点拟合出的两个二次函数系数会不相同，通过计算这些系数，我们就能计算出每个像素的移动向量。当然这种方式只对微小的位移有效，然而更大的位移我们也有相应的技巧可以处理。此外需要注意的是这里为了理解简单，只演示了一个维度上的移动。

该算法的一个技巧就是如果你知道一个距离估计值，则你可以比较原图中的特定像素和新图像中该像素移动后的新位置对应的像素，而不是比较两幅图片中的同一个像素。这种情况下分析技术可以对原始的位置估计进行校正。实际上该思路可以通过算法的迭代实现，并且在每一次迭代中都不断的提高运动位移估计的精度。

这种技巧也可以用于处理较大的位移，考虑使用原图构建图像金字塔，首先在最高层具有最低分辨率的图层上计算位移。因为此时分辨率较低，而原始图像被缩小，因此较大的那些位移也被缩小，即满足位移较小的前提条件。然后使用计算得到的结果作为下一图层运算的位移初始估计，逐层向下迭代运算直至到原始分辨率的最底层图像。这样就能够得到较为准确的大尺度位移。

2.1.1 相关接口

OpenCV提供的相关函数原型如下。

// prevImg：源图片，8位单通道
// nextImg：目标图片，8位单通道，和prevImg大小相同，最好是视频流中prevImg的直接相邻下一帧
// flow：计算出的光流向量，元素格式为CV_32FC2，大小和prevImg相同
// pyrScale：金字塔层之间的缩放系数，必须小于1
// levels：金字塔的层数
// winsize：图像预平滑处理的窗口(卷积核)大小
// iterations：在每层金字塔上算法的迭代次数
// polyN：拟合二次函数时采样的窗口大小
// polySigma：拟合二次函数时样本权重分配使用的高斯分布的标准差，通常设置为稍大于0.2*polyN的值
// flags：算法的一些策略，下文介绍
void cv::calcOpticalFlowFarneback(cv::InputArray prevImg, cv::InputArray nextImg,
                                  cv::InputOutputArray flow, double pyrScale,
                                  int levels, int winsize, int iterations,
                                  int polyN, double polySigma, int flags);

参数winsize控制了在正式计算光流之前，图像预平滑处理的窗口(卷积核)大小。如果该值是大于5的奇数，则函数拟合越不容易受到图像噪声的影响，并且可能检测到更大或者更快的运动。但是这样做也会模糊计算得到的运动向量场，并且使小物体的运动不容易被检测。预平滑处理的策略可以是高斯模糊，也可以是简单的均值滑动窗口，通过参数flags控制。

参数iterations控制在每层金字塔上算法的迭代次数，该值越大计算结果越精确，但是算法计算成本越高。尽管在有些场景中该值取3甚至是1就足够了，但是算法的作者发现在有些场景中该值取6才能得到较好的结果。

参数polyN控制拟合二次函数时采样的窗口大小，它与Sobel微分算子有关。该值越大，则高频的抖动越不会影响拟合得到的二次函数。在计算微分时窗口内的像素都会根据高斯分布分配一个权重值，从而使得距离中心越近的像素对结果贡献越高，而参数polySigma则是这个高斯分布的标准差，通常设置为比polyN的20%稍大的值。参数对polyN = 5，polySigma = 1.1和polyN = 7，polySigma = 1.5通常能够取得较好的结果，建议使用。

参数flags定义了一些算法策略，它们可以用逻辑与符号连接从而实现多选。第一个可取的值为cv::OPTFLOW_USE_INITIAL_FLOW，表明参数flow中提供的矩阵将被看成是初始的运动估计。在处理连续视频帧时通常也会使用到该选项，因为通常连续的视频帧内的运动相似。另一个可取值为cv::OPTFLOW_FARNEBACK_GUASSIAN，表明图像的预平滑策略使用高斯模糊。总的来说使用高斯模糊的效果更佳，但是也会增加运算成本。运算成本不仅来自于高斯模糊的计算方式，来来自于需要更大的窗口，即更大的winsize。算法内部使用的高斯分布标准差等于0.3✖️(winsize/2)，这意味着窗口内质量集中区域仅占整个窗口的60%到70%，因此窗口需要再增加30%左右才能显示出盒子内核的优势。

下图是一个使用该算法的例子，左侧两幅图是一个视频序列的连续两帧图像，右图是计算得到的向量场，尽管这里为了方便观察只展示了部分像素的运动向量，实际上计算结果是包含所有像素的运动向量。下图使用的原图分辨率为640✖️480，winsize = 13，iterations = 10，polyN = 5，polySigma = 1.1，图像预处理使用的方法是盒式滤镜。

2.2 Dual TV-L1算法

Dual TV-L1算法是Horn和Schunck(HS)算法的改进版本。OpenCV内部的实现是基于最早提出该算法的论文【A duality based approach for realtime TV-L 1 optical flow】，以及论文【TV-L1 optical flow estimation.】中对该算法的改进。原始的HS算法基于一个光流公式，该公式使得光流问题可以用数值方法解决，尽管这种方式不是很快。而Dual TV-L1算法使用的光流公式有细微的差异，求解该公式会比求解HS算法的公式更容易，效率更高。

HS算法使用相邻两帧画面的像素强度，和运动向量场定义了如下能量成本函数。

其中It(X)是图像在t时刻X(x, y)位置的亮度，ux和uy是t时刻位置X处对应像素的光流U在x和y轴上的分量，它们是关于x和y的函数，▼ux和▼uy则是它们的全微分。a2是影响第一约束(Fidelity保真度，即像素强度差)相对于第二约束(Smoothness光滑度，即光流分量的平方和)的权重系数。保真度约束是针对每个像素的强度，由于光流具有二维特性，因此仅靠该限制无法解决问题，因此还需要一种连续性约束，即这里使用的光滑度约束。

HS算法的计算过程是使得该能量函数具有最小值，作者建议的方式是将该函数转换为相关的欧拉-拉格朗日等式，并通过迭代的方式解决。这种方式的主要问题是由于欧拉-拉格朗日等式具有很强的局部性，因此每一次迭代只能解决和像素最近相邻项相关的问题，因此在实际运算中，这些高计算成本的迭代运算可能会很多，但是这个问题可以通过分层方式解决。

接下来看下TV-L1算法，它不仅使用了不同的能量函数定义，还在求解该该函数的方式上有所区别。其能量函数定义如下，算法的保真度约束使用总体变化Total Variation替代了HS算法内的平方差，并且使用了L1范数定义了光滑度约束，而不是HS算法中的L2范数，这也是该算法得名的原因。

使用L1范数最主要的优势是局部梯度对算法的影响没有那么大，这样算法在不连续处的表现更好。公式中使用λ而不是a2仅仅是为了遵循这两个方法作者的习惯，它们是完全等价的。算法使用总体变化而不是差的平方是为了便于求解该方程。Dual TV-L1算法将最小化能量函数的计算转化为两个独立的问题，第一个是已知问题，即该算法名字中的dual。第二个问题对于每个像素而言都是理想的完全局部问题，因此可以逐点解决。其解决过程如下。【此处往下为原书中的推导过程，省略了很多重要的逻辑，因此可能造成阅读障碍，具体推导过程参考前文所说的算法引用论文。或者参考】

首先假定光流场u0非常接近真正的光流场u，在算法的实际实现中使用图像金字塔方式从低分辨率的高层图像计算近似光流场u0，再不断以此计算结果为下一层的近似光流场，并直至计算到最底层。此时能量公式中的亮度差分使用一阶泰勒展开式可以近似表示为如下等式。

则此时能量函数可以近似表示为如下等式。

算法的核心是引入了一个新的向量v和辅助变量θ，这样将能量函数替换为如下形式。

这样保真度约束和光滑度约束就可以解耦，从而逐个解决。尽管现在多了一个变量v，但是当θ趋于0时，v和u几乎相等。其实这种转换最大的好处就是我们可以固定v和u中的一个，求出另一个变量，然后再返回来求解被固定的变量，并且以这种方式不断迭代。

当v的值固定时，能量函数如下表示，则可以计算出使得能量取得最低的u值。

当u固定时，能量函数如下表示，则可以计算出使得能量取得最低的v值。

计算u和v的值包含两部分，第一部分有已知的解决方案，称为总体变化降噪模型(Total Variation Denoising Model)，在论文《An algorithm for total variation minimization and applications.》中有详细算法介绍。第二部分是一个完全局部的问题，可以逐像素解决。其中第一个流程引入了两个新的参数，分别是时间步长(Time Step)和停止条件(Stopping Criterion)。这些参数控制了当v固定时，u在什么时候以及如何收敛。具体求解过程请参照相关论文。

2.2.1 相关接口

Dual TV-L算法相关类实例构建函数如下，它和其他光流算法有一些区别，它是一个单独的工厂方法。

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

算法相关的类定义如下，使用上面的接口创建实例后会包含默认属性，可根据实际情况对这些属性进行修改。

class OpticalFlowDual_TVL1 : public DenseOpticalFlow {
public:
// 构造函数
OpticalFlowDual_TVL1();
// 光流计算函数
// I0：输入图像1，8位单通道
// I1：输入图像2，8位单通道
// flow：作为输入参数表示预测的光流
//       作为输出参数表示计算的结果
//       元素数据格式为CV_32FC2
void calc(InputArray I0, InputArray I1, InputOutputArray flow);
// 释放该实例内部创建的内存
void collectGarbage();

// 时间步长，默认值为0.25
double tau;
// 保真度约束的权重λ，默认值为0.15
double lambda;
// 紧密度参数θ，默认值为0.3
double theta;
// 金字塔层数，默认值为5
int nscales;
// 图像金字塔上每层计算u0的迭代次数，默认值为5
int warps;
// 终止条件的计算精度(计算u和v时使用)，默认值为0.01
double epsilon;
// 终止条件的最大迭代次数(计算u和v时使用)，默认值为300
int iterations;
// 是否包含预测的光流场，默认为false
// 当该属性为true时，函数calc调用中参数flow作为输入参数包含预测的光流
bool useInitialFlow;
};

参数tau可以设置为小于0.125的值并且算法保证会收敛，但是从经验上看，为了使算法收敛的速度更快，该值可以高达0.25。参数lambda的理想值取决于图像序列，变化越平滑的场景中该属性的理想值越小。理论上参数theta的值应该很小，但是对于较宽范围的取值算法都能保持稳定。参数warps的取值需要权衡算法的效率和计算结果的准确度。参数epsilon的值越低，计算结果精度越高，但是算法的计算成本也越高。

处理连续视频序列时，由于连续的帧光流相似，因此将上一帧的结果作为下一帧的预测输入，即将属性useInitialFlow设置为yes很有意义。

2.3 简单光流算法

简单光流算法(Simple Flow Algorithm)是另一个最近的光流算法，它由Micheal Tao等人在论文《SimpleFlow: A Non-iterative, Sublinear Optical Flow Algorithm.》中提出。该算法的特点是其所需的时间和图像中像素数量之间的关系时亚线性(Sublinear)的，尽管从技术层面上讲，该算法并不是严格亚线性的，因为算法的某个部分会应用到所有的像素中，但是算法所需要的昂贵的运算并不需要应用到每个像素，原论文的作者证明了4K(4096✖️2160)的图片使用该算法时时间成本是亚线性的。算法时通过图像金字塔方案来实现这个特性的，当算法从低分辨率层向下移动到高分辨率层时，如果在新的层中有发现新的信息，则会在该层上计算新的光流，否则会将光流直接传递到下一层，并进行插值。

简单光流算法尝试为每个点建立一个局部流向量，这些点能够很好的代表其邻域范围内像素的运动。算法将单个像素的能量函数定义如下，其中It(x, y)是t时刻点(x, y)处的像素强度，而u和v是流向量在x和y轴上的分量。

则整体的能量函数表示如下，当其具有最小值时得到的解就是寻找的光流向量。

其中：

这两个公式构建了一个类似双边滤波器，当σd和σc较小时，越偏离中心点的像素，其计算出的权重越低。使用可能的整数域内的(u, v)组合计算出邻域内总体能量函数的最小解，然后在以其为中心3✖️3大小的窗口内拟合出一个抛物线并取极值点，并进行插值计算得到浮点格式的光流向量。接下来将计算得到的光流向量场传递到另外一个双边滤波器中，详细逻辑请参照原论文。需要注意的是该滤波器现在处理的是光流(u, v)，而不是单个像素的能量函数e(x, y, u, v)。它是一个具有独立参数σdfix和σcfix的独立滤波器，这两个参数不仅分别独立于σd和σc，甚至它们没有相同的单位，前者表示速度方差，后者表示强度方差。

上面的公式解决了部分问题，但是为了计算更大尺度的运动，需要在速度空间上搜索更大尺寸的窗口，从而找到最优的能量函数E(x, y, u, v)最小化的解。和计算机视觉领域内的大多数技术实现一样，简单光流算法也使用了图像金字塔技术来解决这个问题。通过这种方式可以在高层的低分辨率图像中寻找到低精度的光流，并在低层高分辨率图像中得到高精度的结果。从高层向下计算的过程中，都是通过上一层的结果上采样后对所有的新像素进行插值计算。上采样使用的技术是联合双边上采样(Joint Bilateral Upsampling)，该技术基于一个事实，即尽管我们对光流的解进行上采样，但是我们已经能够访问更高分辨率的图像。联合双边滤波器还引入了两个参数σdup和σcup，它们分别表示滤波器的空间和颜色范围特征。

简单光流算法一个重要的贡献就是作者提出的流不规则图(Flow Irregularity Map)，它计算了每个点周围像素的光流和该点光流的差值，其定义为如下公式。

当光流算法在图像金字塔上向底层高分辨率图像移动时，使用该公式计算不规则程度，如果不规则度小于参数τ，则只计算大小为N的像素块的顶点光流，内部像素的光流通过插值计算。反之如果不规则度超过参数τ，则在该层需要重复之前的光流计算过程。

2.3.1 相关接口

OpenCV中实现简单光流的相关接口如下，从OpenCV3.0开始，该接口被移至库opencv_contrib的optflow模块。

// 计算简单光流
// from：t0时刻的输入图像，元素格式为CV_8UC3
// to：t1时刻的输入图像，元素格式为CV_8UC3
// flow：计算得到的光流，元素格式为CV_32FC2
// layers：金字塔层数，通常建议设置为5
// averaging_block_size：求解光流时的窗口大小，奇数，通常建议设置为11
// max_flow：最大光流的搜索范围，奇数，通常建议设置为20(原书建议值)
//           求解单个像素邻域总体能量函数最小解时估计的光流范围
void cv::optflow::calcOpticalFlowSF(InputArray from, InputArray to, OutputArray flow,
                                    int layers, int averaging_block_size, int max_flow);

// 计算简单光流
// sigma_dist：能量函数最小化过程中使用的双边滤波器的参数σd，见前文
// sigma_color：能量函数最小化过程中使用的双边滤波器的参数σc，见前文
// postprocess_window：前文讲到速度场交叉双边滤波器的窗口大小
// sigma_dist_fix：前文讲到的速度场交叉双边滤波器的参数σdfix
// sigma_color_fix：前文讲到的速度场交叉双边滤波器的参数σcfix
// occ_thr：遮挡检测阈值
// upscale_averaging_radius：前文讲到的上采样双边滤波器的窗口大小
// upscale_sigma_dist：前文讲到的上采样双边滤波器的参数σdup
// upscale_sigma_color：前文讲到的上采样双边滤波器的参数σcup
// speed_up_thr：前文讲到的检测不规则度确定是否需要在金字塔新层上重新计算光流的阈值
void cv::calcOpticalFlowSF(InputArray from, InputArray to, OutputArray flow,
                           int layers, int averaging_block_size, int max_flow,
                           double sigma_dist, double sigma_color, int postprocess_window,
                           double sigma_dist_fix, double sigma_color_fix,
                           double occ_thr, int upscale_averaging_radius,
                           double upscale_sigma_dist, double upscale_sigma_color,
                           double speed_up_thr);

上述接口后者支持更高的定制化，这个接口是预留给该领域内真正的专家所使用的，在实际使用前建议阅读算法相关论文。前者所使用的默认参数见下表。

3 均值漂移算法和Camshif追踪

本小节将会讨论均值漂移算法(Mean-Shift)和Camshif追踪技术，后者可以理解为是连续的适应均值漂移算法。在很多应用程序中，前者都是一个很常见的数据分析技术，计算机视觉程序只是其中的一种。另外该技术在本系列文章前面关于背景分割的章节中已经有介绍。本小节会先介绍均值漂移算法的基本理论，然后讨论在OpenCV中它是如何被用于在图像中追踪目标的。接下来再介绍基于均值漂移技术的Camshit追踪技术，它能够追踪在视频序列中大小发生改变的目标。

3.1 Mean-Shift算法

均值漂移算法是一个相当深的话题，这里的讨论仅仅限于让读者对该算法有一个直观的了解，如果需要知道更多的细节请阅读论文《Introduction to Statistical Pattern Recognition.》和《Mean shift analysis and applications》。该算法能够在数据集的密度分布中寻找局部极值，即局部样本最集中的分布点，也可以认为是样本集合的质心，另外该算法是一个稳健的方法。对于连续分布而言，该算法可以认为是对数据的密度直方图应用爬山算法(Hill Climbing)，实际上均值漂移算法的某部分还与尺度相关，准确的讲，载连续分布中，均值漂移算法等价于首先使用均值漂移核对图像进行卷积，然后应用爬山算法。

这里的算法稳健性指的是从统计学角度考虑，也就是说均值漂移算法会忽略离群点，即那些原理数据峰值的点。算法每次仅处理数据集中的某个窗口中的点，然后再移动窗口，从而保障其稳健性。

算法的主要流程如下。
1.选择一个搜索窗口

• 确定初始位置搜索位置，即假定的质心
• 确定核函数类型(均匀、多项式、指数或者高斯)，下文介绍算法流程时会解释核函数
• 确定窗口形状(对称或倾斜、可能的旋转、圆形、或者矩形)
• 确定窗口尺寸(卷起或被切断的程度)

2.计算窗口的质心(可能是加权的)，即分布中心
3.质心为中心居中搜索窗口
4.返回到第二步直至窗口停止移动，通常情况下设置最大迭代次数或者窗口中心位移的最小距离来控制循环的终止条件，该距离一定是收敛的

均值漂移算法和核密度估计Kernel Density Estimation的原理相关，核密度估计得名于其依赖每个样本的核函数的积分和来近似估计样本的概率分布，这些核函数关联的概率密度函数都收敛于某个特定值，例如高斯分布函数。通过足够带有合适权重和大小的核覆盖到足够的样本点，则累计出的曲线可以近似看作是整个样本的概率密度分布。而均值漂移算法和核密度估计的区别在于前者只计算样本分布在特定样本上的梯度。当梯度趋于0时，即找到了该分布的峰值，尽管是局部的。该算法计算结束后样本分布空间内在不同区域或者尺度可能存在多个峰值。

核密度估计的计算示例如下图，假定有6个样本，它们的值为【-2.1、-1.3、-0.4、1.9、5.1、6.2】，左图中是他们的分布直方图，这是一种简易的概率密度分布估计。而右图中对于每个样本都分配一个正态分布函数，通过累计所有样本点的分布函数就能近似得到整个样本集的近似概率密度函数。

在介绍正式的均值移动之前再看一个简化的算法，即只考虑使用矩形均匀分布卷积核。则指定窗口内的样本中心可以通过如下方式计算，这里关于矩M的定义可以查看本系列文章的轮廓章节。

接下来介绍正式的均值漂移算法，首先根据核密度估计定义，点X处附近的概率公式表示如下。

核函数K为径向对称函数，可以表示为如下形式。

累加函数的导数直接对函数本身求导，利用复合函数链式求导法则可得。

这里计算出的均值漂移向量可以用于移动窗口，使之以计算得到的质心居中。当然这个向量最终会收敛于向量θ，则最终的收敛中心就是该窗口内样本分布的局部峰值。选择不同的窗口可能会找到不同的峰值，因为它们基本上是尺度敏感结构。

下图是一个计算二维样本的局部峰值示例，其中小的十字是一系列样本，粗实线框是初始的搜索窗口，细实线框和对应的向量表示一次迭代过程计算得到的新窗口和对应的均值漂移向量。可以看见该算法具有很强的稳健型，即搜索窗口外的样本不会影响收敛的结果。

1998年的论文《Real time face and object tracking as a component of a perceptual user interface》和《Computer video face tracking for use in a perceptual user interface》第一次将均值漂移算法应用在视频序列的目标追踪上，并在在论文《Nonparametric information fusion for motion estimation》中对其进行了改进。均值漂移算法在图像分析中的应用，意味着算法的实现需要的不是任意的数据点集(可能是多维的)，而是表示被分析样本集密度分布的图像(可以通过反向投影得到)。可以将图像看成是二维空间中点密度的二维直方图，事实证明，对于视觉而言，这也是大部分时间你需要做的事情，通过这种方式可以追踪感兴趣的特征簇的运动。函数的定义如下。

// 返回值：收敛时算法的迭代总数
// probImage：待计算图，可以看作是密度分布图，元素格式为CV_8UC1或者CV_32FC1
// window：初始的搜索窗口
// criteria：算法终止条件
//           对于分布平坦的区域收敛的速度会相对更慢
int cv::meanShift(cv::InputArray probImage, cv::Rect& window,
                  cv::TermCriteria criteria);

该函数可以用于目标追踪，首先选择一种能够表示某个目标的特征分布，如颜色和纹理。例如通过反向投影计算图像中每个像素属于追踪目标的概率，从而得到一个概率分布图。然后在该特征分布上应用均值漂移窗口，即找到概率最集中的像素即质心。接下来对视频的下一帧作同样的处理。算法找到的两个质心之间的向量就是目标的运动向量。

3.2 Camshift追踪

一个相关的算法是Camshift追踪器，它与均值漂移算法的区别在于它的搜索窗口会自动调整大小。当你有一个分段良好的分布时，如保持紧凑的面部特征，随着人脸离相机的距离变好，面部图像大小随着变化，则Camshift的搜索窗口也会一起变化。OpenCV中该算法的相关实现接口如下。

// 返回值：算法最终的搜索旋转矩形，矩形方向是通过二阶矩计算
// probImage：待搜索的图像，可以看作是密度分布图，元素格式为CV_8UC1或者CV_32FC1
// window：初始的搜索窗口
//        对于视频程序而言，通常将上一帧的搜索结果作为下一帧的初始输入
// criteria：算法终止条件
//          对于分布平坦的区域收敛的速度会相对更慢
RotatedRect cv::CamShift(cv::InputArray probImage, cv::Rect& window,
                         cv::TermCriteria criteria);

需要注意，很多人的直觉会认为上文的两个算法处理的都是目标颜色特征的分布，实际上它们能够处理任何类型特征的分布，因此它们是轻便、稳定和高效的追踪器。

4 运动模版

Bobick和Davis最早载MIT媒体实验室提出了运动模版(Motion Templates)，该技术随后经过进一步的发展为OpenCV中的相关实现奠定了基础。运动模版是一个追踪一般运动的高效方法，特别适用于手势识别。使用运动模版需要目标物体的轮廓，或者至少是一部分轮廓。物体轮廓可以通过如下几个方式获得。

• 最简单的获取目标轮廓的方式是使用一个相对稳定的相机拍摄，并利用视频文件的帧间差分得到运动目标的边缘，该信息已经足够使得运动模版能够正常工作。
• 使用颜色分割，例如当已知背景是亮绿色时，所有不是亮绿色的物体就可以被判断为前景对象。
• 可以使用本系列文章前序背景分割章节内介绍的技术，训练一个背景分割模型，从而将前景对象或者人的轮廓分割出来。
• 使用动态轮廓技术，例如在一面墙壁上安装近红外光，并使用对近红外光敏感的相机拍摄墙壁，则任何介于中间的物体在拍摄得到的图像中都会显示为阴影。
• 使用热成像仪，任何发热的物体，如脸部都会被识别为前景物体。
• 最后你可以使用本系列文章前序背景分割章节内介绍的分割技术，如金字塔分割和均值漂移分割。

现在假定我们已经有了一个分割较好的目标轮廓，即下图A中的白色矩形，这里使用白色表示矩形内的像素都被设置为和当前系统时间戳相关的浮点值。当矩形移动时，新的轮廓被捕获，再使用当前系统时间戳相关的浮点值设置新的轮廓(图B图C中更亮的矩形)，并让其覆盖旧的轮廓(图B图C中更暗的矩形)。在图C中这些逐渐暗淡的矩形则记录了运动的历史，因此被称为运动历史图(Motion History Image)。

如下图，如果一个轮廓的时间戳和当前时间戳相比，其差值大于指定的参数duration，则该轮廓将被隐藏。

OpenCV中相关的实现如下，从OpenCV3.0开始，相关接口被移动到库opencv_contrib的optiflow模块下，以motempl作为命名空间。

// silhouette：当前时刻的轮廓图，非零值表示轮廓，单通道
// mhi：运动历史图
// timestamp：当前时间戳，通常使用毫秒
// duration：最大追踪时长，单位和属性timestamp相同
void cv::motempl::updateMotionHistory(cv::InputArray silhouette,
                                      cv::InputeOutputArray mhi,
                                      double timestamp, double duration);

当使用该函数处理多个连续视频帧后，通过计算参数mhi表示的运动历史图的梯度就能够得到总体的运动。在计算这些梯度时，如使用Scharr或者Sobel梯度函数，会得到一些无效的较大梯度。考虑目标外轮廓的外部区域，它们的值都为0，因此很容易在此处产生较大的假梯度。因为已知调用函数updateMotionHistory的时间间隔，因此可以估计梯度的最大值。接下来就可以使用梯度的量级来清除这些较大的假梯度，最后就能计算出目标的全局运动。OpenCV中提供的计算目标运动的函数如下。

// mhi：运动历史图，基本数据类型为浮点型
// mask：计算得到的蒙版图像，单通道8位，非0值表示该像素点计算得到有效梯度
// orientation：计算得到的梯度方向角度，基本数据类型为浮点型
// delta1：有效梯度最小阈值
// delta2：有效梯度最大阈值
// apertureSize：梯度计算器点孔径大小，值为-1时使用SCHARR梯度算子
void cv::motempl::calcMotionGradient(cv::InputArray mhi, cv::OutputArray mask,
                                     cv::OutputArray orientation,
                                     double delta1, double delta2,
                                     int apertureSize = 3);

参数delta1和delta2可以过滤较大的假梯度，理想的梯度值应该是调用函数calcMotionGradient的平均时间间隔，通常参数delta1设置为0.5倍理想梯度，参数delta2设置为1.5倍理想梯度。参数apertureSize指定了梯度计算器点孔径大小，其取值是有限的序列，当取值为-1时表示使用3✖️3的cv::SCHARR梯度滤镜，取值为1时使用简单的两点中心差分倒数，取值为3时表示使用3✖️3的Sobel滤镜，取值为5时表示使用5✖️5的Sobel滤镜，取值为7时表示使用7✖️7的Sobel滤镜。

OpenCV提供如下函数来计算总体的运动方向。

// 返回值：计算得到的总体运动方向角度，位为角度，取值范围为[0, 360)
// orientation：梯度方向角度，通过函数calcMotionGradient计算
// mask：有效梯度蒙版图像，通过函数calcMotionGradient计算
// mhi：运动历史图，通过函数updateMotionHistory计算
// timestamp：当前时间戳，通常使用毫秒
// duration：最大追踪时长，单位和属性timestamp相同，它决定了计算多长时间内的运动
double cv::motempl::calcGlobalOrientation(cv::InputArray orientation,
                                          cv::InputArray mask, cv::InputArray mhi,
                                          double timestamp, double duration);

下图是一个计算总体运动方向的例子，其中A图是计算的所有梯度，B图是过滤掉较大假梯度后的结果，C图是所有有效梯度计算出的总体运动方向。

可以从运动历史图mhi中每个轮廓的质心处计算全局运动，但是直接对预先计算好的运动向量求和会更快。

OpenCV也支持对运动模版图mhi分区，并计算该区域的局部运动。在下图中，图A中首先搜索当前时刻的轮廓(a)，然后搜索其边界外足够接近的运动(b)。当找到该运动后，向下(时间戳的差值满足算法预设参数)执行浸水填充(Downward Stepping Flood Fill)从而分离局部运动区域(c)，该区域会溢出感兴趣目标的当前位置。图B中当找到该区域后，则计算其局部运动梯度方向。然后在图C中移除该区域(d)，并在图D中重复图B的逻辑，直至寻找到所有的区域。

OpenCV提供的分区接口如下。

// mhi：运动历史图，单通道浮点型数据
// segMask：分割出的区域，元素格式为CV_32FC1
//          算法会使用非0整型递增数值(如1，2等)填充矩阵segMask用于表示不同的区域，
//          0表示未检测到运动
// boundingRects：感兴趣的连接区域，算法计算出分区后会使用矩形表示每一个分区，随后
//               使用函数calcGlobalOrientation计算该区域的梯度
// timestamp：当前的时刻，即mhi中最新轮廓的时间戳，单位通常为毫秒
// segThresh：区域分割的阈值，如果两个轮廓之间的时间戳差值大于等于该值，则将会判定
//           为两个区域，通常最好设置为1.5倍平均相邻轮廓时间戳的差值
void cv::motempl::segmentMotion(cv::InputArray mhi, cv::OutputArray segMask,
                                vector<cv::Rect>& boundingRects,
                                double timestamp, double segThresh);

下面简单分析OpenCV代码仓库目录opencv_contrib/modules/optflow/samples中的文件motempl.cpp，其中函数update_mhi()通过帧间差分的阈值处理后得到前景对象的边缘轮廓图，并使用函数updateMotionHistory计算运动历史图，其代码片段如下。

...
cv::absdiff(buf[idx1], buf[idx2], silh);
cv::threshold(silh, silh, diff_threshold, 1, cv::THRESH_BINARY);
cv::updateMotionHistory(silh, mhi, timestamp, MHI_DURATION);
...

经过上述步骤多次处理后得到运动历史图mhi，接下来计算其有效梯度，并计算分区域的局部运动，代码片段如下。

...
cv::motempl::calcMotionGradient(mhi, mask, orient,
                                MAX_TIME_DELTA, MIN_TIME_DELTA, 3);
vector<cv::Rect> brects;
cv::motempl::segmentMotion(mhi, segmask, brects, timestamp, MAX_TIME_DELTA);
...

得到运动分区后，在如下代码中使用一个for循环来计算所有分区的局部运动。循环的第一个i取值为-1，表示计算整个图像的全局运动。在处理局部运动时，如果分区面积过小，则不会计算该区域的局部运动，直接跳到下一次循环。这个示例程序并没有使用精确的蒙版矩阵计算局部运动，而是使用了包围矩形，但是在接下来过程中通过计算有效运动梯度来清除大部分由非精确蒙版矩阵引入的误差。代码段最后对这些运动可视化。

...
for (i = -1; i < (int)brects.size(); i++) {
    cv::Rect roi; Scalar color; double magnitude;
    cv::Mat maski = mask;
    if (i < 0) {
        // case of the whole image
        roi = Rect(0, 0, img.cols, img.rows);
        color = Scalar::all(255);
        magnitude = 100;
    } else {
        // i-th motion component
        roi = brects[i];
        // reject very small components
        if (roi.area() < 3000) {
            continue;
        }
        color = Scalar(0, 0, 255);
        magnitude = 30;
        maski = mask(roi);
    }

    double angle = cv::motempl::calcGlobalOrientation(orient(roi), maski,
                                                      mhi(roi), timestamp,
                                                      MHI_DURATION);
    // ...[find regions of valid motion]...
    // ...[reset ROI regions]...
    // ...[skip small valid motion regions]...
    // ...[draw the motions]...
}
...

示例程序的完整源码请参考官方仓库，示例程序的运行结果如下。该程序检测了记录一个人挥舞手臂的视频文件，总共分析了8个连续的视频帧，每帧视频处理的结果都放在了原图上方。另外程序使用了形状描述符(Hu矩)匹配了手势特征，在第4和第5帧图像中识别到了Y姿势。有关Hu矩更多的知识可以参考本系列文章的前面章节轮廓。图中大的八边形表示全局运动，而较小的八边形表示的是局部运动。

5 估计器

假定正在追踪一个在摄像机前走动的人，每一帧都需要确定他的位置，像我们了解到那样，可以通过很多方式确定他的位置。但是这些方法其实都是在每帧画面内对人的位置估计。这个估计可能不是非常准确的，这些误差可能由很多原因导致。如传感器对午餐，早期处理阶段的近似计算，由遮挡或者阴影引发的问题，由于人在行走时手臂或者腿部的摆动导致的明显形状变化。无论原因是什么，我们都可以预料这些测量值会和可能通过一些理想的传感器或者传感方法获取的相对准确值不同，这种差异可能是随机的。我们可以将所有的这些不准确因素看成是测量过程中的误差。

我们可以通过最大程度的利用已有的测量结果估计这个人的运动。这样大量测量结果累积可以使得我们检测到被观察人物运动轨迹中不受噪声影响的那部分。一个简单的例子就是当我们已知人是静止的时候，直觉告诉我们可能可以通过对所有测量结果求平均值从而得到最优的位置。运动估计(Motion Estimation)同时解决了为什么我嘛的直觉在处理静止人物时会使用上述的方式，以及更重要的是如何将这种方法推广到运动物体中。

额外的关键因素是需要知道人物运动的模型。例如我们可以通过如下的陈述对一个人的运动建模，即一个人从一边进入场景，并且以一个恒定的速度向另一边穿越场景。通过这个模型，我们可以验证观察到的人物位置和运动速度的最佳值。

任务分为下图所示两个阶段，第一个阶段称为预测间断(Predicttion Phase)，在该阶段我们使用已经学习到历史信息优化模型，并预测目标对象下一时刻的位置。第二阶段称为校正阶段(Correction Phase)，在该阶段进行测量，并基于上一次的预测值对当前测量值进行优化。

实现这个两阶段任务的工具被称为估计器(Estimators)，其中Kalman滤波器是使用最为广泛的一个技术，另外一个重要的方法是凝聚算法(Condensation Algorithm)，它是一种被称为粒子滤波器(Particle Filter)的更广泛方法在计算机计算机视觉领域内的实现。它们的主要区别在于状态概率密度的描述不同。在下面的小节中，我们会详细探索Kalman滤波器，然后简单介绍一些与之相关的技术。我们不会详细介绍凝聚算法，因为在OpenCV中该算法并为被实现。但是我们仍然将会简单介绍它，以及他和Kalman滤波器相关技术的区别。

5.1 卡尔曼滤波器

从1960年该算法提出，Kalman滤波器就因其在大量的信号处理场景中具备很好的性能而崛起。Kalman滤波器的基本思想是，在强力并且合理的假定条件下，给定一个系统的历史测量，可能建立系统当前状态的模型，并基于这些历史测量最大化后验概率(Posteriori Probability)。另外事实证明，我们不需要保存很长的系统历史测量就可以估计出当前状态的模型。事实上，通常我们在连续测量时交错式的更新系统的状态模型，并只保留下次迭代所需要使用的最新模型。下文会简单介绍Kalman滤镜，详细解释清参考论文《An introduction to the Kalman filter》。

5.1.1 输入和输出

Kalman滤波器作为一个估计器，它可以帮我们整合如下信息，即已有的和系统状态相关信息，已有的和系统动态相关信息，系统运行时可能从中学习到的新信息。在实践中，对于我们表示这些事情的方式存在重要的限制。第一个重要的限制就是Kalman滤波器只能处理系统的状态能够被表示为向量和矩阵的场景，其中向量的值表示系统没个自由度的当前值，而矩阵表示这些自由度的不确定性，这里使用矩阵是因为我们不仅关注单个自由度的方差，还关注不同自由度之间的协方差。

状态向量(State Vector)可以包含任意数量的成员，只要我们认为它们和当前工作的系统相关。在上面讨论到的关于观察行走的人物示例中，这个状态向量可以表示为如下形式。其中x🌟和v🌟分别表示人物的位置和速度。这里的🌟表示我们讨论的是最佳估计。

在有些场景中将其展开成如下二维形式。

在更复杂的系统中，状态向量的元素量可能会更多，另外状态向量通常包含一个下标表示时间，星号表示我们讨论的是特定时刻的最佳估计。通常和状态向量相伴的是协方差矩阵，写成如下形式。如果状态向量的元素数为n，则矩阵的大小为n✖️n。

Kalman滤波器允许在已知状态(x0，∑0)的情况下，估计下一个状态(xi，∑i)。关于这个说法，有两个需要注意的点。第一个是这些状态都有相同的形式，就是说它被表示为均值，表示的是我们认为最可能的系统状态，而协方差则是表示的是这个均值的不确定性。这意味着Kalman滤波器只能处理特定类型的状态。例如，考虑一个沿着道路行驶的汽车，这个系统用Kalman滤镜建模是很合理的。然而当车辆行驶到路口时，下一刻它可能向左或者向右转弯，但就是不会向前直行。如果我们不确定车辆转弯的方向，Kalman滤波器就无法正常工作。这是因为滤波器表示的状态是高斯分布，简单的说是中心突出的分布，它是单峰的，不像凝聚算法不限定于单峰分布。因此它无法预测汽车在方向上的剧烈改变。

第二个重要的点是算法假设系统启动时可以使用这样一个高斯分布描述其初始状态。这个出事状态表示为(x0，∑0)，被称为先验分布(Prior Distribution)。使用无信息的先验分布(Uninformative Prior)启动系统总是可能存在的，此时均值是任意值，协方差则会很大，意味着我们完全不知道当前系统的状态。但是重要的是我们必须提供一个使用高斯表示的先验分布。

5.1.2 Kalman滤波器的前提条件

在详细了解Kalman滤波器之前，需要先认识下前文提到的强力并合理的假设集合。在Kalman滤波器的理论结构中要求三条重要的假设，(1)建模的系统是线性的。(2)测量到的噪声是白噪声。(3)测量到的噪声本质上是高斯的。第一条假设意味着如果将系统在k时刻的状态表示为向量vt，则在k+1时刻系统的状态表示为向量vt+1，那么vt+1=F🌟vt，这里F为一个矩阵。第二和第三条假设意味着系统的噪声在时间上是无关联的，并且他们的幅度可以仅仅通过平均值和协方差(例如噪声可以使用一阶和二阶矩描述)精确建模。尽管这些假设看上去使得算法很受限，但是实际上对于很广泛的场景这些条件都是满足的。

基于历史测量最大化后验概率，指的是在测量后考虑之前模型及其不确定性，和新测量结果和其不确定性构建出的模型，是正确模型的概率最高。出于我们的目的，这意味着满足前文所述的三条假定条件下，Kalman滤波器是整合数据的最佳方式，这些数据可能来自于不同的源，或者来自于同一来源的不同时刻。我们基于已经信息和获取到的新信息，使用新旧信息的加权联合结果确定我们对新旧信息的不确定性，决定是否需要修改已知信息。接下来让我们在一维运动中使用一些数学知识来进一步了解Kalman滤波器。

5.1.3 信息融合

Kalman滤波器的核心是信息融合(Information Fusion)。在一维场景中，假定你想知道点位于线上的位置(对于类似轨迹的详细解释请参考Kalman Filters: A Tutorial。考虑到噪声因素，假定计算出两个不可靠的位置结果x1和x2，它们都是基于高斯分布的模型。即模型包含平均值x-1和x-2，以及标准差σ1和σ2。实际上标准差在这里表示测量结果具有高质量的不确定性。对于每个测量值，和位置相关的隐含概率分布是一个高斯分布，其概率密度公式表示如下。

则对于两个测量值x的总体概率密度，与两个测量值概率密度的乘积p12(x)=p1(x)🌟p2(x)成正比。事实证明其乘积是另外一个高斯分布，如下。显然我们可以计算出这个新高斯分布的均值和标准差。

鉴于高斯分布的概率最大值为平均值，则我们可以计算分布函数的倒数，当倒数为0时分布函数取得最大值，计算公式如下。

由于概率密度函数一定不为0，因此中括号内部的这项一定为0，则可以计算出平均值如下。

可以看出总体概率分布的平均值x-12是原始两个测量模型的平均值的加权和，而且权重仅和两个测量模型的不确定性相关。从上面等式可以看出，如果第二次测量的不确定性σ2非常大，则得到的新的平均值接近于第一次测量的平均值x-1，即不确定性更低的结果。

在计算出平均值x-12后，可以将其带入p12(x)等式，通过重新排列组合多项式的方式计算出总体分布的方差，计算结果如下。

这意味着每当我们观察到一个新的测量值后，都可以将其和老的模型合并得到一个新的模型。进一步说一系列连续的测量值都可以通过这样的方式不断的对已经观察到的结果进行修正，这也正是在计算机视觉追踪领域发生的事情。

假定第n次测量结果表示为(xi, σi)，对测量结果进行校正后的模型表示为(xi🌟, σi🌟)，这里测量值就是平均值。假定我们知目标的初始位置模型为(x0🌟, σ0🌟)，该状态是先验概率(Prior Probability)。则第一次测量后经过校正的模型的平均值x1🌟计算公式如下。

方差σi🌟2计算公式如下。

上述公式中平均值的变化(x1-x0 🌟)被称为创新(Innovation)，另外这里还引入了迭代因子的概念，它也被称为更新增益(Update Gain)其表达式如下。

使用K的定义可以获得如下便利的递归表达式。

在Kalman滤波器的相关文献中，通常使用k表示当前时刻，k-1表示上一时刻，下图演示了从k-1时刻到k时刻到剧烈变化，即从N(Xk-1🌟, σk-1🌟2)变化到N(Xk, σk2)，以及优化后的模型N(Xk🌟, σk🌟2)。

5.1.4 动态系统

在前面的例子中我们并没有考虑在两次测量直接系统的状态动态变化，即车辆的运动。正如前文所讲，在完成一次新的测量之后，在整合已有模型和当前测量之前，需要对已有模型的变化进行预测，该阶段称为预测阶段。例如在考虑上文描述的汽车行驶的例子中，假定已知汽车行驶速度为v，在t时刻测量的位置为xt，t1时刻测量的位置为xt1，则我们会基于xt和v预测出在t1时刻的位置pxt1，然后再整合xt1和pxt1等信息得到新的模型。Kalman滤波器只会考虑3种运动。

第一种是动态运动(Dynamical Motion)，该类型运动可以直接从上一次测量的系统状态计算确定。例如在t时刻目标的位置为xt，速度为v，则在t+dt时刻预测目标位置为xt+v🌟dt，此时速度仍可能是v。

第二种运动是控制运动(Control Motion)，这类运动是由一些因为某种原因我们已经意识到的外部影响力对系统造成的运动。正如其名字一样，控制运动最常见的例子就是在估计受控系统的场景，此时我们的行为对系统的运动影响是已知的。这种例子常见于机器人系统中，此时系统控制着机器人，作出例如加速或者向前走的行为。很明显如果机器人在t时刻的位置为xt，速度为v，则在t+dt时刻其位置不是只像未受控制时移动到xt+v🌟dt，还可能走得更远，因为我们知道我们已经告诉机器人加速。

最后一种重要的运动类型是随机运动(Random Motion)，即使在前文讲到的简单的单维例子中，如果我们观察的任何目标都有可能由于某种原因产生各自的运动，则在预测阶段它们将会被考虑。这些随机运动的作用只会仅仅随时间变化增加估计状态的不确定性。随机运动包含任意未知或者不受控制的运动。然而正如Kalman滤波器框架内其他部分一样，存在这样一个假设，即随机运动是高斯的，例如一种随机的走路，或者至少该随机运动可以有效的建模为高斯。

在讲新的测量值融合进仿真模型之前是更新阶段(Update Step)，该阶段首先考虑动态运动，即根据目标之前的状态直接推测出的运动，再考虑控制运动，即我们或者系统外部代理程序的行为所产生的运动，最后再考虑随机运动。通过着三种运动对已有模型进行更新后再和新的测量值进行融合。

实际上，对于状态比仿真模型更复杂的系统而言，动态运动尤其重要。当目标在移动时，状态向量包含多个分量，如目标的位置和速度。当然在这种场景中，系统的状态会根据我们认为它应该具备的速度发生变化。处理多个状态分量的系统是下一小节所要讨论的事情，同样我们也需要了解更多的数学知识。

5.1.5 Kalman方程

接下来开始将前文所描述的知识整合到一起，从而理解如何处理更通用的模型。首先我们将讨论推广到包含很多状态变量的系统中，最简单的例子可能在视频追踪领域中，此时目标能够在二维和三维中移动。总的来说，状态可能包含额外的元素，例如被追踪目标的速度。k时刻的状态xk可以通过k-1时刻的状态xk-1，和控制运动相关变量uk，和随机运动相关的变量wk等计算得到，其计算公式如下。

该公式中xk是一个n维向量，表示k时刻系统状态。F是一个n✖️n矩阵，有时称为转移矩阵(Transfer Matrix)或者转换矩阵(Transition Matrix)，它和动态运动相关。uk是一个c维向量，也被称为控制输入，它和控制运动相关(Control Inputs)，它让系统可以受外部控制。B是将这些控制输入和运动相关联的n✖️c矩阵。wk是和直接影响系统状态的事件或者力量相关的随机变量。我们假定wk的分量是一个以0为均值的高斯分布N(0，Qk)，其中Qk是n✖️n的协方差矩阵，理论上讲该矩阵可以随着时间变化，但是通常不会。

一般而言，我们取得的测量值Zk不一定是真正的状态向量Xk，可以能只是间接的测量值。如果想要确定移动中的车辆速度，可以通过雷达枪直接测量其速度，也可以通过测量尾管发出的声音来间接测量车辆速度。第一种方法中，测量值Zk是实际值Xk再加上一些测量误差。第二种方法中，测量值和实际值之间并没有直接的关系。可以通过如下公式来表示m维测量向量和实际向量之间的关系。

这里H是一个m✖️m矩阵，vk是测量误差，也假定该误差是高斯分布N(0, Rk)，其中Rk是m✖️m的协方差矩阵，这里使用k的原因是H和R可能随着时间变化，但是实际上它通常不会变化。

这里先考虑一个简单的场景，即在停车场行驶的汽车，他的状态向量Xk可以包含4个分量分别是代表位置的x和y，以及在这两个方向上的速度Vx和Vy，则均值和转移矩阵F可以表示为如下形式。

然而当使用相机来测量汽车状态时，可能只能得到如下位置信息。

则隐含的矩阵H结构和如下结构类似。

接下来开始简略推导Kalman方程，由于省略了部分细节可能导致理解困难，建议如果有兴趣可以详细阅读原论文。首先每个更新周期内，在获取新测量结果之前，由上一个阶段状态推导出的状态xk-可以由如下公式计算。这里上标符号-指的是在在获取新测量结果之前的预测值。

在获取新测量结果之前，由上一个阶段状态推导出的状态分量的协方差矩阵可以由以下公式计算，其中Qk-1是对k-1时刻的过程误差协方差矩阵。

Kalman增益矩阵(Kalman Gain)，也称为混合因子(Blending Factor)可以通过如下等式计算。其中RK和测量值的准确性相关。推导过程可参考博文。

为了理解上述公式，这里考虑极简单的场景，即对于系统状态只包含汽车速度的场景，则协方差矩阵∑k-是1✖️1的矩阵，为σk2，Hk和Rk都是1✖️1矩阵，它们的值分别为1和σk+12，则k时刻的Kalman增益系数Kk可以表示如下。

则校正后的系统状态和不确定性计算公式如下。

5.1.6 OpenCV相关接口

OpenCV中的相关接口如下。

class cv::KalmanFilter {

public:
// 默认构造函数
cv::KalmanFilter();

// 构造函数
// dynamParams：状态向量x_k的维度
// measureParams：测量向量z_k的维度
// controlParams：控制向量u_k的维度
// type：矩阵的基本数据格式，取值为CV_32F或者F64
cv::KalmanFilter(int dynamParams, int measureParams,
int controlParams = 0, int type = CV_32F);
// 重新设置部分属性
void init(int dynamParams, int measureParams,
int controlParams = 0, int type = CV_32F);

// 数据准备好后可以调用如下两个方法开始工作
// 计算新测量前的当前状态预测值，该方法调用后会更新statePre和statePost的值
// control：控制向量u_k
const cv::Mat& predict(const cv::Mat& control = cv::Mat());
// 根据预测值校准测量结果，该方法调用后会更新statePost的值
// measurement：测量得到的向量z_k
const cv::Mat & correct(const cv::Mat& measurement);

// 转移矩阵F
cv::Mat transitionMatrix;
// 控制矩阵B
cv::Mat controlMatrix;
// 测量矩阵H
cv::Mat measurementMatrix;
// 流程噪声协方差矩阵Q
cv::Mat processNoiseCov;
// 测量噪声协方差矩阵R
cv::Mat measurementNoiseCov;

// 预测的状态向量x'_k
// x'_k = F * x_(k-1) + B * u_k
cv::Mat statePre;
// 校正后的状态向量x_k
// x_k = x'_k + K_k * ( z_k – H * x'_k )
cv::Mat statePost;
// 预测的状态向量分量的协方差矩阵P'_k
//  Sigma'_k = F * Sigma_(k-1) * F^t + Q
cv::Mat errorCovPre;
// Kalman增益矩阵K_k
// K_k = Sigma'_k * H^t * inv(H*Sigma'_k*H^t+R)
cv::Mat gain;
// 校准后的状态向量分量的协方差矩阵Sigma_k
// Sigma_k  = ( I - K_k * H ) * Sigma'_k
cv::Mat errorCovPost;
...
};

5.1.7 示例程序

接下来通过一个假设的模型加深对Kalman滤镜的理解，假定一个汽车绕着一个点做圆周运动这样一个系统，汽车运行的角速度几乎不变，但是会受一些噪声影响，即过程噪声(Process Noise)。我们使用视觉算法追踪汽车的位置，当然测量的结果也存在误差，即测量噪声(Measurement Noise)。

这里使用的模型很简单，状态向量xk包含两个分量，分别是表示位置的角度和角速度，测量向量zk只包含1个分量，为表示位置的角度。程序KalmanFilter对这个动态系统进行了预测和校正。

首先定义一个宏，用于讲角度转换为二维平面上的坐标。

// 根据状态向量确定点的位置。
// 这里状态向量是通过矩阵mat的形式表示，矩阵为2行1列，共两个元素分别表示角和角速度。
// 以整幅图片img的中心为原点，图片列数的1/3位半径，确定目标的位置。
#define phi2xy(mat) \
cv::Point(cvRound(img.cols / 2 + img.cols / 3 * cos(mat.at<float>(0))), \
cvRound(img.rows / 2 - img.cols / 3 * sin(mat.at<float>(0))))

接下来定义一个Kalman滤波器，并设置关键属性，和系统的初始状态。

// 1. 初始化Kalman矩阵
// 状态向量维度为2维，分别表示目标的角和角速度
// 测量向量维度为1维，只测量角度
// 控制项链维度为0维，即不会受到外部控制改变运动状态
cv::KalmanFilter kalman(2, 1, 0);

// 2. 设置初始Kalman滤镜计算关键属性
//=======================预测过程参数====================//
// 设置转移矩阵，用于预测k+1时刻的状态向量
// 从物理模型上考虑，k时刻系统状态为(φ, ω)，则k+1时刻系统状态为(φ+ω*dt, w)，
// 则转移矩阵为{1, dt, 0, 1}，这里为了方便设dt=1，实际上应该根据连续两个
// 视频帧的时间差来设置该值
float F[] = {1, 1, 0, 1};
kalman.transitionMatrix = cv::Mat(2, 2, CV_32F, F).clone();
// 设置流程的噪声协方差矩阵，用于预测k+1时刻的状态协方差矩阵
// 假设状态向量维度为N，则其尺寸为N✖️N
cv::setIdentity(kalman.processNoiseCov, cv::Scalar(1e-5));
//=======================校准过程参数====================//
// 设置测量矩阵，用于从k时刻测量结果反应出的校准前矩阵计算实际测量结果的矩阵
cv::setIdentity(kalman.measurementMatrix, cv::Scalar(1));
// 设置测量的噪声协方差矩阵，假设测量向量维度为N，则其尺寸为N✖️N
cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar(1e-1));

// 3. 设置初始的状态向量，和初始的不确定度即状态向量的协方差矩阵
cv::Mat x_k = cv::Mat::zeros(2, 1, CV_32F);
randn(x_k, 0.0, 0.1);
// 设置初始状态为随机值
kalman.statePost = x_k.clone();
// 设置初始状态向量的协方差矩阵
// 这里设置为单位向量表示不同维度之间的分布是独立的，无相关
cv::setIdentity(kalman.errorCovPost, cv::Scalar(1));

然后然后预测系统新状态，再通过一个假定的测量值校准预测值，得到最终的系统状态。并通过迭代不断循环，同时使用不同颜色的圈表示上一时刻的状态，当前时刻预测值，测量值，校准值。

// 4. 循环更新系统状态，并可视化目标位置
// 准备绘图用的矩阵
cv::Mat img(500, 500, CV_8UC3);
// 初始化测量矩阵
cv::Mat z_k = cv::Mat::zeros(1, 1, CV_32F);
// 每次测量时的随机偏差矩阵，用于计算z_k
cv::Mat z_k_r = cv::Mat::zeros(1, 1, CV_32F);
for (;;) {
    // 记录上一次的校准值，用于绘图
    cv::Mat lastCorrectedState = kalman.statePost.clone();

    // 预测当前状态向量
    kalman.predict();

    // 真正的应用程序中应该使用传感的测量值，这里为了演示使用了模拟值
    // 在上一次的测量结果上，加上随机值作为当前的测量向量z_k
    // 这里随机分布的方差取测量误差的标准差
    cv::randn(z_k_r, 0.0,
        sqrt(static_cast<double>(kalman.measurementNoiseCov.at<float>(0, 0))));
    z_k = kalman.measurementMatrix * lastCorrectedState + z_k_r;
    
    // 校准测量值
    kalman.correct(z_k);

    // 绘图
    img = cv::Scalar::all(0);
    // 用蓝色圆表示上一次的测量值
    cv::circle(img, phi2xy(lastCorrectedState), 6, cv::Scalar(255, 0, 0), 2, cv::LINE_AA);
    // 用绿色圆表示预测值
    cv::circle(img, phi2xy(kalman.statePre), 6, cv::Scalar(0, 255, 0), 2, cv::LINE_AA);
    // 用红色圆表示观察值
    cv::circle(img, phi2xy(z_k), 6, cv::Scalar(0, 0, 255), 2, cv::LINE_AA);
    // 用白色圆表示校准后的值
    cv::circle(img, phi2xy(kalman.statePost), 6, cv::Scalar(255, 255, 255), 2, cv::LINE_AA);
    cv::imshow("Kalman", img);

    // 挂起程序
    if ((cv::waitKey(100) & 255) == 27) {
        break;
    }
}

示例程序KalmanFilter的运行结果中连续的6个时刻状态示意图如下，其中蓝色圆表示上一次的测量值，绿色圆表示预测值，红色圆表示观察值，白色圆表示校准后的值。刚开始，由于预测的状态模型协方差较于测量值协方差更高，因此校准后的白色圆更偏向测量值，随后由于状态模型协方差不断降低，因此校准后的白色圆更偏向预测值。

在上面的例子中并未使用到控制输入，假如系统受到外部控制，例如通过遥控器控制汽车时，在计算模型状态过程中，需要设置kalman滤波器的controlMatrix属性为控制矩阵B，在调用预测函数predict()时需要传入额外的控制向量uk。

5.2 扩展卡尔曼滤波器简述

上面的示例中使用到的Kalman滤镜测量矩阵、控制矩阵和转移矩阵在系统的生命周期内都保持不变，即要求系统是线性的，显然这种要求十分苛刻。事实证明在非线性的动态系统中Kalman滤镜仍然是有用的，并且OpenCV提供的相关接口也能正常工作。

假定可以控制行驶汽车的油门，显然汽车的速度和油门力度之间是非线性变化的。另外当一艘小船绕固定点做圆周运动时，如果水流按固定方向移动，这也是非线性系统。在上述两个情况下，基本的Kalman滤镜都不能解决非线性系统问题，即不同通过线性函数从t时刻的状态计算出t+1时刻的状态。一个解决办法是局部线性化(Locally Linearize)相关过程，如转移矩阵F和控制矩阵B。也就是说在每个时刻都要给予状态向量xk计算出新的转移矩阵F和控制矩阵B。尽管这些值只是近似值，但是已经足够了。这种Kalman滤波器的扩展被称为扩展Kalman滤波器(Extended Kalman Filter, EKF)。

OpenCV并未提供额外的实现，因为使用Kalman滤波器的接口，并在每个时刻更新属性transitionMatrix和controlMatrix即可。前文讲到的Kalman滤镜的矩阵更新等式的一般表达如下。

这了函数f和h都是任意非线性函数，则为了使用Kalman滤波器提供的线性方程，使用下面的定义重新计算矩阵F和H的值，这里省略了推导过程。

需要注意这里使用偏导数计算k时刻矩阵F和H值时使用的是k-1时刻的状态向量。则k时刻预测的状态向量和测量向量的公式可以表示为如下形式。需要注意在上式中函数f是状态向量和控制向量的多元函数，因此控制矩阵B被吸收到了矩阵F之中。

这样Kalman滤波器就被很优雅的扩展到了非线性系统中，这也被称为Unscented粒子滤镜(Unscented Particle Filter)《The unscented particle filter》。另外论文《Thrun, S., W. Burgard, and D. Fox. Probabilistic Robotics: Intelligent Robotics and Autonomous Agents, Cambridge, MA: MIT Press, 2005.》也对Kalman滤波器进行了很好的概括，还包含了最近的研究进展以及例子滤镜。

6 小节

上一章对稀疏光流算法进行了介绍，本章在此基础上对稠密光流算法进行了讨论，并且还介绍了均值漂移算法、Camshift算法和运动模版技术。在最后讨论了一些递归估计器，如Kalman滤波器。最后还对其进行了扩展，讨论了如何处理不完全遵守Kalman滤波器前提条件的更复杂的系统。在后续章节《附录B》中的函数optflow和tracking还包含了更多的光流和追踪算法。