Feature tracking support

CMakeLists.txt

@@ -158,6 +158,7 @@ add_library(${TARGET_CORE_NAME}
     src/pipeline/node/ObjectTracker.cpp
     src/pipeline/node/IMU.cpp
     src/pipeline/node/EdgeDetector.cpp
+    src/pipeline/node/FeatureTracker.cpp
     src/pipeline/datatype/Buffer.cpp
     src/pipeline/datatype/ImgFrame.cpp
     src/pipeline/datatype/ImageManipConfig.cpp
@@ -173,6 +174,8 @@ add_library(${TARGET_CORE_NAME}
     src/pipeline/datatype/IMUData.cpp
     src/pipeline/datatype/StereoDepthConfig.cpp
     src/pipeline/datatype/EdgeDetectorConfig.cpp
+    src/pipeline/datatype/FeatureTrackerData.cpp
+    src/pipeline/datatype/FeatureTrackerConfig.cpp
     src/utility/Initialization.cpp
     src/utility/Resources.cpp
     src/xlink/XLinkConnection.cpp

cmake/Depthai/DepthaiDeviceSideConfig.cmake

@@ -2,7 +2,7 @@
 set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot")
 
 # "full commit hash of device side binary"
-set(DEPTHAI_DEVICE_SIDE_COMMIT "5f0f94deabdae2c41a7c4be1469477a0e77a1264")
+set(DEPTHAI_DEVICE_SIDE_COMMIT "0582bfa0a5cb21ffab8317f8c8feb54d0318dbf8")
 
 # "version if applicable"
 set(DEPTHAI_DEVICE_SIDE_VERSION "")

examples/CMakeLists.txt

@@ -268,6 +268,8 @@ dai_add_example(bootloader_version src/bootloader_version.cpp ON)
 #dai_add_example(flash_bootloader src/flash_bootloader.cpp OFF)
 dai_add_example(edge_detector src/edge_detector.cpp ON)
 
+dai_add_example(feature_tracker src/feature_tracker.cpp ON)
+
 # Calibration Read and write samples
 dai_add_example(calibration_flash src/calibration_flash.cpp OFF)
 dai_add_example(calibration_flash_version5 src/calibration_flash_v5.cpp OFF)

examples/src/feature_tracker.cpp

@@ -0,0 +1,158 @@
+#include <iostream>
+
+// Inludes common necessary includes for development using depthai library
+#include "depthai/depthai.hpp"
+#include "unordered_map"
+#include "unordered_set"
+
+static const auto lineColor = cv::Scalar(200, 0, 200);
+static const auto pointColor = cv::Scalar(0, 0, 255);
+
+class FeatureTrackerDrawer {
+   private:
+    static const int circleRadius = 2;
+    static const int maxTrackedFeaturesPathLength = 30;
+    // for how many frames the feature is tracked
+    int trackedFeaturesPathLength = 10;
+
+    using featureIdType = decltype(dai::Point2f::x);
+
+    std::unordered_set<featureIdType> trackedIDs;
+    std::unordered_map<featureIdType, std::deque<dai::Point2f>> trackedFeaturesPath;
+
+   public:
+    void trackFeaturePath(std::vector<dai::TrackedFeatures>& features) {
+        std::unordered_set<featureIdType> newTrackedIDs;
+        for(auto& currentFeature : features) {
+            auto currentID = currentFeature.id;
+            newTrackedIDs.insert(currentID);
+
+            if(!trackedFeaturesPath.count(currentID)) {
+                trackedFeaturesPath.insert({currentID, std::deque<dai::Point2f>()});
+            }
+            std::deque<dai::Point2f>& path = trackedFeaturesPath.at(currentID);
+
+            path.push_back(currentFeature.position);
+            while(path.size() > std::max(1, trackedFeaturesPathLength)) {
+                path.pop_front();
+            }
+        }
+
+        std::unordered_set<featureIdType> featuresToRemove;
+        for(auto& oldId : trackedIDs) {
+            if(!newTrackedIDs.count(oldId)) {
+                featuresToRemove.insert(oldId);
+            }
+        }
+
+        for(auto& id : featuresToRemove) {
+            trackedFeaturesPath.erase(id);
+        }
+
+        trackedIDs = newTrackedIDs;
+    }
+
+    void drawFeatures(cv::Mat& img) {
+        // For every feature,
+        for(auto& featurePath : trackedFeaturesPath) {
+            std::deque<dai::Point2f>& path = featurePath.second;
+            int j = 0;
+            // Draw the feature movement path.
+            for(j = 0; j < path.size() - 1; j++) {
+                auto src = cv::Point(path[j].x, path[j].y);
+                auto dst = cv::Point(path[j + 1].x, path[j + 1].y);
+                cv::line(img, src, dst, lineColor, 1, cv::LINE_AA, 0);
+            }
+
+            cv::circle(img, cv::Point(path[j].x, path[j].y), circleRadius, pointColor, -1, cv::LINE_AA, 0);
+        }
+    }
+
+    FeatureTrackerDrawer(std::string trackbarName, std::string windowName) {
+        cv::namedWindow(windowName.c_str());
+        cv::createTrackbar(trackbarName.c_str(), windowName.c_str(), &trackedFeaturesPathLength, maxTrackedFeaturesPathLength, nullptr);
+    }
+};
+
+int main() {
+    using namespace std;
+
+    // Create pipeline
+    dai::Pipeline pipeline;
+
+    // Define sources and outputs
+    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
+    auto monoRight = pipeline.create<dai::node::MonoCamera>();
+    auto featureTrackerLeft = pipeline.create<dai::node::FeatureTracker>();
+    auto featureTrackerRight = pipeline.create<dai::node::FeatureTracker>();
+
+    auto xoutPassthroughFrameLeft = pipeline.create<dai::node::XLinkOut>();
+    auto xoutTrackedFeaturesLeft = pipeline.create<dai::node::XLinkOut>();
+    auto xoutPassthroughFrameRight = pipeline.create<dai::node::XLinkOut>();
+    auto xoutTrackedFeaturesRight = pipeline.create<dai::node::XLinkOut>();
+
+    xoutPassthroughFrameLeft->setStreamName("passthroughFrameLeft");
+    xoutTrackedFeaturesLeft->setStreamName("trackedFeaturesLeft");
+    xoutPassthroughFrameRight->setStreamName("passthroughFrameRight");
+    xoutTrackedFeaturesRight->setStreamName("trackedFeaturesRight");
+
+    // Properties
+    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);
+
+    // Linking
+    monoLeft->out.link(featureTrackerLeft->inputImage);
+    featureTrackerLeft->passthroughInputImage.link(xoutPassthroughFrameLeft->input);
+    featureTrackerLeft->outputFeatures.link(xoutTrackedFeaturesLeft->input);
+
+    monoRight->out.link(featureTrackerRight->inputImage);
+    featureTrackerRight->passthroughInputImage.link(xoutPassthroughFrameRight->input);
+    featureTrackerRight->outputFeatures.link(xoutTrackedFeaturesRight->input);
+
+    // Connect to device and start pipeline
+    dai::Device device(pipeline);
+
+    // Output queues used to receive the results
+    auto passthroughImageLeftQueue = device.getOutputQueue("passthroughFrameLeft", 8, false);
+    auto outputFeaturesLeftQueue = device.getOutputQueue("trackedFeaturesLeft", 8, false);
+    auto passthroughImageRightQueue = device.getOutputQueue("passthroughFrameRight", 8, false);
+    auto outputFeaturesRightQueue = device.getOutputQueue("trackedFeaturesRight", 8, false);
+
+    const auto leftWindowName = "left";
+    auto leftFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", leftWindowName);
+
+    const auto rightWindowName = "right";
+    auto rightFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", rightWindowName);
+
+    while(true) {
+        auto inPassthroughFrameLeft = passthroughImageLeftQueue->get<dai::ImgFrame>();
+        cv::Mat passthroughFrameLeft = inPassthroughFrameLeft->getFrame();
+        cv::Mat leftFrame;
+        cv::cvtColor(passthroughFrameLeft, leftFrame, cv::COLOR_GRAY2BGR);
+
+        auto inPassthroughFrameRight = passthroughImageRightQueue->get<dai::ImgFrame>();
+        cv::Mat passthroughFrameRight = inPassthroughFrameRight->getFrame();
+        cv::Mat rightFrame;
+        cv::cvtColor(passthroughFrameRight, rightFrame, cv::COLOR_GRAY2BGR);
+
+        auto trackedFeaturesLeft = outputFeaturesLeftQueue->get<dai::FeatureTrackerData>()->trackedFeatures;
+        leftFeatureDrawer.trackFeaturePath(trackedFeaturesLeft);
+        leftFeatureDrawer.drawFeatures(leftFrame);
+
+        auto trackedFeaturesRight = outputFeaturesRightQueue->get<dai::FeatureTrackerData>()->trackedFeatures;
+        rightFeatureDrawer.trackFeaturePath(trackedFeaturesRight);
+        rightFeatureDrawer.drawFeatures(rightFrame);
+
+        // Show the frame
+        cv::imshow(leftWindowName, leftFrame);
+        cv::imshow(rightWindowName, rightFrame);
+
+        int key = cv::waitKey(1);
+        if(key == 'q') {
+            break;
+        }
+    }
+    return 0;
+}

include/depthai/pipeline/datatype/FeatureTrackerConfig.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <unordered_map>
+#include <vector>
+
+#include "depthai-shared/datatype/RawFeatureTrackerConfig.hpp"
+#include "depthai/pipeline/datatype/Buffer.hpp"
+
+namespace dai {
+
+/**
+ * FeatureTrackerConfig message. Carries ROI (region of interest) and threshold for depth calculation
+ */
+class FeatureTrackerConfig : public Buffer {
+    std::shared_ptr<RawBuffer> serialize() const override;
+    RawFeatureTrackerConfig& cfg;
+
+   public:
+    /**
+     * Construct FeatureTrackerConfig message.
+     */
+    FeatureTrackerConfig();
+    explicit FeatureTrackerConfig(std::shared_ptr<RawFeatureTrackerConfig> ptr);
+    virtual ~FeatureTrackerConfig() = default;
+
+    /**
+     * Retrieve configuration data for FeatureTracker
+     * @returns Vector of configuration parameters for ROIs (region of interests)
+     */
+    FeatureTrackerConfigData getConfigData() const;
+};
+
+}  // namespace dai

include/depthai/pipeline/datatype/FeatureTrackerData.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <unordered_map>
+#include <vector>
+
+#include "depthai-shared/datatype/RawTrackedFeatures.hpp"
+#include "depthai/pipeline/datatype/Buffer.hpp"
+
+namespace dai {
+
+/**
+ * FeatureTrackerData message. Carries spatial information (X,Y,Z) and their configuration parameters
+ */
+class FeatureTrackerData : public Buffer {
+    std::shared_ptr<RawBuffer> serialize() const override;
+    RawTrackedFeatures& rawdata;
+
+   public:
+    /**
+     * Construct FeatureTrackerData message.
+     */
+    FeatureTrackerData();
+    explicit FeatureTrackerData(std::shared_ptr<RawTrackedFeatures> ptr);
+    virtual ~FeatureTrackerData() = default;
+
+    std::vector<TrackedFeatures>& trackedFeatures;
+};
+
+}  // namespace dai

include/depthai/pipeline/datatypes.hpp

@@ -4,6 +4,8 @@
 #include "datatype/ADatatype.hpp"
 #include "datatype/Buffer.hpp"
 #include "datatype/CameraControl.hpp"
+#include "datatype/FeatureTrackerConfig.hpp"
+#include "datatype/FeatureTrackerData.hpp"
 #include "datatype/IMUData.hpp"
 #include "datatype/ImageManipConfig.hpp"
 #include "datatype/ImgDetections.hpp"
@@ -13,4 +15,4 @@
 #include "datatype/SpatialLocationCalculatorConfig.hpp"
 #include "datatype/SpatialLocationCalculatorData.hpp"
 #include "datatype/SystemInformation.hpp"
-#include "datatype/Tracklets.hpp"
+#include "datatype/Tracklets.hpp"

include/depthai/pipeline/node/EdgeDetector.hpp

@@ -52,6 +52,11 @@ class EdgeDetector : public Node {
      */
     Output outputImage{*this, "outputImage", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}};
 
+    /**
+     * Passthrough message on which the calculation was performed.
+     */
+    Output passthroughInputImage{*this, "passthroughInputImage", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}};
+
     // Functions to set properties
     /**
      * Specify whether or not wait until configuration message arrives to inputConfig Input.

include/depthai/pipeline/node/FeatureTracker.hpp

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <depthai/pipeline/Node.hpp>
+
+// standard
+#include <fstream>
+
+// shared
+#include <depthai-shared/properties/FeatureTrackerProperties.hpp>
+
+#include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp"
+
+namespace dai {
+namespace node {
+
+/**
+ * @brief FeatureTracker node. Calculates spatial location data on a set of ROIs on depth map.
+ */
+class FeatureTracker : public Node {
+   public:
+    using Properties = dai::FeatureTrackerProperties;
+
+   private:
+    std::string getName() const override;
+    nlohmann::json getProperties() override;
+    std::shared_ptr<Node> clone() override;
+
+    std::shared_ptr<RawFeatureTrackerConfig> rawConfig;
+    Properties properties;
+
+   public:
+    FeatureTracker(const std::shared_ptr<PipelineImpl>& par, int64_t nodeId);
+
+    /**
+     * Initial config to use for feature tracking.
+     */
+    FeatureTrackerConfig initialConfig;
+
+    /**
+     * Input FeatureTrackerConfig message with ability to modify parameters in runtime.
+     * Default queue is non-blocking with size 4.
+     */
+    Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::FeatureTrackerConfig, false}}};
+    /**
+     * Input message with frame data on which feature tracking is performed.
+     * Default queue is non-blocking with size 4.
+     */
+    Input inputImage{*this, "inputImage", Input::Type::SReceiver, false, 4, {{DatatypeEnum::ImgFrame, false}}};
+
+    /**
+     * Outputs FeatureTrackerData message that carries tracked features results.
+     */
+    Output outputFeatures{*this, "outputFeatures", Output::Type::MSender, {{DatatypeEnum::FeatureTrackerData, false}}};
+
+    /**
+     * Passthrough message on which the calculation was performed.
+     * Suitable for when input queue is set to non-blocking behavior.
+     */
+    Output passthroughInputImage{*this, "passthroughInputImage", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}};
+
+    // Functions to set properties
+    /**
+     * Specify whether or not wait until configuration message arrives to inputConfig Input.
+     * @param wait True to wait for configuration message, false otherwise.
+     */
+    void setWaitForConfigInput(bool wait);
+};
+
+}  // namespace node
+}  // namespace dai

