文章目录
魔改例子
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <stdio.h>
using namespace std;
using namespace cv;
/** @brief 计算圆周上的点相对窗口的坐标值 = 中心点 + 圆周上的点相对圆心
@param center 中心点
@param R 半径
@param angle 角度,因为坐标原点在左上角,角度是逆时针的(和通常的极坐标系顺时针的相反),所以sin前有个负号
*/
Point calcPoint(Point2f center, double R, double angle)
{
angle = angle / 180.0 * 3.1415926;
return center + Point2f( (float)cos(angle), (float)-sin(angle) ) *(float)R;
}
/** @brief 画X形十字线,通过中心点和半长计算四个顶点的坐标
@param img 要作画的图像.
@param center 中心点.
@param d 一半长.
@param color 颜色.
*/
void drawCross(const Mat & img, Point center, int d ,const Scalar & color){
line( img, Point( center.x - d, center.y - d ),Point( center.x + d, center.y + d ), color, 4);
line( img, Point( center.x + d, center.y - d ),Point( center.x - d, center.y + d ), color, 5);
}
int main(int, char**)
{
Mat img(800, 800, CV_8UC3);
/** @brief 创建卡尔曼滤波器对象KF.
@param dynamParams 2,状态向量的维度,二维列向量(角度,△角度)
@param measureParams 1,测量向量的维度,一维列向量(角度)
@param controlParams 0,控制向量的维度
*/
KalmanFilter KF(2, 1, 0);
/* 定义三个列向量:状态向量Xk(state)、测量向量Zk(measurement)和控制向量Uk(control) */
// 定义状态向量Xk(state):不用它的值,所以不用初始化
// 定义测量向量Zk(measurement)
// for循环中需要传入其测量值,所以初始化
Mat measurement = Mat::zeros(1, 1, CV_32F);
// 控制向量Uk(control)简单实例就不考虑了
/* 初始化三个矩阵:状态转移矩阵Ak(transitionMatrix),控制矩阵Bk(controlMatrix)和测量矩阵Hk(measurementMatrix) */
// 初始化状态转移矩阵Ak(transitionMatrix):定值
KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
// 初始化测量矩阵Hk(measurementMatrix):单位矩阵化
setIdentity(KF.measurementMatrix);
// 控制向量都没有,控制矩阵Bk(controlMatrix)更没有。
/* 两个估计的状态向量:先验估计的状态向量(statePre)和后验估计的状态向量P(k-1|k-1)(statePost) */
// 先验估计的状态向量(statePre)第一次循环完在函数predict中就自己初始化了
// 初始化后验估计的状态向量P(statePost):
randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
/* 定义两个噪声:系统噪声Wk和测量噪声Vk */
// 定义系统噪声Wk:二维列向量
Mat processNoise(2, 1, CV_32F);
// 测量噪声Vk之后再说
/* 两个噪声的协方差矩阵: 系统噪声的协方差矩阵Qk(processNoiseCov)和测量噪声的协方差矩阵Rk(measurementNoiseCov)*/
// 系统噪声的协方差矩阵Qk(processNoiseCov)
setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
// 测量噪声的协方差矩阵Rk(measurementNoiseCov)
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-5));
/** @brief 初始化系统噪声
不加噪声的话就是匀速圆周运动,加了点噪声类似匀速圆周运动,因为噪声的原因,运动方向可能会改变
*/
randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
/* 两个错误估计的协方差矩阵: 后验错误估计的协方差矩阵P(errorCovPost)和*/
// errorCovPost 后验错误估计的协方差矩阵P
setIdentity(KF.errorCovPost, Scalar::all(1));
// errorCovPre 先验错误估计的协方差矩阵P之后再说
/* 圆周运动的设定 */
// center图像中心点
Point2f center(img.cols*0.5f, img.rows*0.5f);
// 半径
float R = img.cols/3.f;
// 退出waitKey()的返回值
char code = (char)-1;
for(;;)
{
// 清零画布
img = Scalar::all(0);
/* 上一次的点 */
// 上一次的点的角度stateAngle:取状态向量state的第一个值,即角度
double lastAngle = KF.statePost.at<float>(0);
// 上一次的点的坐标statePt:(在圆周上)相对Mat画布的点。
Point lastPt = calcPoint(center, R, lastAngle);
// 计算预测值
KF.predict();
/* 预测点 */
// 预测点的角度predictAngle:取预测值prediction的第一个值,即角度
double predictAngle = KF.statePre.at<float>(0);
// 预测点的坐标predictPt:(在圆周上)相对Mat画布的点。
Point predictPt = calcPoint(center, R, predictAngle);
/* 测量点 */
// 初始化测量向量Zk(measurement):随机值;然后和上一次的测量值加起来
// 表示每次的测量值都不一样,物体在移动
measurement += Scalar(theRNG().uniform(10.0, 40.0));
// 测量点的角度measAngle:取预测值measurement的第一个值,即角度
double measAngle = measurement.at<float>(0);
// 测量点的坐标measPt:(在圆周上)相对Mat画布的点。
Point measPt = calcPoint(center, R, measAngle);
// 融合更新
KF.correct(measurement);
// 最优点的角度measAngle:取预测值measurement的第一个值,即角度
double optimalAngle = KF.statePost.at<float>(0);
// 测量点的坐标measPt:(在圆周上)相对Mat画布的点。
Point optimalPt = calcPoint(center, R, optimalAngle);
/* 画 */
// 轨道
circle(img, center, R, Scalar::all(70), 1);
// 上一个的点(白色)
drawCross( img , lastPt, 3 ,Scalar(255,255,255));
// 测量点(红色)
drawCross( img , measPt, 3 ,Scalar(0,0,255));
// 预测点(绿色)
drawCross( img , predictPt, 10 , Scalar(0,255,0));
// 最优点(蓝色)
drawCross( img , optimalPt, 10 , Scalar(255,0,0));
// 测量点到预测点
line( img, measPt, predictPt, Scalar(0,255,0), 2);
// 测量点到最优点
line( img, measPt, optimalPt, Scalar(255,0,0), 2);
// cout << "角度:\n";
// cout << lastAngle << endl;
// cout << measAngle << endl;
// cout << predictAngle << endl;
// cout << optimalAngle << endl;
imshow( "Kalman", img );
code = (char)waitKey(400);
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
return 0;
}