【目标跟踪】光流跟踪（python、c++代码）

读书猿

发布于 2024-02-05 07:25:24

96105

代码可运行

文章被收录于专栏：无人驾驶感知无人驾驶感知

运行总次数：5

代码可运行

前言

光流利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息的一种方法。
本文主要展示代码以及代码解释，对于相对应的原理，以后有机会再写（下次一定）。
本文所用数据源于网上开源数据。找不到数据的小伙伴可以私我拿数据。
文章提供 python、c++ 代码。python 代码可以直接跑通。c++ 代码集成一个 class ，可以在自己工程中使用。
效果图：

一、代码流程与思路

输入：上一帧图片、preImage 上一帧图片检测框、image 当前帧图片。输出：当前帧光流预测框
特征点提取。对上一帧图片 preImage 提取目标框里的特征点，这里采取的是 fast 角点检测。
preImage、image 光流跟踪、在 image 中找出对应的特征点。
由特征点对应关系可以得出当前帧的目标框。

二、python 代码

2.1 代码详解

(1) fast 角点检测

fast = cv2.FastFeatureDetector_create(threshold=9, nonmaxSuppression=True, type=cv2.FastFeatureDetector_TYPE_9_16)

threshold：边缘轨迹点和中心点的差值阈值。
nonmaxSuppression：是否进行非极大值抑制
type：提供轨迹范围。我们这里是从圆周轨迹16个点，当9个满足条件，此判定圆心像素点为特征点

我们这里只对检测框里的像素做特征点检测

def SelectPointByBox(img, det):
    top_x, top_y, bottom_x, bottom_y = [int(_) for _ in det[:4]]
    cutimg = img[max(0, top_y - 2):min(bottom_y + 2, 1080), max(0, top_x - 2):min(1920, bottom_x + 2)]
    fast = cv2.FastFeatureDetector_create(threshold=9, nonmaxSuppression=True, type=cv2.FastFeatureDetector_TYPE_9_16)
    kps = fast.detect(cutimg, 10)  # Ip-t < Ip < Ip+t
    kp = []
    for p in kps:
        t = []
        t.append(np.float32(p.pt[0] + top_x))
        t.append(np.float32(p.pt[1] + top_y))
        kp.append(np.array(t).reshape(1, 2))
    return np.array(kp)

(2) 追踪稀疏特征点

cv2.calcOpticalFlowPyrLK(preImgGray, gray, prePt, pt, **lkParms)

preImgGray：前一帧图片灰度图。
gray：当前帧图片灰度图
prePt：前一帧图片的特征点
pt：None

