FeatureTracker on Oak 4 D

MatteoL

Following this discussian, I have found some other issues with the FeatureTracker node on a Oak 4 D. I ran your example on the Oak-D W PoE and on the Oak 4 D:

oak-d-w-poe.mp4

11MB

oak-4-d.mp4

8MB

The result are incompatible. You can see that the features on the ceiling beams move incorrectly in the newer model. The two implementations are clearly different and the Oak 4 returns incorrect result.

DepthAI v3.8 on Windows, Oak 4 D R9 with Luxonis OS v1.35.

Both videos have been blurred on the right side for privacy reasons.

Here is the full script code, I have made minor changes from your example for running the FeatureTracker node against the left camera and configured it for using the Shi-Tomasi corner detector:

import cv2
from collections import deque
import depthai as dai

class FeatureTrackerDrawer:
    lineColor = (200, 0, 200)
    pointColor = (0, 0, 255)
    circleRadius = 2
    maxTrackedFeaturesPathLength = 30
    trackedFeaturesPathLength = 10

    trackedIDs = None
    trackedFeaturesPath = None

    def onTrackBar(self, val):
        FeatureTrackerDrawer.trackedFeaturesPathLength = val
        pass

    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

    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)

    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()

print("Press 'm' to enable/disable motion estimation!")

inputConfigQueue = None
def on_trackbar(val):
    try:
        cfg = dai.FeatureTrackerConfig()
        cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
        cornerDetector.numMaxFeatures = cv2.getTrackbarPos('numMaxFeatures', 'Features')
        cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures

        thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
        thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')
        cornerDetector.thresholds = thresholds
    except cv2.error as e:
        pass

    cfg.setCornerDetector(cornerDetector)
    if inputConfigQueue:
        inputConfigQueue.send(cfg)

cv2.namedWindow('Features', cv2.WINDOW_NORMAL)
cv2.resizeWindow('Features', 1080, 800)

cv2.createTrackbar('harrisScore','Features',20000,25000, on_trackbar)
cv2.createTrackbar('numMaxFeatures','Features',256,1024, on_trackbar)

# Create pipeline
with dai.Pipeline() as pipeline:
    camera = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
    # camOutput = camera.requestOutput((640, 640), dai.ImgFrame.Type.NV12)
    # manip = pipeline.create(dai.node.ImageManip)
    # manip.initialConfig.setFrameType(dai.ImgFrame.Type.GRAY8)
    # camOutput.link(manip.inputImage)

    featureTracker = pipeline.create(dai.node.FeatureTracker)

    # TYPE = dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS
    TYPE = dai.FeatureTrackerConfig.CornerDetector.Type.SHI_THOMASI

    featureTracker.initialConfig.setCornerDetector(TYPE)
    featureTracker.initialConfig.setMotionEstimator(False)
    featureTracker.initialConfig.setNumTargetFeatures(256)

    motionEstimator = dai.FeatureTrackerConfig.MotionEstimator()
    motionEstimator.enable = True
    featureTracker.initialConfig.setMotionEstimator(motionEstimator)

    cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
    cornerDetector.numMaxFeatures = 256
    cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures

    # RVC2 specific setting to allow for more features
    featureTracker.setHardwareResources(2,2)

    outputFeaturePassthroughQueue = featureTracker.passthroughInputImage.createOutputQueue()
    outputFeatureQueue = featureTracker.outputFeatures.createOutputQueue()

    camera.requestFullResolutionOutput(type=dai.ImgFrame.Type.GRAY8).link(featureTracker.inputImage)

    inputConfigQueue = featureTracker.inputConfig.createInputQueue()

    thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
    thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')

    cornerDetector.thresholds = thresholds
    featureTracker.initialConfig.setCornerDetector(cornerDetector)

    leftWindowName = "Features"
    leftFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", leftWindowName)

    pipeline.start()
    while pipeline.isRunning():
        outputPassthroughImage : dai.ImgFrame = outputFeaturePassthroughQueue.get()

        passthroughImage = outputPassthroughImage.getCvFrame()
        passthroughImage = cv2.cvtColor(passthroughImage, cv2.COLOR_GRAY2RGB)

        trackedFeaturesLeft = outputFeatureQueue.get().trackedFeatures


        leftFeatureDrawer.trackFeaturePath(trackedFeaturesLeft)
        leftFeatureDrawer.drawFeatures(passthroughImage)

        # Show the frame
        cv2.imshow(leftWindowName, passthroughImage)

        key = cv2.waitKey(1)
        if key == ord('q'):
            break
        elif key == ord('m'):
            cfg = dai.FeatureTrackerConfig()
            cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
            cornerDetector.type = TYPE
            cornerDetector.numMaxFeatures = cv2.getTrackbarPos('numMaxFeatures', 'Features')
            cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures

            thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
            thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')
            cornerDetector.thresholds = thresholds

            cfg.setCornerDetector(cornerDetector)
            cfg.setMotionEstimator(motionEstimator)

            if motionEstimator.enable == False:
                motionEstimator.enable = True
                cfg.setMotionEstimator(motionEstimator)
                print("Enabling motionEstimator")
            else:
                motionEstimator.enable = False
                cfg.setMotionEstimator(motionEstimator)
                print("Disabling motionEstimator")

            inputConfigQueue.send(cfg)

