水上船舶智能驾驶系统项目

（我的部分：图像滤波，分割，跟踪提取障碍物，维护障碍物队列）

      由于三维雷达点云的处理耗时太长，为了保证实时性，折中之法：将三维激光雷达扫描到的三维点投影到二维平面（去掉Z坐标），生成二维栅格图像，再采用opencv针对图像的处理方法分割提取障碍物，并采用KCF跟踪算法跟踪运动的障碍物：

//KCF跟踪提取障碍物队列
void Preprocess::Tracking_KCF_For_Obstacle(cv::Mat image, vector &SuspectObstacleQueue, vector &ActualObstacleQueue, vector &ROIs)
{
	cv::Mat frame;
	cv::Mat imageForRect;
	vector rects;
	frame = Preprocessing03(image, rects, imageForRect);//frame是8U
	cv::Mat framefortracking = frame.clone();//framefortracking是8U
	if (rects.size() < 1)
	{
		cout << "rects is empty" << endl;
		return;
	}
	else frameCount++;
	cout << "rects.size():" << rects.size() << endl;
	cout << "frameCount=" << frameCount << endl;
	if (SuspectObstacleQueue.empty())//怀疑障碍物为空或者为头帧
	{
		RectsToSuspectObstacleQueue(SuspectObstacleQueue, rects, frame);
		cout << "SuspectObstacleQueue.size:" << SuspectObstacleQueue.size() << endl;

	}
	if (!preFrame.empty())
	{
		//判断当前帧中是否出现了新的rect,并将新的rect加入怀疑障碍物队列
		AddNewRectToSuspectObstacleQueue(preFrame, frame, rects, SuspectObstacleQueue);

	}
	if (frameCount % 10 == 1)//每15帧更新跟踪器 
	{
		ROIs.clear();
		MultiTrackers.clear();

		SuspectObstacleQueue.clear();
		ActualObstacleQueue.clear();
		RectsToSuspectObstacleQueue(SuspectObstacleQueue, rects, frame);

		GetROIsFromSuspectObstacleQueue(SuspectObstacleQueue, ROIs);//SuspectObstacle中的ROIs还是上一帧处理过后的状态
		cout << "ROIs.size:" << ROIs.size() << endl;
		if (ROIs.size() < 1)
		{
			cout << " ROIs is empty" << endl;
			return;
		}
		for (auto i = 0; i < ROIs.size(); i++)
		{
			MultiTrackers.push_back(cv::TrackerKCF::create());
			MultiTrackers[i]->init(framefortracking, ROIs[i]);
		}
		cout << "每15帧更新跟踪器 " << endl;
	}
	else
	{
		//开始跟踪
		cout << "start tracking" << endl;
		vector getObjects;
		for (int i = 0; i < MultiTrackers.size(); i++)
		{
			MultiTrackers[i]->update(framefortracking, ROIs[i]);
			getObjects.push_back(ROIs[i]);
		}
		cout << "getObjects.size():" << getObjects.size() << endl;
		for (auto j = 0; j < getObjects.size(); j++)
		{
			// draw the tracked object
			cv::Rect2d roi = getObjects[j];
			bool isVanish = IsVanishInRoi(frame, roi, SuspectObstacleQueue,j);
			if (!isVanish)
			{
				//框出跟踪结果
				cout << "框出跟踪结果" << endl;
				cv::rectangle(imageForRect, getObjects[j], cv::Scalar(255), 2, 1);//imageForRect
			}
			//根据跟踪结果更新两个队列，若怀疑队列有移除一个元素，则返回元素下标
			int move = UpdateTwoQueues(j, SuspectObstacleQueue, ActualObstacleQueue, roi, isVanish);//SuspectObstacleQueue的顺序始终与ROIs顺序一一对应
			if (move != -1)
			{
				getObjects.erase(getObjects.begin() + move);
				j = j - 1;
				//如果怀疑队列移除了一个元素，则更新跟踪器：
				ROIs.clear();
				MultiTrackers.clear();
				if (SuspectObstacleQueue.empty())//怀疑障碍物为空或者为头帧
				{
					RectsToSuspectObstacleQueue(SuspectObstacleQueue, rects, frame);
					cout << "SuspectObstacleQueue.size:" << SuspectObstacleQueue.size() << endl;

				}
				GetROIsFromSuspectObstacleQueue(SuspectObstacleQueue, ROIs);//SuspectObstacle中的ROIs还是上一帧处理过后的状态
				cout << "ROIs.size:" << ROIs.size() << endl;
				if (ROIs.size() < 1)
				{
					cout << " ROIs is empty" << endl;
					return;
				}
				for (auto i = 0; i < ROIs.size(); i++)
				{
					MultiTrackers.push_back(cv::TrackerKCF::create());
					MultiTrackers[i]->init(framefortracking, ROIs[i]);
				}
			}
			for (int i = 0; i < SuspectObstacleQueue.size(); i++)
			{
				cout << "SuspectObstacleQueue[" << i << "].appear_counts=" << SuspectObstacleQueue[i].appear_counts << "  ";
				cout << "SuspectObstacleQueue[" << i << "].centre=(" << SuspectObstacleQueue[i].centre.x << " " << SuspectObstacleQueue[i].centre.y << ")  ";
				cout << "SuspectObstacleQueue[" << i << "].vel= " << SuspectObstacleQueue[i].vel.value << "m/s, " << SuspectObstacleQueue[i].vel.theta << endl;
			}
		}
		cv::imshow("tracking", imageForRect);

		cv::waitKey(30);
	}
	preFrame = frame.clone();//记录下前一帧
	return;
}