目标跟踪之--卡尔曼滤波（C++代码解析）

本文对卡尔曼滤波的C++工程代码进行解析，原理流程已在上篇博客中给出。
具体解析已在代码中给出。
1、KalmanTracker.h

#ifndef KALMAN_H
#define KALMAN_H 2

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

using namespace std;
using namespace cv;

#define StateType Rect<float>

//此类表示作为边界框观察的各个跟踪对象的内部状态。
class KalmanTracker
{
public:
	KalmanTracker()
	{
		init_kf(StateType(), int());
		m_time_since_update = 0;
		m_hits = 0;
		m_hit_streak = 0;
		m_age = 0;
		m_id = kf_count;
		kf_count++;
		classId;
	}
	KalmanTracker(StateType initRect, int)
	{
		init_kf(initRect, int());
		m_time_since_update = 0;
		m_hits = 0;
		m_hit_streak = 0;
		m_age = 0;
		m_id = kf_count;
		kf_count++;
		classId;
	}
  
  ~KalmanTracker()
	{
		m_history.clear();
	}

	StateType predict();
	void update(StateType stateMat, int ClassId);
	
	StateType get_state();
	StateType get_rect_xysr(float cx, float cy, float s, float r);

	static int kf_count;

	int m_time_since_update;
	int m_hits;
	int m_hit_streak;
	int m_age;
	int m_id;
	int classId;

private:
	void init_kf(StateType stateMat,int ClassId);

	cv::KalmanFilter kf;
	cv::Mat measurement;

	std::vector<StateType> m_history;
};

#endif

2、KalmanTracker.cpp

#include "KalmanTracker.h"

int KalmanTracker::kf_count = 0;

// 	初始化卡尔曼滤波器
void KalmanTracker::init_kf(StateType stateMat,int ClassId)
{
	int stateNum = 7;			//使用7个状态值，把IOU的框也给加进去了
	int measureNum = 4;
	kf = KalmanFilter(stateNum, measureNum, 0);

	classId = ClassId;

	measurement = Mat::zeros(measureNum, 1, CV_32F);

	kf.transitionMatrix = (Mat_<float>(stateNum, stateNum)<<			//状态转移矩阵
		1, 0, 0, 0, 1, 0, 0,
		0, 1, 0, 0, 0, 1, 0,
		0, 0, 1, 0, 0, 0, 1,
		0, 0, 0, 1, 0, 0, 0,
		0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 1);
    
   setIdentity(kf.measurementMatrix);									//观测矩阵
	setIdentity(kf.processNoiseCov, Scalar::all(1e-2));					//过程噪声协方差矩阵
	setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));				//测量噪声协方差矩阵
	setIdentity(kf.errorCovPost, Scalar::all(1));						//估计值和真实值之间的误差协方差矩阵
	
	//使用[cx,cy,s,r]格式的标定框来初始化状态向量
  kf.statePost.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
	kf.statePost.at<float>(1, 0) = stateMat.y + stateMat.height / 2;
	kf.statePost.at<float>(2, 0) = stateMat.area();
	kf.statePost.at<float>(3, 0) = stateMat.width / stateMat.height;
}

//预测估计的标定框
StateType KalmanTracker::predict()
{
	// predict
	Mat p = kf.predict();
	m_age += 1;

	if (m_time_since_update > 0)
		m_hit_streak = 0;
	m_time_since_update += 1;

	StateType predictBox = get_rect_xysr(p.at<float>(0, 0), p.at<float>(1, 0), p.at<float>(2, 0), p.at<float>(3, 0));

	m_history.push_back(predictBox);
	return m_history.back();
}