Oskar-Luxonis

Hey MatteoL - thanks for the detailed report. We tested the same pipeline on an OAK4 and found an important configuration detail: although TYPE is initially set to SHI_THOMASI, the final CornerDetector() configuration replaces it and defaults back to Harris. Therefore, the pipeline initially runs Harris rather than Shi–Tomasi.

Pressing m sends a configuration which explicitly sets Shi–Tomasi; on the first press it also disables motion estimation. In our testing, this produced more stable tracks.

Could you please set cornerDetector.type = TYPE on the final corner-detector configuration, then test motion estimation enabled and disabled separately? This will isolate which setting produces the improvement.

We also recommend trying the same test with DepthAI 3.9.0.

Best,
Oskar

MatteoL

Hi Oskar,
I agree with your point. I fixed the code to switch between HARRIS and SHI_THOMASI separately from enabling or disabling the motion detection.

However,

I don't notice any qualitative change while switching between Harris and Shi-Tomasi corner detectors. This may be an hint per se of some misconfiguration, as the two algorithm should return different results.
For both corner detectors while the motion tracking is enabled, some corners are tracked moving around while both the image and the camera are still.
The results sensibly differ between Oak-D W PoE and Oak 4 D. The same code returns different results on the two devices. In particular, you can see from the video I attached in the previous post that feature point on edges (ceiling beams) do move along the edge while the camera moves. Moreover, a false corner tracked in Oak-D W PoE becomes short-lived, whereas on the Oak 4 D the feature points are tracked on wrong paths.

oak-4-d.mp4
504kB

oak-d-w-poe.mp4
476kB

My point is that the implementation of FeatureTracker node differ between the new generation of cameras and the old one. With the new one being substantially worse.

I tested the code with DepthAI v3.9.0. The camera runs on Luxonis OS v1.37.0.

Here is the updated code:

import cv2
from collections import deque
import depthai as dai

class FeatureTrackerDrawer:
    lineColor = (200, 0, 200)
    pointColor = (0, 0, 255)
    circleRadius = 2
    maxTrackedFeaturesPathLength = 30
    trackedFeaturesPathLength = 10

    trackedIDs = None
    trackedFeaturesPath = None

    def onTrackBar(self, val):
        FeatureTrackerDrawer.trackedFeaturesPathLength = val
        pass

    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

    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)

    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()

print("Press 'm' to enable/disable motion estimation!")

inputConfigQueue = None
def on_trackbar(val):
    try:
        cfg = dai.FeatureTrackerConfig()
        cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
        cornerDetector.numMaxFeatures = cv2.getTrackbarPos('numMaxFeatures', 'Features')
        cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures

        thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
        thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')
        cornerDetector.thresholds = thresholds
    except cv2.error as e:
        pass

    cfg.setCornerDetector(cornerDetector)
    if inputConfigQueue:
        inputConfigQueue.send(cfg)