include/depthai/pipeline/nodes.hpp

@@ -4,6 +4,7 @@
 #include "node/ColorCamera.hpp"
 #include "node/DetectionNetwork.hpp"
 #include "node/EdgeDetector.hpp"
+#include "node/FeatureTracker.hpp"
 #include "node/IMU.hpp"
 #include "node/ImageManip.hpp"
 #include "node/MonoCamera.hpp"

src/pipeline/datatype/FeatureTrackerConfig.cpp

@@ -0,0 +1,17 @@
+#include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp"
+
+namespace dai {
+
+std::shared_ptr<RawBuffer> FeatureTrackerConfig::serialize() const {
+    return raw;
+}
+
+FeatureTrackerConfig::FeatureTrackerConfig() : Buffer(std::make_shared<RawFeatureTrackerConfig>()), cfg(*dynamic_cast<RawFeatureTrackerConfig*>(raw.get())) {}
+FeatureTrackerConfig::FeatureTrackerConfig(std::shared_ptr<RawFeatureTrackerConfig> ptr)
+    : Buffer(std::move(ptr)), cfg(*dynamic_cast<RawFeatureTrackerConfig*>(raw.get())) {}
+
+FeatureTrackerConfigData FeatureTrackerConfig::getConfigData() const {
+    return cfg.config;
+}
+
+}  // namespace dai