//使用观测的标定框来更新状态向量
void KalmanTracker::update(StateType stateMat, int ClassId)
{
	m_time_since_update = 0;
	m_history.clear();
	m_hits += 1;
	m_hit_streak += 1;
	classId = ClassId;
	// measurement
	measurement.at<float>(0, 0) = stateMat.x + stateMat.width / 2;
	measurement.at<float>(1, 0) = stateMat.y + stateMat.heigh / 2;
	measurement.at<float>(2, 0) = stateMat.area();
	measurement.at<float>(3, 0) = stateMat.width /  stateMat.height;

	// update
	kf.correct(measurement);
}

//返回当前状态向量
StateType KalmanTracker::get_state()
{
	Mat s = kf.statePost;
	return get_rect_xysr(s.at<float>(0, 0), s.at<float>(1, 0), s.at<float>(2, 0), s.at<float>(3, 0));
}

//把标定框从[cx,cy,s,r]格式转成[x,y,w,h]
StateType KalmanTracker::get_rect_xysr(float cx, float cy, float s, float r)
{
	float w = sqrt(s * r);
	float h = s / w;
	float x = (cx - w / 2);
	float y = (cy - h / 2);

	if (x < 0 && cx > 0)
		x = 0;
	if (y < 0 && cy > 0)
		y = 0;

	return StateType(x, y, w, h);
}

3、Tracker.h

#pragma once

#include "Hungarian.h"
#include "KalmanTracker.h"

#include <set>

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

//using namespace std;

typedef struct TrackingBox					//定义一个结构体，来存储帧序号，车id，车框，其中车框是rect类型的，默认车id为-1
{
	int frame;
	long long id = -1;
	int classId = -1;
	Rect_<float> box;
}TrackingBox;

// Computes IOU between two boundboxes   计算两个标定框之间的IOU值
double GetIOU(Rect_<float> bb_test, Rect_<float> bb_gt);

class Tracker
{
public:
	void Initial(vector<KalmanTracker> &trackers, vector<TrackingBox> &tempVec);

	void predict(vector<KalmanTracker>& trackers, vector<Rect_<float>>& predictedBoxes);

	void getMatchedPair(vector<KalmanTracker>& trackers, vector<TrackingBox>& tempVec, vector<Rect_<float>>& predictedBoxes);
  
  	void getResult(vector<cv::Point>& matchedPairs, vector<KalmanTracker>& trackers, vector<TrackingBox>& tempVec);
	
	void Tracking(vector<TrackingBox>& tempVec);
	
	vector<cv::Point> MatchedPairResult();

	set<int> unmatchedDetectionsResult();

	vector<TrackingBox> getTrackedObject();
	
	vector<KalmanTracker> getBackTrackers();

private:

	vector<KalmanTracker> trackers;

	int max_age = 20;
	int min_hits = 3;
	double iouThreshold = 0.3;

	vector<Rect_<float>> predictedBoxes;

	vector<vector<double>> iouMatrix;
	vector<int> assignment;
	set<int> unmatchedDetections;
	set<int> unmatchedTrajectories;
	set<int> allItems;
	set<int> matchedItems;
	vector<cv::Point> matchedPairs;
  	vector<TrackingBox> frameTrackingResult;
	vector<KalmanTracker> trackersResult;
	unsigned int trkNum = 0;
	unsigned int detNum = 0;

	int total_frames = 0;
	double total_time = 0.0;
};

4、Tracker.cpp

#include "Tracker.h"

double GetIOU(Rect_<float> bb_test, Rect_<float> bb_gt)		//计算两个框的交并比
{
	float in = (bb_test & bb_gt).area();
	float un = bb_test.area() + bb_gt.area() - in;

	if (un < DBL_EPSILON)
		return 0;
   
   return (double)(in / un);
}

void Tracker::Initial(vector<KalmanTracker> &trackers, vector<TrackingBox> &tempVec)
{
  for (unsigned int i = 0; i < tempVec.size(); i++)                         //第几帧，该帧有多少行
  {
    KalmanTracker trk = KalmanTracker(tempVec[i].box, tempVec[i].classid);  //该帧的第i行，其实这里就是用第1帧的所有行进行初始化
    trackers.push_back(trk);                                                //初始化后的结果放进一个vector中
  }
}

