卡尔曼滤波目标跟踪实例 opencv

#include <opencv/cv.h> #include <opencv/highgui.h> #include <stdio.h> #include <iostream> #include <map> #include "opencv2/video/tracking.hpp" #include "opencv2/highgui/highgui.hpp" #include "Tracking.h" //#include "Object.h" #include "Detection.h" #include "STCategory.h" #include "Frame.h" #include "DynamicFeature.h" #include "BoundingBox.h" //#include "BoundingBox.h" using namespace cv; using namespace std; Object m_ObjectOK; //存储匹配成功的Object Target m_TargetOK; //存储匹配成功的Target Object m_ObjectSelf; //存储跟踪目标的Object static inline Point calcPoint(Point2f center, double R, double angle) //返回坐标点 { return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R; } //判断两个map是否相等 int DynamicFeature::m_CompareMap(map<EMColor, float> m_Map1, map<EMColor, float> m_Map2) { int OK = 1; if (m_Map1.size() != m_Map2.size()) { OK = 0; return OK; } else { if (m_Map1[EMColor_white] != m_Map2[EMColor_white]) { OK = 0; } if (m_Map1[EMColor_black] != m_Map2[EMColor_black]) { OK = 0; } if (m_Map1[EMColor_gray] != m_Map2[EMColor_gray]) { OK = 0; } if (m_Map1[EMColor_red] != m_Map2[EMColor_red]) { OK = 0; } if (m_Map1[EMColor_orange] != m_Map2[EMColor_orange]) { OK = 0; } if (m_Map1[EMColor_yellow] != m_Map2[EMColor_yellow]) { OK = 0; } if (m_Map1[EMColor_green] != m_Map2[EMColor_green]) { OK = 0; } if (m_Map1[EMColor_blue] != m_Map2[EMColor_blue]) { OK = 0; } if (m_Map1[EMColor_purple] != m_Map2[EMColor_purple]) { OK = 0; } } return OK; } //计算矩形框的重合率 float DynamicFeature::m_OverlapRect( BoundingBox m_bPreBox, BoundingBox m_bMeaBox)//m_bPreBox为预测矩阵，m_bMeaBox为对比的矩阵 { if (m_bPreBox.m_iLeftUp.x > m_bMeaBox.m_iLeftUp.x + m_bMeaBox.m_nWidth) { return 0; } if (m_bPreBox.m_iLeftUp.y > m_bMeaBox.m_iLeftUp.y + m_bMeaBox.m_nHeight) { return 0; } if (m_bPreBox.m_iLeftUp.x + m_bPreBox.m_nWidth < m_bMeaBox.m_iLeftUp.x) { return 0; } if (m_bPreBox.m_iLeftUp.y + m_bPreBox.m_nHeight < m_bMeaBox.m_iLeftUp.y) { return 0; } float f_WidthOverlap = abs(min(m_bPreBox.m_iLeftUp.x + m_bPreBox.m_nWidth, m_bMeaBox.m_iLeftUp.x + m_bMeaBox.m_nWidth) - max(m_bPreBox.m_iLeftUp.x, m_bMeaBox.m_iLeftUp.x)); float f_HeightOverlap = abs(min(m_bPreBox.m_iLeftUp.y + m_bPreBox.m_nHeight, m_bMeaBox.m_iLeftUp.y + m_bMeaBox.m_nHeight) - max(m_bPreBox.m_iLeftUp.y, m_bMeaBox.m_iLeftUp.y)); float f_OverlapArea = f_HeightOverlap * f_WidthOverlap; float f_PreboxArea = m_bPreBox.m_nHeight * m_bPreBox.m_nWidth; float f_MeaboxArea = m_bMeaBox.m_nHeight * m_bMeaBox.m_nWidth; return f_OverlapArea / (f_PreboxArea + f_MeaboxArea - f_OverlapArea); } //遍历当前帧Detection该类别的所有物体，对比静态特征，找出是否有相匹配的目标， //该m_ObjectXXXcmp是m_iDetector下的当前帧的vector<Object> //m_Objectcmp:相匹配的Object m_Targetcmp:相匹配的Target,m_ObjectSelpcmp:跟踪目标的Object void DynamicFeature::m_CompareOT(Object &m_Objectcmp, Target &m_Targetcmp, vector<Object> m_ObjectXXXcmp, vector<Target> m_TargetXXXcmp, Object m_ObjectSelfcmp) { vector<Object> m_ObjectTempCoin; m_ObjectTempCoin.clear(); int i, j; for (i = 0; i < m_ObjectXXXcmp.size(); i++)//遍历当前帧的所有该类别的Object，比较静态特征 { if (1 == m_CompareMap(m_ObjectXXXcmp[i].m_iStaticFeature.m_mapColor, m_ObjectSelfcmp.m_iStaticFeature.m_mapColor)) //比较静态特征,匹配成功,找出对应的Object { // m_StateTail = 2; //目标状态,跟踪异常 m_ObjectTempCoin.push_back(m_ObjectXXXcmp[i]); } } //多个静态特征相同 if (m_ObjectTempCoin.size() > 1) { m_StateTail = 2; //目标状态,跟踪异常 } //只有一个静态特征相同 else if (m_ObjectTempCoin.size() == 1) { m_StateTail = 2; //目标状态,跟踪异常 m_Objectcmp = m_ObjectTempCoin[0]; //存储对匹配成功的Object for (j = 0; j < m_TargetXXXcmp.size(); j++) //找出匹配成功的Object对应的Target { if (m_TargetXXXcmp[j].m_iSelfLink.dObjectID == m_Objectcmp.m_dID) { m_Targetcmp = m_TargetXXXcmp[j]; //存储对匹配成功的Target break; } } } else if (m_ObjectTempCoin.size() == 0) //匹配失败 { m_StateTail = 0; } } //初始化 void DynamicFeature::m_InitKalman(int m_nStateN, int m_mMeasureN) { // img.create(500, 500, CV_8UC3); m_KF.init(m_nStateN, m_mMeasureN, 0); //Kalman初始化 m_mState.create(m_nStateN, 1, CV_32FC1); //state(x,y,detaX,detaY) m_mProcessNoise.create(m_nStateN, 1, CV_32F); m_mMeasurement = Mat::zeros(m_mMeasureN, 1, CV_32F); //测量矩阵measurement(x, y,dx,dy) m_mTargetTrue = Mat::zeros(m_mMeasureN, 1, CV_32F); //临时存放Target randn(m_mState, Scalar::all(0), Scalar::all(0.1)); //随机生成一个矩阵，期望是0，标准差为0.1; m_KF.transitionMatrix = (Mat_<float>(4, 4) << //状态转移矩阵 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);//元素导入矩阵，按行; //setIdentity: 缩放的单位对角矩阵 setIdentity(m_KF.measurementMatrix); //观测模型 setIdentity(m_KF.processNoiseCov, Scalar::all(1e-5)); //wk 是过程噪声，并假定其符合均值为零，协方差矩阵为Qk(Q)的多元正态分布 setIdentity(m_KF.measurementNoiseCov, Scalar::all(1e-1)); //vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布 setIdentity(m_KF.errorCovPost, Scalar::all(1)); randn(m_KF.statePost, Scalar::all(0), Scalar::all(0.1)); //f = 1; code = (char)-1; } void DynamicFeature::m_PreKalman(Tracking &All_Tracking, int dID, int Category, Mat img, int n_NumFrame) { vector<Object> m_TmpObject; //临时存储跟踪目标的类别的全局Object m_TmpObject.clear(); // int m_LocarObject; // switch (Category) //提取该跟踪目标的类别的所有Object { case EMCategory_car: { for (vector<Object>::iterator begin = (*All_Tracking.m_vpiUnvCar).begin(); begin != (*All_Tracking.m_vpiUnvCar).end(); ++begin) { m_TmpObject.push_back(*begin); } break; } break; case EMCategory_bus: { for (vector<Object>::iterator begin = (*All_Tracking.m_vpiUnvBus).begin(); begin != (*All_Tracking.m_vpiUnvBus).end(); ++begin) { m_TmpObject.push_back(*begin); } break; } break; case EMCategory_truck: { for (vector<Object>::iterator begin = (*All_Tracking.m_vpiUnvTruck).begin(); begin != (*All_Tracking.m_vpiUnvTruck).end(); ++begin) { m_TmpObject.push_back(*begin); } break; } case EMCategory_pedestrian: { for (vector<Object>::iterator begin = (*All_Tracking.m_vpiUnvPedestrian).begin(); begin != (*All_Tracking.m_vpiUnvPedestrian).end(); ++begin) { m_TmpObject.push_back(*begin); } break; } case EMCategory_bicycle: { for (vector<Object>::iterator begin = (*All_Tracking.m_vpiUnvBicycle).begin(); begin != (*All_Tracking.m_vpiUnvBicycle).end(); ++begin) { m_TmpObject.push_back(*begin); } break; } case EMCategory_licensePlate: { for (vector<Object>::iterator begin = (*All_Tracking.m_vpiUnvLincessPlate).begin(); begin != (*All_Tracking.m_vpiUnvLincessPlate).end(); ++begin) { m_TmpObject.push_back(*begin); } break; } default: break; } //找出跟踪目标对应的Object,复制到m_ObjectSelf中 for (m_LocarObject = 0; m_LocarObject < m_TmpObject.size(); m_LocarObject++) { if (m_TmpObject[m_LocarObject].m_dID == dID) { m_ObjectSelf = m_TmpObject[m_LocarObject]; //模拟跟踪目标的矩形框长宽 m_bSelfbox.m_nHeight = 40; m_bSelfbox.m_nWidth = 50; break; } } /*************************** 1.预测 ***************************/ Point2f center(img.cols*0.5f, img.rows*0.5f); //center图像中心点 float R = img.cols / 3.f; //半径 double state