cv2.namedWindow('Features', cv2.WINDOW_NORMAL)
cv2.resizeWindow('Features', 1080, 800)

cv2.createTrackbar('harrisScore','Features',20000,25000, on_trackbar)
cv2.createTrackbar('numMaxFeatures','Features',256,1024, on_trackbar)

# Create pipeline
with dai.Pipeline() as pipeline:
    camera = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
    # camOutput = camera.requestOutput((640, 640), dai.ImgFrame.Type.NV12)
    # manip = pipeline.create(dai.node.ImageManip)
    # manip.initialConfig.setFrameType(dai.ImgFrame.Type.GRAY8)
    # camOutput.link(manip.inputImage)

    featureTracker = pipeline.create(dai.node.FeatureTracker)

    # TYPE = dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS
    TYPE = dai.FeatureTrackerConfig.CornerDetector.Type.SHI_THOMASI

    motionEstimator = dai.FeatureTrackerConfig.MotionEstimator()
    motionEstimator.enable = True
    featureTracker.initialConfig.setMotionEstimator(motionEstimator)

    print(f'Corner type {TYPE}')
    cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
    cornerDetector.type = TYPE
    cornerDetector.numMaxFeatures = 256
    cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures

    # RVC2 specific setting to allow for more features
    featureTracker.setHardwareResources(2,2)

    outputFeaturePassthroughQueue = featureTracker.passthroughInputImage.createOutputQueue()
    outputFeatureQueue = featureTracker.outputFeatures.createOutputQueue()

    camera.requestFullResolutionOutput(type=dai.ImgFrame.Type.GRAY8).link(featureTracker.inputImage)

    inputConfigQueue = featureTracker.inputConfig.createInputQueue()

    thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
    thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')

    cornerDetector.thresholds = thresholds
    featureTracker.initialConfig.setCornerDetector(cornerDetector)

    leftWindowName = "Features"
    leftFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", leftWindowName)

    pipeline.start()
    while pipeline.isRunning():
        outputPassthroughImage : dai.ImgFrame = outputFeaturePassthroughQueue.get()

        passthroughImage = outputPassthroughImage.getCvFrame()
        passthroughImage = cv2.cvtColor(passthroughImage, cv2.COLOR_GRAY2RGB)

        trackedFeaturesLeft = outputFeatureQueue.get().trackedFeatures

        leftFeatureDrawer.trackFeaturePath(trackedFeaturesLeft)
        leftFeatureDrawer.drawFeatures(passthroughImage)

        # Show the frame
        cv2.imshow(leftWindowName, passthroughImage)

        key = cv2.waitKey(1)
        if key == ord('q'):
            break
        elif key == ord('m') or key == ord('c'):
            cfg = dai.FeatureTrackerConfig()
            cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
            if key == ord('c'):
                TYPE = dai.FeatureTrackerConfig.CornerDetector.Type.SHI_THOMASI if TYPE == dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS else dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS
                print(f'Corner type {TYPE}')
            cornerDetector.type = TYPE
            cornerDetector.numMaxFeatures = cv2.getTrackbarPos('numMaxFeatures', 'Features')
            cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures

            thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
            thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')
            cornerDetector.thresholds = thresholds

            cfg.setCornerDetector(cornerDetector)
            cfg.setMotionEstimator(motionEstimator)

            if key == ord('m'):
                if motionEstimator.enable == False:
                    motionEstimator.enable = True
                    cfg.setMotionEstimator(motionEstimator)
                    print("Enabling motionEstimator")
                else:
                    motionEstimator.enable = False
                    cfg.setMotionEstimator(motionEstimator)
                    print("Disabling motionEstimator")

            inputConfigQueue.send(cfg)

Oskar-Luxonis

Hey MatteoL - the update to FeatureTracker node for RVC4 devices, should be in few weeks (your previous bug you found and this should also be improved). It wil be release in DepthAi 3.10..
I will tag you when it's out.

Thanks,
Oskar