void Tracker::predict(vector<KalmanTracker>& trackers, vector<Rect_<float>>& predictedBoxes)
{
	//3.1. get predicted locations from existing trackers.		通过现有的追踪器来得到预测位置
	predictedBoxes.clear();
  for (auto it = trackers.begin(); it != trackers.end();)
	{
		Rect_<float> pBox = (*it).predict();
    if (pBox.x >= 0 && pBox.y >= 0)
		{
			predictedBoxes.push_back(pBox);
			it++;
		}
		else
		{
			it = trackers.erase(it);
		}
	}
}

void Tracker::getMatchedPair(vector<KalmanTracker>& trackers, vector<TrackingBox>& tempVec, vector<Rect_<float>& predictedBoxes)
{
  ///
	// 3.2. associate detections to tracked object (both represented as bounding boxes)		将检测关联到追踪对象（均表现为标定框）
	// dets : detFrameData[fi]
	trkNum = predictedBoxes.size();		//预测框的尺寸，即有多少个框
	detNum = tempVec.size();			//一帧有多少行

	iouMatrix.clear();
  iouMatrix.resize(trkNum, vector<double>(detNum, 0));

	for (unsigned int i = 0; i < trkNum; i++) // compute iou matrix as a distance matrix		计算iou矩阵作为距离矩阵
	{
		for (unsigned int j = 0; j < detNum; j++)
		{
			// use 1-iou because the hungarian algorithm computes a minimum-cost assignment.	使用1-iou因为匈牙利算法计算最小成本分配
			iouMatrix[i][j] = 1 - GetIOU(predictedBoxes[i], tempVec[j].box);					//每一个预测框和该帧中所有行计算
		}
	}

	// solve the assignment problem using hungarian algorithm.		使用匈牙利算法来解决分配问题
  // the resulting assignment is [track(prediction) : detection], with len=preNum		分配结果是[track(prediction) : detection]，长度等于preNum
	HungarianAlgorithm HungAlgo;
	assignment.clear();
  HungAlgo.Solve(iouMatrix, assignment);
  
  unmatchedTrajectories.clear();
	unmatchedDetections.clear();
	allItems.clear();
	matchedItems.clear();

	if (detNum > trkNum) //	there are unmatched detections		如果一帧的行数大于预测框的数量，即未匹配的检测
	{
    for (unsigned int n = 0; n < detNum; n++)
			allItems.insert(n);

		for (unsigned int i = 0; i < trkNum; ++i)
			matchedItems.insert(assignment[i]);

		set_difference(allItems.begin(), allItems.end(),
			matchedItems.begin(), matchedItems.end(),
			insert_iterator<set<int>>(unmatchedDetections, unmatchedDetections.begin()));
	}
	else if (detNum < trkNum) // there are unmatched trajectory/predictions		一帧的行数小于预测框的数量，即有未匹配的追踪/预测
	{
		for (unsigned int i = 0; i < trkNum; ++i)
			if (assignment[i] == -1) // unassigned label will be set as -1 in the assignment algorithm		未分配的标签将在分配算法中设置为-1
				unmatchedTrajectories.insert(i);
	}

	// filter out matched with low IOU		过滤掉低IOU的匹配
  matchedPairs.clear();
	for (unsigned int i = 0; i < trkNum; ++i)
	{
		if (assignment[i] == -1) //pass over invalid values
			continue;
		if (1 - iouMatrix[i][assignment[i]] < iouThreshold)
		{
      unmatchedTrajectories.insert(i);
			unmatchedDetections.insert(assignment[i]);
		}
		else
		{
			matchedPairs.push_back(cv::Point(i, assignment[i]));
		}
	}
}

void Tracker::getResult(vector<cv::Point>& matchedPairs, vector<KalmanTracker>& trackers, vector<TrackingBox>& tempVec)
{
	///
	// 3.3. updating trackers

	// update matched trackers with assigned detections.
	// each prediction is corresponding to a tracker
	int detIdx, trkIdx;
	for (unsigned int i = 0; i < matchedPairs.size(); i++)
	{
		trkIdx = matchedPairs[i].x;
		detIdx = matchedPairs[i].y;
		trackers[trkIdx].update(tempVec[detIdx].box, tempVec[detIdx].classId);
	}

	// create and initialise new trackers for unmatched detections
	for (auto umd : unmatchedDetections)
	{
		KalmanTracker tracker = KalmanTracker(tempVec[umd].box, tempVec[umd].classId);
		trackers.push_back(tracker);
	}

	// get trackers' output
	frameTrackingResult.clear();
	for (auto it = trackers.begin(); it != trackers.end();)
	{
		if (((*it).m_time_since_update < 1) &&
			((*it).m_hit_streak >= min_hits))
		{
			TrackingBox res;
			res.box = (*it).get_state();
			res.id = (*it).m_id + 1;
			res.classId = (*it).classId;
			//res.frame = frame_count;
			frameTrackingResult.push_back(res);
			it++;
		}
		else
		{
			it++;
		}
			
		// remove dead tracklet
		if (it != trackers.end() && (*it).m_time_since_update > max_age)
		{
			it = trackers.erase(it);
		}			
	}

}


set<int> Tracker::unmatchedDetectionsResult()
{
	return unmatchedDetections;
}

vector<cv::Point> Tracker::MatchedPairResult()
{
	return matchedPairs;
}

vector<TrackingBox> Tracker::getTrackedObject()
{
	return frameTrackingResult;
}

vector<KalmanTracker> Tracker::getBackTrackers()
{
	return trackersResult;
}


void Tracker::Tracking(vector<TrackingBox>& tempVec)
{
	if (trackers.size() == 0) // the first frame met	第一帧
	{
		Initial(trackers, tempVec);
		
		//continue;
	}
	else
	{
		//得到预测框
		predict(trackers, predictedBoxes);
		//得到匹配的和未匹配的
		getMatchedPair(trackers, tempVec, predictedBoxes);
    matchedPairs = MatchedPairResult();
		unmatchedDetections = unmatchedDetectionsResult();
		得到追踪结果
		getResult(matchedPairs, trackers, tempVec);
	}
	
}

5、mainDemo.cpp

#include <iostream>
#include <fstream>
#include <iomanip> // to format image names using setw() and setfill()
#include <io.h>    // to check file existence using POSIX function access(). On Linux include <unistd.h>.
#include <set>

#include "KalmanTracker.h"
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#include "readtxt.h"
#include "Tracker.h"

// global variables for counting	用来计数的全局变量
#define CNUM 20

int main()
{
  //为了显示，随机初始化一些颜色
	RNG rng(0xFFFFFFFF);
	Scalar_<int> randColor[CNUM];
	for (int i = 0; i < CNUM; i++)
		rng.fill(randColor[i], RNG::UNIFORM, 0, 256);

	 3. update across frames		通过帧来更新

	// variables used in the for-loop		for循环中的变量
  vector<Rect_<float>> predictedBoxes;
	set<int> unmatchedDetections;

	unsigned int trkNum = 0;
	unsigned int detNum = 0;

	double cycle_time = 0.0;
	int64 start_time = 0;
  
 
 std::string sequence = "F:/MOT/tail/tracker";    //读取txt文件，每个文件包含一帧所有的检测框
 std::string video_path = "F:/MOT/tail/dj.mp4";   //读取视频
 
 ///读取文件夹中的boundingbox信息
	std::vector<cv::String> txt_files;
	//std::vector<cv::String> txt_files;

  std::vector<std::vector<cv::Rect_<float> > > groundtruth_rect;		//用来存储所有框的信息，最外围是第几帧，内围是每一帧有多少行
	std::vector<std::vector<cv::Point3f>> carTypes;						//用来存储车辆类型及中心点，维度同上
	vector<TrackingBox> TrackingResult;									//最终的追踪结果
	vector<cv::Point> matchedPairs;

	cv::glob(sequence, txt_files);
  
  	for (size_t i = 0; i < txt_files.size(); ++i)
	{
		//std::cout << txt_files[i] << std::endl;					//检测是否读取全部txt文件，打印文件名
		groundtruth_rect.push_back(getgroundtruth(txt_files[i]));		//读取所有文件中的全部框信息
    carTypes.push_back(getCarType(txt_files[i]));					//读取每个框的车辆类型信息并计算中心点
	}

	//将读取的值存进TrackingBox的类当中，但是现在是所有帧，需要改写为一次给一帧数据
	vector<vector<TrackingBox>> detFrameData;
	vector<TrackingBox> tempVec;
  for (size_t i = 0; i < groundtruth_rect.size(); i++)
	{//第i帧
    TrackingBox tb;
		for (size_t j = 0; j < groundtruth_rect[i].size(); j++)
		{//第i帧的第j行
			tb.classId = carTypes[i][j].x;
			tb.box = Rect_<float>(Point_<float>(groundtruth_rect[i][j].x, groundtruth_rect[i][j].y), Point_<float>(groundtruth_rect[i][j].width, groundtruth_rect[i][j].height));
      tempVec.push_back(tb);		//把相同帧序号的tb放进一个数组里
		}
		detFrameData.push_back(tempVec);
		tempVec.clear();
   }
   
   //文件在上面已经读取完了，不需要再读取文件了，直接对已有的数组的数组里面的数据进行处理
   
   ///下面开始读取视频
	cv::VideoCapture cap(video_path);
	//if (!cap.isOpened())return -1;
	cv::Mat frame, outFrame;

	Tracker Tt;

	for (int fi = 0; fi < detFrameData.size(); fi++)
  {
    Tt.Tracking(detFrameData[fi]);
		TrackingResult = Tt.getTrackedObject();
    //
		//追踪结束，下面是绘图显示
    cap >> frame;
		if (frame.empty())
			break;
    cv::resize(frame, outFrame, cv::Size(416, 416), (0, 0), (0, 0), cv::INTER_LINEAR);
    
    for (auto tb : TrackingResult)
		{
      std::stringstream st;
			st << tb.id;
			std::string str = st.str();
			cv::rectangle(outFrame, cv::Rect(tb.box.x, tb.box.y, tb.box.width, tb.box.height), randColor[tb.id % 20], 1, 1, 0);
			cv::putText(outFrame, str, cv::Point(tb.box.x, tb.box.y + tb.box.height), cv::FONT_HERSHEY_PLAIN, 2.0, randColor[tb.id % 20], 2);
			st.clear();
			str.clear();
			std::stringstream st2;
			st2 << tb.classId;
			std::string str2 = st2.str();
			cv::putText(outFrame, str2, cv::Point(tb.box.x, tb.box.y), cv::FONT_HERSHEY_PLAIN, 2.0, randColor[tb.id % 20], 2);
			st2.clear();
			str2.clear();
    }
    
    imshow("Video", outFrame);
		//TrackingResult.clear();
		//等待50ms，如果从键盘输入的是q、Q、或者是Esc键，则退出
		int key = cv::waitKey(10);
		if (key == 'q' || key == 'Q' || key == 27)
			break;
    
  }
  
  return 0;
  
}

其中，在mainDemo.cpp中，有输入视频，每帧的gt数据，此处分享一个根据连续图片合成视频的python代码，合成视频的快慢，可以根据代码中的帧数来设置。
只需在anaconda下安装moviepy安装包即可：
pip install moviepy

from moviepy.editor import ImageSequenceClip

image_path = "./sss/"   # 图片目录
fps = 30   # 此处设置合成视频的播放速度
clip = ImageSequenceClip(image_path, fps=fps)

clip.write_videofile("output.mp4", fps=fps)