目录
特征跟踪
特征跟踪，也称为目标跟踪或点跟踪，是指在序列图像中追踪物体或者场景中的特定特征点的过程。特征点可以是具有独特性质的像素，如角点、边缘、纹理等。通过跟踪这些特征点，可以在连续图像帧中确定它们的位置、速度和运动轨迹。
在计算机视觉和图像处理中，特征点是图像中具有显著性质或者信息的位置。对图像序列进行特征点的跟踪可以用于很多应用，例如目标追踪、运动估计、相机姿态估计、结构重建等。
特征点的选择通常取决于应用的需求和图像中的内容，常用的特征点检测算法包括Harris角点检测、Shi-Thomasi角点检测、SIFT（尺度不变特征变换）等。特征点跟踪的算法有很多，例如光流法、角点追踪等，它们可以根据特征点在前后帧之间的移动来判断物体或场景的运动。
DepthAI给我们提供了特征跟踪（Feature Tracker）功能模块，它用于在连续的图像帧中追踪和识别特定的特征点。Depthai 通过使用硬件加速和可编程的算法，实现了实时的特征跟踪功能。
DepthAI 的特征跟踪功能通常用于目标追踪和运动估计应用。它可以识别并跟踪视频中的关键点、边缘、角点或其他具有显著性质的图像特征。这些特征点的位置和运动轨迹可以用于估计物体的运动，在许多应用中非常有用，比如机器人导航、增强现实、虚拟现实等。
另外，DepthAI 给我们提供了编程接口，使开发人员能够配置和使用特征跟踪功能。通过适当的配置参数和算法选择，可以优化特征跟踪的性能和准确性，以满足应用的需求。
下面我们来实现一个OAK相机特征跟踪的代码
Setup 1: 创建文件
- 创建新建7-feature-tracker文件夹
- 用vscode打开该文件夹
- 新建一个main.py 文件
Setup 2: 安装依赖
安装依赖前需要先创建和激活虚拟环境，我这里已经创建了虚拟环境OAKenv，在终端中输入cd…退回到OAKenv的根目录，输入 OAKenv\Scripts\activate激活虚拟环境
安装pip依赖项：
pip install numpy opencv-python depthai blobconverter --user
Setup 3: 导入需要的包
在main.py中导入项目需要的包
import cv2
import depthai as dai
from collections import deque
这里从 Python 的 collections 模块中导入 deque 类，deque 类提供了各种方法来操作队列，如 append()、appendleft()、pop()、popleft()，以及像列表一样的索引访问。当需要高效地从队列的两端添加或删除元素时，deque 尤为有用。
Setup 4: 定义FeatureTrackerDrawer类
定义一个名为 FeatureTrackerDrawer 的类，用于绘制特征追踪器的路径。
class FeatureTrackerDrawer:
定义变量
lineColor = (200, 0, 200)
pointColor = (0, 0, 255)
circleRadius = 2
maxTrackedFeaturesPathLength = 30
trackedFeaturesPathLength = 10
trackedIDs = None
trackedFeaturesPath = None
lineColor：追踪路径的线条颜色。pointColor：特征点的颜色。circleRadius：绘制特征点的圆的半径。maxTrackedFeaturesPathLength：追踪路径的最大长度。trackedFeaturesPathLength：追踪路径的当前长度。trackedIDs：当前追踪的特征点的 ID 集合。trackedFeaturesPath：特征点的追踪路径。
定义onTrackBar方法
def onTrackBar(self, val):
FeatureTrackerDrawer.trackedFeaturesPathLength = val
pass
这个 onTrackBar 方法接收一个参数 val，该参数表示滑动条的当前值。
在这个方法中，它将传入的 val 值赋给 FeatureTrackerDrawer.trackedFeaturesPathLength，以更新追踪路径的长度。这样，当滑块的值改变时，追踪路径的长度也会相应地改变。
pass 语句表示该方法暂时不执行任何具体的操作，仅作为占位符存在。
定义trackFeaturePath方法
trackFeaturePath方法用于追踪特征点的路径，并更新追踪路径记录。
def trackFeaturePath(self, features):
newTrackedIDs = set()
for currentFeature in features:
currentID = currentFeature.id
newTrackedIDs.add(currentID)
if currentID not in self.trackedFeaturesPath:
self.trackedFeaturesPath[currentID] = deque()
path = self.trackedFeaturesPath[currentID]
path.append(currentFeature.position)
while(len(path) > max(1, FeatureTrackerDrawer.trackedFeaturesPathLength)):
path.popleft()
self.trackedFeaturesPath[currentID] = path
featuresToRemove = set()
for oldId in self.trackedIDs:
if oldId not in newTrackedIDs:
featuresToRemove.add(oldId)
for id in featuresToRemove:
self.trackedFeaturesPath.pop(id)
self.trackedIDs = newTrackedIDs
该方法接受一个名为 features 的参数，表示当前帧中的特征点列表。
-
使用
newTrackedIDs = set()创建一个空的集合newTrackedIDs用于存储新追踪的特征点的 ID -
使用
for循环遍历features中的每个特征点currentFeature。在循环中，首先获取当前特征点的 ID，并将其添加到
newTrackedIDs集合中。检查当前特征点的 ID 是否已经存在于
trackedFeaturesPath字典中。如果不存在，就使用deque()方法在字典中创建一个空的双向队列path来存储该特征点的路径。将当前特征点的位置 (
currentFeature.position) 添加到path队列的末尾。使用
while循环，确保path的长度不超过当前设置的最大追踪路径长度 (max(1, FeatureTrackerDrawer.trackedFeaturesPathLength))。如果超过了，则从队列的前端删除元素。将更新后的
path队列赋值给trackedFeaturesPath字典中对应的特征点 ID。 -
创建一个空的集合
featuresToRemove用于存储需要移除的特征点的 ID。 -
使用
for循环遍历trackedIDs中的每个旧的特征点 ID。如果该 ID 不在newTrackedIDs集合中，表示该特征点不再存在于当前帧的特征点列表中，将其 ID 添加到featuresToRemove集合中。 -
使用
for循环遍历featuresToRemove集合中的每个特征点 ID，并从trackedFeaturesPath字典中移除对应的特征点路径。 -
将
newTrackedIDs集合赋值给trackedIDs，更新已追踪的特征点的 ID。
定义drawFeatures方法
定义drawFeatures方法，绘制特征点的路径和特征点本身。通过这个方法，可以在图像上绘制特征点的路径，并将特征点的位置设置为当前的追踪路径长度。
def drawFeatures(self, img):
cv2.setTrackbarPos(self.trackbarName, self.windowName, FeatureTrackerDrawer.trackedFeaturesPathLength)
for featurePath in self.trackedFeaturesPath.values():
path = featurePath
for j in range(len(path) - 1):
src = (int(path[j].x), int(path[j].y))
dst = (int(path[j + 1].x), int(path[j + 1].y))
cv2.line(img, src, dst, self.lineColor, 1, cv2.LINE_AA, 0)
j = len(path) - 1
cv2.circle(img, (int(path[j].x), int(path[j].y)), self.circleRadius, self.pointColor, -1, cv2.LINE_AA, 0)
drawFeatures 方法用于在图像上绘制特征点的路径。方法接收一个名为 img 的参数，表示要绘制特征点路径的图像。
-
使用
cv2.setTrackbarPos方法设置特征点的位置，以保持特征点的值与FeatureTrackerDrawer.trackedFeaturesPathLength相同。 -
使用
for循环遍历trackedFeaturesPath字典中的每个特征点路径。在循环中，首先获取当前特征点路径
featurePath。然后，使用
for循环遍历路径path中的每个点的索引。在循环中，获取当前点
path[j]和下一个点path[j+1]的坐标，并将其转换为整数类型。调用
cv2.line方法，在图像上绘制从当前点到下一个点的线段，线段的颜色为self.lineColor，宽度为 1，线段的类型为cv2.LINE_AA，线段的连接方式为 0。然后，将索引值
j更新为路径path中的最后一个点的索引。接着，使用
cv2.circle方法，在图像上绘制路径path中最后一个点的圆形标记，圆心坐标为路径最后一个点的坐标(int(path[j].x), int(path[j].y))，圆的半径为self.circleRadius，圆的颜色为self.pointColor，圆的类型为cv2.LINE_AA。
定义FeatureTrackerDrawer类的构造函数
定义FeatureTrackerDrawer类的构造函数 __init__() ，它初始化了类的一些属性，该方法接受两个参数trackbarName和windowName，分别代表跟踪点的名称和窗口的名称。
def __init__(self, trackbarName, windowName):
self.trackbarName = trackbarName
self.windowName = windowName
cv2.namedWindow(windowName)
cv2.createTrackbar(trackbarName, windowName, FeatureTrackerDrawer.trackedFeaturesPathLength, FeatureTrackerDrawer.maxTrackedFeaturesPathLength, self.onTrackBar)
self.trackedIDs = set()
self.trackedFeaturesPath = dict()
-
将
trackbarName和windowName赋值给self.trackbarName和self.windowName属性。 -
使用
cv2.namedWindow方法创建一个窗口，并将windowName作为窗口的名称。 -
使用
cv2.createTrackbar方法cv2.createTrackbar函数用于在GUI窗口中创建一个滚动条，并为其设置回调函数。滚动条的名称为trackbarName，所在的窗口为windowName。滚动条的初始值设置为FeatureTrackerDrawer.trackedFeaturesPathLength，最大值设置为FeatureTrackerDrawer.maxTrackedFeaturesPathLength。在滚动条位置发生变化时，会调用self.onTrackBar方法。 -
将空集合赋值给
self.trackedIDs属性，并将空字典赋值给self.trackedFeaturesPath属性。
通过这个构造函数，我们可以创建一个FeatureTrackerDrawer对象，并初始化相关的变量和属性。
Setup 5: 创建pipeline
pipeline = dai.Pipeline()
Setup 6: 创建节点
创建相机节点
monoLeft = pipeline.createMonoCamera()
monoRight = pipeline.createMonoCamera()
创建特征检测节点
featureTrackerLeft = pipeline.createFeatureTracker()
featureTrackerRight = pipeline.createFeatureTracker()
创建数据交互节点
xoutPassthroughFrameLeft = pipeline.createXLinkOut()
xoutTrackedFeaturesLeft = pipeline.createXLinkOut()
xoutPassthroughFrameRight = pipeline.createXLinkOut()
xoutTrackedFeaturesRight = pipeline.createXLinkOut()
xinTrackedFeaturesConfig = pipeline.createXLinkIn()
xoutPassthroughFrameLeft.setStreamName("passthroughFrameLeft")
xoutTrackedFeaturesLeft.setStreamName("trackedFeaturesLeft")
xoutPassthroughFrameRight.setStreamName("passthroughFrameRight")
xoutTrackedFeaturesRight.setStreamName("trackedFeaturesRight")
xinTrackedFeaturesConfig.setStreamName("trackedFeaturesConfig")
Setup 7:设置相关属性
设置相机的分辨率和板载插槽
monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
Setup 8: 建立链接关系
建立相机和特征跟踪器之间的数据连接
monoLeft.out.link(featureTrackerLeft.inputImage)
featureTrackerLeft.passthroughInputImage.link(xoutPassthroughFrameLeft.input)
featureTrackerLeft.outputFeatures.link(xoutTrackedFeaturesLeft.input)
xinTrackedFeaturesConfig.out.link(featureTrackerLeft.inputConfig)
monoRight.out.link(featureTrackerRight.inputImage)
featureTrackerRight.passthroughInputImage.link(xoutPassthroughFrameRight.input)
featureTrackerRight.outputFeatures.link(xoutTrackedFeaturesRight.input)
xinTrackedFeaturesConfig.out.link(featureTrackerRight.inputConfig)
Setup 9: 为跟踪器对象设置硬件资源
setHardwareResources函数为跟踪器设置硬件资源。
numShaves = 2
numMemorySlices = 2
featureTrackerLeft.setHardwareResources(numShaves, numMemorySlices)
featureTrackerRight.setHardwareResources(numShaves, numMemorySlices)
featureTrackerConfig = featureTrackerRight.initialConfig.get()
print("Press 's' to switch between Lucas-Kanade optical flow and hardware accelerated motion estimation!")
使用setHardwareResources函数为两个跟踪器对象featureTrackerLeft和featureTrackerRight设置硬件资源。
numShaves（Myriad X硬件加速器的shave数量）和numMemorySlices（内存切片的数量）是两个参数，用于指定可用的硬件资源数量。
通过featureTrackerRight的initialConfig属性获取了其初始配置，并将其存储在featureTrackerConfig变量中。
Setup 10: 连接设备并启动管道
with dai.Device(pipeline) as device:
Setup 11: 创建与DepthAI设备通信的输入队列和输出队列
passthroughImageLeftQueue = device.getOutputQueue("passthroughFrameLeft", 8, False)
outputFeaturesLeftQueue = device.getOutputQueue("trackedFeaturesLeft", 8, False)
passthroughImageRightQueue = device.getOutputQueue("passthroughFrameRight", 8, False)
outputFeaturesRightQueue = device.getOutputQueue("trackedFeaturesRight", 8, False)
inputFeatureTrackerConfigQueue = device.getInputQueue("trackedFeaturesConfig")
使用FeatureTrackerDrawer类创建两个特征跟踪器的绘图对象
leftWindowName = "left"
leftFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", leftWindowName)
rightWindowName = "right"
rightFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", rightWindowName)
使用FeatureTrackerDrawer类创建了两个特征跟踪器的绘图对象，一个用于左侧（leftFeatureDrawer），一个用于右侧（rightFeatureDrawer）。
这些绘图对象用于可视化特征跟踪的持续时间，并且与相应的窗口名称关联起来。
Setup 12: 主循环
while True:
从队列中获取输入的图像帧。
inPassthroughFrameLeft = passthroughImageLeftQueue.get()
passthroughFrameLeft = inPassthroughFrameLeft.getFrame()
leftFrame = cv2.cvtColor(passthroughFrameLeft, cv2.COLOR_GRAY2BGR)
inPassthroughFrameRight = passthroughImageRightQueue.get()
passthroughFrameRight = inPassthroughFrameRight.getFrame()
rightFrame = cv2.cvtColor(passthroughFrameRight, cv2.COLOR_GRAY2BGR)
-
分别从名为passthroughImageLeftQueue和passthroughImageRightQueue的队列中获取输入的图像帧。
-
从inPassthroughFrameLeft和inPassthroughFrameRight中获取帧，并将它们分别存储在passthroughFrameLeft和passthroughFrameRight变量中。
-
使用cv2.cvtColor函数将passthroughFrameLeft和passthroughFrameRight从灰度图像转换为BGR彩色图像，并分别存储在leftFrame和rightFrame变量中。
从队列中获取跟踪的特征
trackedFeaturesLeft = outputFeaturesLeftQueue.get().trackedFeatures
leftFeatureDrawer.trackFeaturePath(trackedFeaturesLeft)
leftFeatureDrawer.drawFeatures(leftFrame)
trackedFeaturesRight = outputFeaturesRightQueue.get().trackedFeatures
rightFeatureDrawer.trackFeaturePath(trackedFeaturesRight)
rightFeatureDrawer.drawFeatures(rightFrame)
从outputFeaturesLeftQueue队列中获取跟踪的特征，并将其存储在trackedFeaturesLeft变量中。
-
使用leftFeatureDrawer对象的trackFeaturePath方法跟踪这些特征的路径，并使用drawFeatures方法在leftFrame图像上绘制这些特征。
-
从outputFeaturesRightQueue队列中获取跟踪的特征，并将其存储在trackedFeaturesRight变量中。
-
使用rightFeatureDrawer对象的trackFeaturePath方法跟踪这些特征的路径，并使用drawFeatures方法在rightFrame图像上绘制这些特征。
这样，我们就完成了特征跟踪和绘制的步骤，可以在图像上可视化跟踪的特征路径。
显示帧图像
cv2.imshow(leftWindowName, leftFrame)
cv2.imshow(rightWindowName, rightFrame)
对键盘输入响应的程序
key = cv2.waitKey(1)
if key == ord('q'):
break
elif key == ord('s'):
if featureTrackerConfig.motionEstimator.type == dai.FeatureTrackerConfig.MotionEstimator.Type.LUCAS_KANADE_OPTICAL_FLOW:
featureTrackerConfig.motionEstimator.type = dai.FeatureTrackerConfig.MotionEstimator.Type.HW_MOTION_ESTIMATION
print("Switching to hardware accelerated motion estimation")
else:
featureTrackerConfig.motionEstimator.type = dai.FeatureTrackerConfig.MotionEstimator.Type.LUCAS_KANADE_OPTICAL_FLOW
print("Switching to Lucas-Kanade optical flow")
cfg = dai.FeatureTrackerConfig()
cfg.set(featureTrackerConfig)
inputFeatureTrackerConfigQueue.send(cfg)
使用cv2.waitKey函数来等待用户按下键盘上的按键。
-
如果用户按下键盘上的’q’键，程序将跳出循环，从而退出程序。
-
如果用户按下键盘上的’s’键，程序将执行以下操作：
-
检查featureTrackerConfig.motionEstimator.type的当前值。如果其值是dai.FeatureTrackerConfig.MotionEstimator.Type.LUCAS_KANADE_OPTICAL_FLOW，则会执行以下操作：
- 将featureTrackerConfig.motionEstimator.type设置为dai.FeatureTrackerConfig.MotionEstimator.Type.HW_MOTION_ESTIMATION，以切换到硬件加速的运动估计。
-
如果featureTrackerConfig.motionEstimator.type的当前值不是dai.FeatureTrackerConfig.MotionEstimator.Type.LUCAS_KANADE_OPTICAL_FLOW，则会执行以下操作：
- 将featureTrackerConfig.motionEstimator.type设置为dai.FeatureTrackerConfig.MotionEstimator.Type.LUCAS_KANADE_OPTICAL_FLOW，以切换到Lucas-Kanade光流。
-
创建一个新的dai.FeatureTrackerConfig对象cfg，并将featureTrackerConfig设置为其值。
-
使用inputFeatureTrackerConfigQueue队列发送cfg对象。
Setup 13:运行程序
在终端中输入如下指令运行程序
python main.py