lkParms = dict(winSize=(15, 15), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

winSize: 每个金字塔级别上搜索窗口的大小
maxLevel: 最大金字塔层数
criteria：指定迭代搜索算法的终止条件，在指定的最大迭代次数 10 之后或搜索窗口移动小于 0.03

def OpticalFlowLk(preImg, curImg, prePt, pt):
    lkParms = dict(winSize=(15, 15), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
    gray = cv2.cvtColor(curImg, cv2.COLOR_BGR2GRAY)
    preImgGray = cv2.cvtColor(preImg, cv2.COLOR_BGR2GRAY)
    # nextPts:前一帧图像的特征点跟踪后的点  st：特征点是否找到，找到状态为1，否则为0  err：每个特征点的误差，即前一帧和当前帧中特征点的位置差异
    nextPts, st, err = cv2.calcOpticalFlowPyrLK(preImgGray, gray, prePt, pt, **lkParms)
    # print("p1", nextPts, "st", st, "err", err)
    goodNewPt = nextPts[st == 1]  # 光流跟踪后特征点
    goodOldPt = prePt[st == 1]  # 上一帧特征点
    return goodOldPt, goodNewPt

(3) 预测当前帧目标检测框

现在我们获取到了 prePt curPt pre_detect_box
由像素对应关系，我们可以求出 cur_detect_box

def CalculateShift(prePt, curPt):
    x = curPt[:, 0] - prePt[:, 0]
    y = curPt[:, 1] - prePt[:, 1]
    avgX = np.mean(x)
    avgY = np.mean(y)
    return avgX, avgY


def get_box(ditection, prePt, curPt):
    d_x, d_y = CalculateShift(prePt, curPt)  # 计算偏移量
    box = [0] * 4
    box[0], box[2], box[1], box[3] = ditection[0] + d_x, ditection[2] + d_x, ditection[1] + d_y, ditection[3] + d_y
    return box

2.2 完整代码

代码可直接跑通

import cv2
import os
import numpy as np


def GetImg(path, num):
    fn = os.path.join(path, 'img', '%06d.jpg' % (num))
    im = cv2.imread(fn)
    return im


def GetDetFrameRes(seq_dets, frame):
    detects = seq_dets[seq_dets[:, 0] == frame, 2:7]
    detects[:, 2:4] += detects[:, 0:2]  # convert to [x1,y1,w,h] to [x1,y1,x2,y2]
    return detects


def SelectPointByBox(img, det):
    top_x, top_y, bottom_x, bottom_y = [int(_) for _ in det[:4]]
    cutimg = img[max(0, top_y - 2):min(bottom_y + 2, 1080), max(0, top_x - 2):min(1920, bottom_x + 2)]
    fast = cv2.FastFeatureDetector_create(threshold=9, nonmaxSuppression=True, type=cv2.FastFeatureDetector_TYPE_9_16)
    kps = fast.detect(cutimg, 10)  # Ip-t < Ip < Ip+t
    kp = []
    for p in kps:
        t = []
        t.append(np.float32(p.pt[0] + top_x))
        t.append(np.float32(p.pt[1] + top_y))
        kp.append(np.array(t).reshape(1, 2))
    return np.array(kp)


def OpticalFlowLk(preImg, curImg, prePt, pt):
    lkParms = dict(winSize=(15, 15), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
    gray = cv2.cvtColor(curImg, cv2.COLOR_BGR2GRAY)
    preImgGray = cv2.cvtColor(preImg, cv2.COLOR_BGR2GRAY)
    # nextPts:前一帧图像的特征点跟踪后的点    st：特征点是否找到，找到状态为1，否则为0     err：每个特征点的误差，即前一帧和当前帧中特征点的位置差异
    nextPts, st, err = cv2.calcOpticalFlowPyrLK(preImgGray, gray, prePt, pt, **lkParms)
    # print("p1", nextPts, "st", st, "err", err)
    goodNewPt = nextPts[st == 1]  # 光流跟踪后特征点
    goodOldPt = prePt[st == 1]  # 上一帧特征点
    return goodOldPt, goodNewPt


def CalculateShift(prePt, curPt):
    x = curPt[:, 0] - prePt[:, 0]
    y = curPt[:, 1] - prePt[:, 1]
    avgX = np.mean(x)
    avgY = np.mean(y)
    return avgX, avgY


def get_box(ditection, prePt, curPt):
    d_x, d_y = CalculateShift(prePt, curPt)  # 计算偏移量
    box = [0] * 4
    box[0], box[2], box[1], box[3] = ditection[0] + d_x, ditection[2] + d_x, ditection[1] + d_y, ditection[3] + d_y
    return box


def Test():
    pathroot = ".\\"
    resPath = pathroot + "det.txt"
    video_path = pathroot + "video.mp4"
    video = cv2.VideoWriter(video_path, cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), 10, (1920, 1080))
    detRes = np.loadtxt(resPath, delimiter=',')
    preImg = GetImg(pathroot, 1)  # 初始化为000001.jpg   preImg:上一帧图片
    for num in range(2, int(max(detRes[:, 0]))):
        print(num)
        img = GetImg(pathroot, num)  # img:当前帧图片
        dets = GetDetFrameRes(detRes, num - 1)  # 上一帧图片的检测框
        drawImg = img.copy()
        for i in range(len(dets)):
            detect = dets[i]  # 上一帧图片的单个框
            boxKeyPt = SelectPointByBox(preImg, detect)  # 找在框里的关键点
            if (len(boxKeyPt) < 3):
                continue  # 框里关键点少于3 不做跟踪
            prePt, curPt = OpticalFlowLk(preImg, img, boxKeyPt, None)
            bbox = get_box(detect, prePt, curPt)
            if np.isnan(bbox[0]): continue
            for i in range(curPt.shape[0] - 1, -1, -1):
                c, d = curPt[i].ravel()
                if not (max(0, bbox[0] - 2) <= c <= min(1920, bbox[2] + 2) and
                        max(0, bbox[1] - 2) <= d <= min(1080, bbox[3] + 2)):
                    prePt = np.delete(prePt, i, 0)
                    curPt = np.delete(curPt, i, 0)
            new_b = get_box(detect, prePt, curPt)  # 最终框
            if np.isnan(new_b[0]): continue
            cv2.rectangle(drawImg, (int(new_b[0]), int(new_b[1])), (int(new_b[2]), int(new_b[3])), (96, 48, 176), 2)
            mask = np.zeros_like(preImg)
            color = np.random.randint(0, 255, (20000, 3))
            for i, (new, old) in enumerate(zip(prePt, curPt)):
                a, b = new.ravel()
                c, d = old.ravel()
                mask = cv2.line(mask, (int(a), int(b)), (int(c), int(d)), color[i].tolist(), 2)
                drawImg = cv2.circle(drawImg, (int(a), int(b)), 1, color[i].tolist(), -1)
            drawImg = cv2.add(drawImg, mask)

        cv2.imshow("img", drawImg)
        cv2.waitKey(10)
        preImg = img.copy()
        video.write(drawImg)
    video.release()


if __name__ == "__main__":
    Test()

三、c++ 代码

Optical(std::vector<cv::Rect_> boxes, cv::Mat preImg, cv::Mat curImg) 构造函数
void OpticalDeal(); 处理计算
std::vector<cv::Rect_> GetBoxResult(); 获取结果

Optical.h 文件

#include <vector>
#include "opencv2/opencv.hpp"
#include "opencv2/features2d.hpp"


class Optical
{
public:
    Optical(std::vector<cv::Rect_<float>> boxes, cv::Mat preImg, cv::Mat curImg)
    {
        mBoxes = boxes;
        mCurImg = curImg;
        mPreImg = preImg;
    }   
    void OpticalDeal();                                     // 计算
    std::vector<cv::Rect_<float>> GetBoxResult();           // 获取光流跟踪后得到的结果框 

private:
    std::vector<cv::Point2f> GetCornorPoint();              // fast检测关键点坐标
    cv::Rect_<float> GetExpBox(cv::Rect_<float> box);       // 获取比检测框大pixeParam像素的框
    void OpticalFlowLk(std::vector<cv::Point2f> prePt);     // 光流跟踪
    cv::Rect_<float> GetUpdateBox(cv::Rect_<float> box, std::vector<cv::Point2f> prePoints, std::vector<cv::Point2f> curPoints);    // 修正框
    void SelectPt(cv::Rect_<float> box, std::vector<cv::Point2f> &prePoints, std::vector<cv::Point2f> &curPoints);                  // 选取合适的关键点 过滤一部分关键点
    cv::Rect_<float> CorrectBox(cv::Rect_<float> box); 

private:
    int pixeParam = 2;                      // 关键点选取像素参数 多截取pixeParam像素
    int fastFeatureDetectParam = 10;        // fast关键点检测参数，参数越小，关键点检测越多
    int keyPointCountParam = 3;             // 检测框里关键点较少就不进行光流跟踪
    std::vector<int> mIndex = {0};          // 光流跟踪每个框关键点的索引位置 
    std::vector<cv::Rect_<float>> mBoxes;   // 检测框
    cv::Mat mPreImg;                        // 上一帧图
    cv::Mat mCurImg;                        // 当前图片
};

Optical.cpp 文件

#include "Optical.h"

std::vector<cv::Rect_<float>> Optical::GetBoxResult()
{
    return mBoxes;
}

void Optical::OpticalDeal()
{
    std::vector<cv::Point2f> fastKeyPoint = GetCornorPoint();   // fast检测的角点
    OpticalFlowLk(fastKeyPoint);                                // 光流跟踪 获取点与点匹配
}

std::vector<cv::Point2f> Optical::GetCornorPoint()
{   
    std::vector<cv::Point2f> res;
    cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create(fastFeatureDetectParam);
    int num = 0;                                            // 计数多少个关键点 
    for (int i = 0; i < mBoxes.size(); ++i) {
        std::vector<cv::KeyPoint> keyPoints;
        cv::Rect_<float> newBox = GetExpBox(mBoxes[i]);
        cv::Mat image = mPreImg(newBox);                    // 截取检测框检测的图片
        detector->detect(image, keyPoints);
        num = num + keyPoints.size();
        mIndex.push_back(num);
        for (auto points:keyPoints) {
            points.pt = points.pt + cv::Point_<float>(newBox.x, newBox.y);
            res.push_back(points.pt);
        }
    }
    return res;
}

void Optical::OpticalFlowLk(std::vector<cv::Point2f> prePt)
{
    cv::Mat curImgGray, preImgGray;
    std::vector<uchar> status;
	std::vector<float> err;
    cv::cvtColor(mCurImg, curImgGray, cv::COLOR_RGBA2GRAY);     // 当前图片灰度
    cv::cvtColor(mPreImg, preImgGray, cv::COLOR_RGBA2GRAY);     // 上一帧图片灰度
    std::vector<cv::Point2f> pt;
    cv::calcOpticalFlowPyrLK(preImgGray, curImgGray, prePt, pt, status, err); 
    for (int i = 0; i < mIndex.size() - 1; ++i) {
        int leftIndex = mIndex[i], rightIndex = mIndex[i + 1];
        // 关键点太少不进行光流跟踪(1)
        if (rightIndex - leftIndex >= keyPointCountParam) {
            std::vector<cv::Point2f> preIndexPt(prePt.begin() + leftIndex, prePt.begin() + rightIndex);
            std::vector<cv::Point2f> indexPt(pt.begin() + leftIndex, pt.begin()+rightIndex);
            std::vector<uchar> indexStatus(status.begin() + leftIndex, status.begin()+rightIndex);
            int length = preIndexPt.size(); 
            for (int j = length - 1 ; j > -1; --j) {
                if (status[j] != 1) {
                    indexPt.erase(indexPt.begin() + i);
                    preIndexPt.erase(preIndexPt.begin() + j);
                }
            }
            // 跟踪到的关键点少不进行光流跟踪(2)
            if (preIndexPt.size() > keyPointCountParam) {
                cv::Rect_<float> newBox = GetUpdateBox(mBoxes[i], preIndexPt, indexPt);
                SelectPt(newBox, preIndexPt, indexPt);
                if (preIndexPt.size() > keyPointCountParam) {
                    mBoxes[i] = GetUpdateBox(mBoxes[i], preIndexPt, indexPt);
                }
            }
        }
    }
}

// expend pixeParam bounding box to optical track
cv::Rect_<float> Optical::GetExpBox(cv::Rect_<float> box) 
{
    cv::Rect_<float> newBox = box + cv::Point_<float>(-pixeParam, -pixeParam) + cv::Size_<float>(2 * pixeParam, 2 * pixeParam);
    return CorrectBox(newBox);
}

cv::Rect_<float> Optical::GetUpdateBox(cv::Rect_<float> box, std::vector<cv::Point2f> prePoints, std::vector<cv::Point2f> curPoints)
{
    float avgX = 0, avgY = 0;
    int length = prePoints.size();
    for (int i = 0; i < length; ++i) {
        avgX += curPoints[i].x - prePoints[i].x;
        avgY += curPoints[i].y - prePoints[i].y;
    }
    avgX = avgX / length;
    avgY = avgY / length;
    cv::Rect_<float> resBox = box + cv::Point_<float>(avgX, avgY);
    return CorrectBox(resBox);
}

void Optical::SelectPt(cv::Rect_<float> box, std::vector<cv::Point2f> &prePoints, std::vector<cv::Point2f> &curPoints)
{
    int length = prePoints.size();
    for (int i = length - 1 ; i >= 0; --i) {
        float x = curPoints[i].x, y = curPoints[i].y;
        if (x < (box.x - pixeParam) || x > (box.x + box.width + pixeParam) || y < (box.y - pixeParam) || y > (box.y + box.height + pixeParam)) {
            curPoints.erase(curPoints.begin() + i);
            prePoints.erase(prePoints.begin() + i);
        }
    }
}

// correct box when box beyond border
cv::Rect_<float> Optical::CorrectBox(cv::Rect_<float> box)
{
    int w = mPreImg.cols, h = mPreImg.rows;
    box.x = (box.x <= 0) ? 0 : box.x;
    box.y = (box.y <= 0) ? 0 : box.y;
    box.width = ((box.width + box.x) >= w - 1) ? w - box.x - 1 : box.width;
    box.height = ((box.height + box.y) >= h - 1) ? h - box.y - 1 : box.height;
    return box;
}