robomaster比赛之卡尔曼滤波篇--之一

由于临近大四，面临考研与工作的人生选择，所以较长一段时间处于准备考研和投递简历的状态中，很少有时间对自己在robomaster比赛中学到的知识做总结。我会尽量在空闲时间完成卡尔曼篇的写作，涉及现实需要到公式推导到代码实现，再到工程应用。

1. 首先第一点是现实需要。在robomaster的赛场中机器人处于快速移动状态，简单的图像处理提取特征在静态情况下的效果可能较好，可一旦涉及运动问题自瞄系统如果没有考虑的运动预测那实际效果就会很不理想。我们就是从这个问题开始接触卡尔曼滤波，但遗憾的是由于当时的个人能力水平，对于卡尔曼滤波只能照猫画虎的根据他人的代码来写，但是最后发现使用卡尔曼滤波我们并没有达到很好的预测效果，换言之，我们对卡尔曼滤波的使用只实现了对于目标的跟踪，而且具有相当的滞后性。
2. 由于在和电控对接时我们发现，视觉识别到的装甲板图像会出现在连续帧中缺失的情况，造成视觉输出的装甲板信息传输到电控形成的是阶跃信号，这对于电控的控制部分会存在一定影响，我们也希望可以解决这个问题。通过卡尔曼滤波跟踪我们也进行了一定的实验，卡尔曼跟踪很好的平滑了数据，但是由于数据有着严重的滞后性，当时的研究和使用便到此为止。
3. 接下来会介绍一些Opencv自带的跟踪算法，与卡尔曼滤波跟踪。
4. 跟踪算法的python实现，可以通过视频来测试不同算法的跟踪效果，我们的结论是在综合考虑准确性与速度的情况下kcf跟踪表现效果最佳。

