Feature Tracker

1import cv2
2from collections import deque
3import depthai as dai
4
5class FeatureTrackerDrawer:
6    lineColor = (200, 0, 200)
7    pointColor = (0, 0, 255)
8    circleRadius = 2
9    maxTrackedFeaturesPathLength = 30
10    trackedFeaturesPathLength = 10
11
12    trackedIDs = None
13    trackedFeaturesPath = None
14
15    def onTrackBar(self, val):
16        FeatureTrackerDrawer.trackedFeaturesPathLength = val
17        pass
18
19    def trackFeaturePath(self, features):
20
21        newTrackedIDs = set()
22        for currentFeature in features:
23            currentID = currentFeature.id
24            newTrackedIDs.add(currentID)
25
26            if currentID not in self.trackedFeaturesPath:
27                self.trackedFeaturesPath[currentID] = deque()
28
29            path = self.trackedFeaturesPath[currentID]
30
31            path.append(currentFeature.position)
32            while(len(path) > max(1, FeatureTrackerDrawer.trackedFeaturesPathLength)):
33                path.popleft()
34
35            self.trackedFeaturesPath[currentID] = path
36
37        featuresToRemove = set()
38        for oldId in self.trackedIDs:
39            if oldId not in newTrackedIDs:
40                featuresToRemove.add(oldId)
41
42        for id in featuresToRemove:
43            self.trackedFeaturesPath.pop(id)
44
45        self.trackedIDs = newTrackedIDs
46
47    def drawFeatures(self, img):
48
49        cv2.setTrackbarPos(self.trackbarName, self.windowName, FeatureTrackerDrawer.trackedFeaturesPathLength)
50
51        for featurePath in self.trackedFeaturesPath.values():
52            path = featurePath
53
54            for j in range(len(path) - 1):
55                src = (int(path[j].x), int(path[j].y))
56                dst = (int(path[j + 1].x), int(path[j + 1].y))
57                cv2.line(img, src, dst, self.lineColor, 1, cv2.LINE_AA, 0)
58            j = len(path) - 1
59            cv2.circle(img, (int(path[j].x), int(path[j].y)), self.circleRadius, self.pointColor, -1, cv2.LINE_AA, 0)
60
61    def __init__(self, trackbarName, windowName):
62        self.trackbarName = trackbarName
63        self.windowName = windowName
64        cv2.namedWindow(windowName)
65        cv2.createTrackbar(trackbarName, windowName, FeatureTrackerDrawer.trackedFeaturesPathLength, FeatureTrackerDrawer.maxTrackedFeaturesPathLength, self.onTrackBar)
66        self.trackedIDs = set()
67        self.trackedFeaturesPath = dict()
68
69print("Press 'm' to enable/disable motion estimation!")
70
71inputConfigQueue = None
72def on_trackbar(val):
73    try:
74        cfg = dai.FeatureTrackerConfig()
75        cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
76        cornerDetector.numMaxFeatures = cv2.getTrackbarPos('numMaxFeatures', 'Features')
77        cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures
78
79        thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
80        thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')
81        cornerDetector.thresholds = thresholds
82    except cv2.error as e:
83        pass
84
85    cfg.setCornerDetector(cornerDetector)
86    if inputConfigQueue:
87        inputConfigQueue.send(cfg)
88
89cv2.namedWindow('Features', cv2.WINDOW_NORMAL)
90cv2.resizeWindow('Features', 1080, 800)
91
92cv2.createTrackbar('harrisScore','Features',20000,25000, on_trackbar)
93cv2.createTrackbar('numMaxFeatures','Features',256,1024, on_trackbar)
94
95# Create pipeline
96with dai.Pipeline() as pipeline:
97    camera = pipeline.create(dai.node.Camera).build()
98    camOutput = camera.requestOutput((640, 640), dai.ImgFrame.Type.NV12)
99    manip = pipeline.create(dai.node.ImageManip)
100    manip.initialConfig.setFrameType(dai.ImgFrame.Type.GRAY8)
101    camOutput.link(manip.inputImage)
102
103    featureTracker = pipeline.create(dai.node.FeatureTracker)
104
105    featureTracker.initialConfig.setCornerDetector(dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS)
106    featureTracker.initialConfig.setMotionEstimator(False)
107    featureTracker.initialConfig.setNumTargetFeatures(256)
108
109    motionEstimator = dai.FeatureTrackerConfig.MotionEstimator()
110    motionEstimator.enable = True
111    featureTracker.initialConfig.setMotionEstimator(motionEstimator)
112
113    cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
114    cornerDetector.numMaxFeatures = 256
115    cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures
116
117    # RVC2 specific setting to allow for more features
118    featureTracker.setHardwareResources(2,2)
119
120    outputFeaturePassthroughQueue = camOutput.createOutputQueue()
121    outputFeatureQueue = featureTracker.outputFeatures.createOutputQueue()
122
123    manip.out.link(featureTracker.inputImage)
124
125    inputConfigQueue = featureTracker.inputConfig.createInputQueue()
126
127    thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
128    thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')
129
130    cornerDetector.thresholds = thresholds
131    featureTracker.initialConfig.setCornerDetector(cornerDetector)
132
133    leftWindowName = "Features"
134    leftFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", leftWindowName)
135
136    pipeline.start()
137    while pipeline.isRunning():
138        outputPassthroughImage : dai.ImgFrame = outputFeaturePassthroughQueue.get()
139
140        passthroughImage = outputPassthroughImage.getCvFrame()
141        trackedFeaturesLeft = outputFeatureQueue.get().trackedFeatures
142
143
144        leftFeatureDrawer.trackFeaturePath(trackedFeaturesLeft)
145        leftFeatureDrawer.drawFeatures(passthroughImage)
146
147        # Show the frame
148        cv2.imshow(leftWindowName, passthroughImage)
149
150        key = cv2.waitKey(1)
151        if key == ord('q'):
152            break
153        elif key == ord('m'):
154            cfg = dai.FeatureTrackerConfig()
155            cornerDetector = dai.FeatureTrackerConfig.CornerDetector()
156            cornerDetector.numMaxFeatures = cv2.getTrackbarPos('numMaxFeatures', 'Features')
157            cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures
158
159            thresholds = dai.FeatureTrackerConfig.CornerDetector.Thresholds()
160            thresholds.initialValue = cv2.getTrackbarPos('harrisScore','Features')
161            cornerDetector.thresholds = thresholds
162
163            cfg.setCornerDetector(cornerDetector)
164            cfg.setMotionEstimator(motionEstimator)
165
166            if motionEstimator.enable == False:
167                motionEstimator.enable = True
168                cfg.setMotionEstimator(motionEstimator)
169                print("Enabling motionEstimator")
170            else:
171                motionEstimator.enable = False
172                cfg.setMotionEstimator(motionEstimator)
173                print("Disabling motionEstimator")
174
175            inputConfigQueue.send(cfg)

1#include <deque>
2#include <iostream>
3#include <map>
4#include <memory>
5#include <opencv2/opencv.hpp>
6#include <set>
7
8#include "depthai/depthai.hpp"
9
10class FeatureTrackerDrawer {
11   public:
12    static const cv::Scalar lineColor;
13    static const cv::Scalar pointColor;
14    static const int circleRadius = 2;
15    static const int maxTrackedFeaturesPathLength = 30;
16    static int trackedFeaturesPathLength;
17
18    FeatureTrackerDrawer(const std::string& trackbarName, const std::string& windowName) : trackbarName(trackbarName), windowName(windowName) {
19        cv::namedWindow(windowName);
20        cv::createTrackbar(trackbarName, windowName, &trackedFeaturesPathLength, maxTrackedFeaturesPathLength, onTrackBar, this);
21    }
22
23    static void onTrackBar(int val, void* userdata) {
24        trackedFeaturesPathLength = val;
25    }
26
27    void trackFeaturePath(const std::vector<dai::TrackedFeature>& features) {
28        std::set<int> newTrackedIDs;
29
30        for(const auto& currentFeature : features) {
31            int currentID = currentFeature.id;
32            newTrackedIDs.insert(currentID);
33
34            if(trackedFeaturesPath.find(currentID) == trackedFeaturesPath.end()) {
35                trackedFeaturesPath[currentID] = std::deque<dai::Point2f>();
36            }
37
38            auto& path = trackedFeaturesPath[currentID];
39            path.push_back(currentFeature.position);
40
41            while(path.size() > std::max(1, trackedFeaturesPathLength)) {
42                path.pop_front();
43            }
44        }
45
46        // Remove features that are no longer tracked
47        std::set<int> featuresToRemove;
48        for(const auto& oldId : trackedIDs) {
49            if(newTrackedIDs.find(oldId) == newTrackedIDs.end()) {
50                featuresToRemove.insert(oldId);
51            }
52        }
53
54        for(const auto& id : featuresToRemove) {
55            trackedFeaturesPath.erase(id);
56        }
57
58        trackedIDs = newTrackedIDs;
59    }
60
61    void drawFeatures(cv::Mat& img) {
62        cv::setTrackbarPos(trackbarName.c_str(), windowName.c_str(), trackedFeaturesPathLength);
63
64        for(const auto& [id, path] : trackedFeaturesPath) {
65            for(size_t j = 0; j < path.size() - 1; j++) {
66                cv::Point src(static_cast<int>(path[j].x), static_cast<int>(path[j].y));
67                cv::Point dst(static_cast<int>(path[j + 1].x), static_cast<int>(path[j + 1].y));
68                cv::line(img, src, dst, lineColor, 1, cv::LINE_AA, 0);
69            }
70
71            if(!path.empty()) {
72                size_t j = path.size() - 1;
73                cv::Point point(static_cast<int>(path[j].x), static_cast<int>(path[j].y));
74                cv::circle(img, point, circleRadius, pointColor, -1, cv::LINE_AA, 0);
75            }
76        }
77    }
78
79   private:
80    std::string trackbarName;
81    std::string windowName;
82    std::set<int> trackedIDs;
83    std::map<int, std::deque<dai::Point2f>> trackedFeaturesPath;
84};
85
86// Initialize static members
87const cv::Scalar FeatureTrackerDrawer::lineColor(200, 0, 200);
88const cv::Scalar FeatureTrackerDrawer::pointColor(0, 0, 255);
89int FeatureTrackerDrawer::trackedFeaturesPathLength = 10;
90
91void onTrackbar(int val, void* userdata) {
92    auto* inputConfigQueue = static_cast<std::shared_ptr<dai::InputQueue>*>(userdata);
93    try {
94        auto cfg = std::make_shared<dai::FeatureTrackerConfig>();
95        auto cornerDetector = dai::FeatureTrackerConfig::CornerDetector();
96        cornerDetector.numMaxFeatures = cv::getTrackbarPos("numMaxFeatures", "Features");
97        cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures;
98
99        auto thresholds = dai::FeatureTrackerConfig::CornerDetector::Thresholds();
100        thresholds.initialValue = cv::getTrackbarPos("harrisScore", "Features");
101        cornerDetector.thresholds = thresholds;
102
103        cfg->setCornerDetector(cornerDetector);
104        if(*inputConfigQueue) {
105            (*inputConfigQueue)->send(cfg);
106        }
107    } catch(const cv::Exception& e) {
108        // Ignore OpenCV errors
109    }
110}
111
112int main() {
113    std::cout << "Press 'm' to enable/disable motion estimation!" << std::endl;
114
115    // Create device
116    std::shared_ptr<dai::Device> device = std::make_shared<dai::Device>();
117
118    // Create pipeline
119    dai::Pipeline pipeline(device);
120
121    // Create nodes
122    auto camera = pipeline.create<dai::node::Camera>()->build();
123    auto camOutput = camera->requestOutput(std::make_pair(640, 640), dai::ImgFrame::Type::NV12);
124
125    auto manip = pipeline.create<dai::node::ImageManip>();
126    manip->initialConfig->setFrameType(dai::ImgFrame::Type::GRAY8);
127    camOutput->link(manip->inputImage);
128
129    auto featureTracker = pipeline.create<dai::node::FeatureTracker>();
130    featureTracker->initialConfig->setCornerDetector(dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS);
131    featureTracker->initialConfig->setMotionEstimator(false);
132    featureTracker->initialConfig->setNumTargetFeatures(256);
133
134    auto motionEstimator = dai::FeatureTrackerConfig::MotionEstimator();
135    motionEstimator.enable = true;
136    featureTracker->initialConfig->setMotionEstimator(motionEstimator);
137
138    auto cornerDetector = dai::FeatureTrackerConfig::CornerDetector();
139    cornerDetector.numMaxFeatures = 256;
140    cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures;
141
142    // RVC2 specific setting to allow for more features
143    featureTracker->setHardwareResources(2, 2);
144
145    auto outputFeaturePassthroughQueue = camOutput->createOutputQueue();
146    auto outputFeatureQueue = featureTracker->outputFeatures.createOutputQueue();
147    auto inputConfigQueue = featureTracker->inputConfig.createInputQueue();
148
149    manip->out.link(featureTracker->inputImage);
150
151    auto thresholds = dai::FeatureTrackerConfig::CornerDetector::Thresholds();
152    thresholds.initialValue = 20000;  // Default value
153
154    cornerDetector.thresholds = thresholds;
155    featureTracker->initialConfig->setCornerDetector(cornerDetector);
156
157    // Create window and trackbars
158    cv::namedWindow("Features", cv::WINDOW_NORMAL);
159    cv::resizeWindow("Features", 1080, 800);
160
161    cv::createTrackbar("harrisScore", "Features", nullptr, 25000, onTrackbar, &inputConfigQueue);
162    cv::createTrackbar("numMaxFeatures", "Features", nullptr, 1024, onTrackbar, &inputConfigQueue);
163    cv::setTrackbarPos("harrisScore", "Features", 20000);
164    cv::setTrackbarPos("numMaxFeatures", "Features", 256);
165
166    std::string leftWindowName = "Features";
167    FeatureTrackerDrawer leftFeatureDrawer("Feature tracking duration (frames)", leftWindowName);
168
169    // Start pipeline
170    pipeline.start();
171
172    while(true) {
173        auto outputPassthroughImage = outputFeaturePassthroughQueue->get<dai::ImgFrame>();
174        if(outputPassthroughImage == nullptr) continue;
175
176        cv::Mat passthroughImage = outputPassthroughImage->getCvFrame();
177        auto trackedFeaturesLeft = outputFeatureQueue->get<dai::TrackedFeatures>()->trackedFeatures;
178
179        leftFeatureDrawer.trackFeaturePath(trackedFeaturesLeft);
180        leftFeatureDrawer.drawFeatures(passthroughImage);
181
182        cv::imshow(leftWindowName, passthroughImage);
183
184        int key = cv::waitKey(1);
185        if(key == 'q') {
186            break;
187        } else if(key == 'm') {
188            auto cfg = std::make_shared<dai::FeatureTrackerConfig>();
189            auto cornerDetector = dai::FeatureTrackerConfig::CornerDetector();
190            cornerDetector.numMaxFeatures = cv::getTrackbarPos("numMaxFeatures", "Features");
191            cornerDetector.numTargetFeatures = cornerDetector.numMaxFeatures;
192
193            auto thresholds = dai::FeatureTrackerConfig::CornerDetector::Thresholds();
194            thresholds.initialValue = cv::getTrackbarPos("harrisScore", "Features");
195            cornerDetector.thresholds = thresholds;
196
197            cfg->setCornerDetector(cornerDetector);
198            cfg->setMotionEstimator(motionEstimator);
199
200            if(!motionEstimator.enable) {
201                motionEstimator.enable = true;
202                cfg->setMotionEstimator(motionEstimator);
203                std::cout << "Enabling motionEstimator" << std::endl;
204            } else {
205                motionEstimator.enable = false;
206                cfg->setMotionEstimator(motionEstimator);
207                std::cout << "Disabling motionEstimator" << std::endl;
208            }
209
210            inputConfigQueue->send(cfg);
211        }
212    }
213
214    return 0;
215}