import sys
import cv2
import click
#@click.command()
#@click.option('--video', help = 'input video')
#@click.option('--algorithm', help = 'tracker algorithm, BOOSTING、MIL、KCF、TLD、MEDIANFLOW、GOTURN、CSRT、MOSSE')
def main(video, algorithm):
    
    #major_ver, minor_ver, subminor_ver = (cv2.__version__).split('.')
    # 根据opencv的不同版本，创建跟踪器
    #if int(minor_ver) < 3:
    #tracker = cv2.Tracker_create(algorithm)
    #else:
    if algorithm == 'BOOSTING':
        tracker = cv2.TrackerBoosting_create()
    if algorithm == 'MIL':
        tracker = cv2.TrackerMIL_create()
    if algorithm == 'KCF':
        tracker = cv2.TrackerKCF_create()
    if algorithm == 'TLD':
        tracker = cv2.TrackerTLD_create()
    if algorithm == 'MEDIANFLOW':
        tracker = cv2.TrackerMedianFlow_create()
    if algorithm == 'GOTURN':
        tracker = cv2.TrackerGOTURN_create()
    if algorithm == "CSRT":
        tracker = cv2.TrackerCSRT_create()
    if algorithm == 'MOSSE':
        tracker = cv2.TrackerMOSSE_create()

	# 读取视频文件
    video_cap = cv2.VideoCapture(video)

	# 检查视频文件是否被正确打开
    if not video_cap.isOpened():
        
        print("Open video failed.")
        sys.exit()

	# 读取第一帧数据
    
    ok, frame = video_cap.read()
    
    if not ok:
        print('Read video file failed.')
        sys.exit()

    	# 手动选择关注的区域
    bbox = cv2.selectROI(frame, False)
    ok= tracker.init(frame, bbox)
    while True:
            
	    # 读取下一帧数据
        ok,frame = video_cap.read()
            
        if not ok:
            break

    	    # 开始计时器
        timer = cv2.getTickCount()

    	    # 更新跟踪器
        ok,bbox = tracker.update(frame)

    	    # 计算fps
        fps= cv2.getTickFrequency() / (cv2.getTickCount() - timer)

	    # 画出 bounding box
        if ok:
            p1 = (int(bbox[0]), int(bbox[1]))
            p2=(int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3]))
            cv2.rectangle(frame, p1, p2, (255, 0, 0), 2, 1)
        else:
		# Tracking failure
                cv2.putText(frame, "Tracking failure detected", (100, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)

		# 显示
        cv2.putText(frame, algorithm + " Tracker", (100, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
        cv2.putText(frame, "FPS : " + str(int(fps)), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
        cv2.imshow("Tracking", frame)
		
		# 接收到q键，退出循环
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
if __name__ == '__main__':
 #测试视频需和本代码放置同一文件夹下。跟踪算法可以替换
	main("test.mp4",'KCF')
	

5. 卡尔曼摄像头跟踪，该代码可以在配置了opencv的c++环境下编译运行，本人使用环境为vs2019、opencv3.4.1

#include "opencv2/opencv.hpp"
#include <Windows.h>
using namespace std;
using namespace cv;

TermCriteria criteria(TermCriteria::MAX_ITER
                      + TermCriteria::EPS,
                      20,
                      1.0);

bool selecting = false;
Rect selection;
Point spo; // selection point origin

void onMouse(int event, int x, int y, int, void*)
{
    switch(event)
    {
    case EVENT_LBUTTONDOWN:
    {
        spo.x = x;
        spo.y = y;
        selection.x = spo.x;
        selection.y = spo.y;
        selection.width = 0;
        selection.height = 0;
        selecting = true;

    } break;
    case EVENT_LBUTTONUP:
    {
        selecting = false;
    } break;
    default:
    {
        selection.x = min(x, spo.x);
        selection.y = min(y, spo.y);
        selection.width = abs(x - spo.x);
        selection.height = abs(y - spo.y);
    } break;
    }
}

int main()
{
    VideoCapture cam(0);
    if(!cam.isOpened())
        return -1;

    KalmanFilter kalman(4,2);

    Mat_<float> tm(4, 4); // transition matrix
    tm << 1,0,1,0, // next x = 1x+0y+1x'+0y'
            0,1,0,1, // next x'= 0x+1y+0x'+1y'
            0,0,1,0, // next y = 0x+0y+1x'+0y
            0,0,0,1; // next y'= 0x+0y+0x'+1y'
    kalman.transitionMatrix =  tm;
    Mat_<float> pos(2,1);
    pos.at<float>(0) = 0;
    pos.at<float>(1) = 0;

    kalman.statePre.at<float>(0) = 0; // init x
    kalman.statePre.at<float>(1) = 0; // init y
    kalman.statePre.at<float>(2) = 0; // init x'
    kalman.statePre.at<float>(3) = 0; // init y'

    setIdentity(kalman.measurementMatrix);

    setIdentity(kalman.processNoiseCov,
                Scalar::all(0.00001));

    Rect srchWnd;

    string outputWindow = "Display";
    namedWindow(outputWindow);
    cv::setMouseCallback(outputWindow, onMouse);

    Mat histogram, backProject, mask;

    int key = -1;
    while(key != ' ')
    {
        Mat frame;
        cam >> frame;
        if(frame.empty())
            break;

        Mat frmHsv, hue;
        vector<Mat> hsvChannels;
        cvtColor(frame, frmHsv, COLOR_BGR2HSV);
        split(frmHsv, hsvChannels);
        hue = hsvChannels[0];

        int bins = 120;
        int nimages = 1;
        int channels[] = {0};
        float rangeHue[] = {0, 180};
        const float* ranges[] = {rangeHue};
        int histSize[] = { bins };
        bool uniform = true;

        if(selecting && selection.area() > 0)
        {
            Mat sel(frame, selection);

            int lbHue = 00 , hbHue = 180;
            int lbSat = 30 , hbSat = 256;
            int lbVal = 30 , hbVal = 230;

            inRange(frmHsv,
                    Scalar(lbHue,lbSat,lbVal),
                    Scalar(hbHue, hbSat, hbVal),
                    mask);

            Mat roi(hue, selection);
            Mat maskroi(mask, selection);

            calcHist(&roi,
                     nimages,
                     channels,
                     maskroi,
                     histogram,
                     1,
                     histSize,
                     ranges,
                     uniform);

            normalize(histogram,
                      histogram, 0, 255, NORM_MINMAX);

            bitwise_not(sel, sel);

            srchWnd = selection;
        }
        else if(!histogram.empty())
        {

            double scale = 1.0;
            calcBackProject(&hue,
                            nimages,
                            channels,
                            histogram,
                            backProject,
                            ranges,
                            scale,
                            uniform);

            erode(backProject,
                  backProject,
                  Mat());


            //            meanShift(backProject,
            //                      srchWnd,
            //                      criteria);

            CamShift(backProject,
                     srchWnd,
                     criteria);

            Point objectPos(srchWnd.x + srchWnd.width/2,
                            srchWnd.y + srchWnd.height/2);

            pos(0) = objectPos.x;
            pos(1) = objectPos.y;

            Mat estimation = kalman.correct(pos);
            kalman.predict();

            Point estPt(estimation.at<float>(0),
                        estimation.at<float>(1));

            drawMarker(frame,
                       estPt,
                       Scalar(0,255,0),
                       MARKER_CROSS,
                       30,
                       2);

            cvtColor(backProject, backProject, COLOR_GRAY2BGR);
            drawMarker(backProject,
                       estPt,
                       Scalar(0,255,0),
                       MARKER_CROSS,
                       30,
                       2);

        }

        switch(key)
        {
        case 'b':
            if(!backProject.empty()) imshow(outputWindow, backProject);
            else imshow(outputWindow, frame);
            break;
        case 'v': default: imshow(outputWindow, frame);
            break;

        }

        int k = waitKey(10);
        if(k > 0)
            key = k;
    }

    cam.release();

    return 0;

}

也可以通过cmake编译运行，cmakelists文件如下

cmake_minimum_required(VERSION 3.10)
project(CvTrackKalman)
set(OpenCV_DIR $ENV{OPENCV_PATH})
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(${PROJECT_NAME} "main.cpp")
target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS})