diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b08979b5..9a3b62621 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -254,6 +254,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/node/EdgeDetector.cpp src/pipeline/node/SPIIn.cpp src/pipeline/node/FeatureTracker.cpp + src/pipeline/node/FeatureTrackerRvc4.cpp src/pipeline/node/ToF.cpp src/pipeline/node/DetectionParser.cpp src/pipeline/node/test/MyProducer.cpp @@ -286,6 +287,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/datatype/EdgeDetectorConfig.cpp src/pipeline/datatype/TrackedFeatures.cpp src/pipeline/datatype/FeatureTrackerConfig.cpp + src/pipeline/datatype/FeatureTrackerConfigRvc4.cpp src/pipeline/datatype/ToFConfig.cpp src/pipeline/datatype/BenchmarkReport.cpp src/pipeline/datatype/PointCloudConfig.cpp diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 6f100d2d5..e08af2286 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -95,6 +95,7 @@ set(SOURCE_LIST src/pipeline/node/IMUBindings.cpp src/pipeline/node/EdgeDetectorBindings.cpp src/pipeline/node/FeatureTrackerBindings.cpp + src/pipeline/node/FeatureTrackerBindingsRvc4.cpp src/pipeline/node/ToFBindings.cpp src/pipeline/node/AprilTagBindings.cpp src/pipeline/node/DetectionParserBindings.cpp @@ -117,6 +118,7 @@ set(SOURCE_LIST src/pipeline/datatype/CameraControlBindings.cpp src/pipeline/datatype/EdgeDetectorConfigBindings.cpp src/pipeline/datatype/FeatureTrackerConfigBindings.cpp + src/pipeline/datatype/FeatureTrackerConfigBindingsRvc4.cpp src/pipeline/datatype/ToFConfigBindings.cpp src/pipeline/datatype/ImageManipConfigBindings.cpp src/pipeline/datatype/ImgDetectionsBindings.cpp diff --git a/bindings/python/src/DatatypeBindings.cpp b/bindings/python/src/DatatypeBindings.cpp index 485ff4732..72113f4bd 100644 --- a/bindings/python/src/DatatypeBindings.cpp +++ b/bindings/python/src/DatatypeBindings.cpp @@ -10,6 +10,7 @@ void bind_buffer(pybind11::module& m, void* pCallstack); void bind_cameracontrol(pybind11::module& m, void* pCallstack); void bind_edgedetectorconfig(pybind11::module& m, void* pCallstack); void bind_featuretrackerconfig(pybind11::module& m, void* pCallstack); +void bind_featuretrackerconfigRvc4(pybind11::module& m, void* pCallstack); void bind_tofconfig(pybind11::module& m, void* pCallstack); void bind_imagemanipconfig(pybind11::module& m, void* pCallstack); void bind_imgdetections(pybind11::module& m, void* pCallstack); @@ -42,6 +43,7 @@ void DatatypeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_cameracontrol); callstack.push_front(bind_edgedetectorconfig); callstack.push_front(bind_featuretrackerconfig); + callstack.push_front(bind_featuretrackerconfigRvc4); callstack.push_front(bind_tofconfig); callstack.push_front(bind_imagemanipconfig); callstack.push_front(bind_imgdetections); @@ -102,6 +104,7 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){ .value("IMUData", DatatypeEnum::IMUData) .value("StereoDepthConfig", DatatypeEnum::StereoDepthConfig) .value("FeatureTrackerConfig", DatatypeEnum::FeatureTrackerConfig) + .value("FeatureTrackerConfigRvc4", DatatypeEnum::FeatureTrackerConfigRvc4) .value("TrackedFeatures", DatatypeEnum::TrackedFeatures) .value("PointCloudConfig", DatatypeEnum::PointCloudConfig) .value("PointCloudData", DatatypeEnum::PointCloudData) diff --git a/bindings/python/src/pipeline/PipelineBindings.cpp b/bindings/python/src/pipeline/PipelineBindings.cpp index 3fc0c6eee..d5116c34c 100644 --- a/bindings/python/src/pipeline/PipelineBindings.cpp +++ b/bindings/python/src/pipeline/PipelineBindings.cpp @@ -33,6 +33,7 @@ #include "depthai/pipeline/node/IMU.hpp" #include "depthai/pipeline/node/EdgeDetector.hpp" #include "depthai/pipeline/node/FeatureTracker.hpp" +#include "depthai/pipeline/node/FeatureTrackerRvc4.hpp" #include "depthai/pipeline/node/AprilTag.hpp" #include "depthai/pipeline/node/DetectionParser.hpp" #include "depthai/pipeline/node/UVC.hpp" @@ -207,6 +208,7 @@ void PipelineBindings::bind(pybind11::module& m, void* pCallstack){ .def("createIMU", &Pipeline::create) .def("createEdgeDetector", &Pipeline::create) .def("createFeatureTracker", &Pipeline::create) + .def("createFeatureTrackerRvc4", &Pipeline::create) .def("createAprilTag", &Pipeline::create) .def("createDetectionParser", &Pipeline::create) .def("createUVC", &Pipeline::create) diff --git a/bindings/python/src/pipeline/datatype/FeatureTrackerConfigBindingsRvc4.cpp b/bindings/python/src/pipeline/datatype/FeatureTrackerConfigBindingsRvc4.cpp new file mode 100644 index 000000000..c79156310 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/FeatureTrackerConfigBindingsRvc4.cpp @@ -0,0 +1,126 @@ +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" +#include +#include + +// depthai +#include "depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp" + +//pybind +#include +#include + +// #include "spdlog/spdlog.h" + +void bind_featuretrackerconfigRvc4(pybind11::module& m, void* pCallstack){ + + using namespace dai; + + // py::class_> rawFeatureTrackerConfig(m, "RawFeatureTrackerConfig", DOC(dai, RawFeatureTrackerConfig)); + py::class_, Buffer, std::shared_ptr> featureTrackerConfigRvc4(m, "FeatureTrackerConfigRvc4", DOC(dai, FeatureTrackerConfigRvc4)); + py::class_ cornerDetector(featureTrackerConfigRvc4, "CornerDetector", DOC(dai, FeatureTrackerConfigRvc4, CornerDetector)); + py::enum_ cornerDetectorType(cornerDetector, "Type", DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, Type)); + py::class_ cornerDetectorThresholds(cornerDetector, "Thresholds", DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, Thresholds)); + py::class_ motionEstimator(featureTrackerConfigRvc4, "MotionEstimator", DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator)); + py::enum_ motionEstimatorType(motionEstimator, "Type", DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, Type)); + py::class_ motionEstimatorOpticalFlow(motionEstimator, "OpticalFlow", DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, OpticalFlow)); + py::class_ featureMaintainer(featureTrackerConfigRvc4, "FeatureMaintainer", DOC(dai, FeatureTrackerConfigRvc4, FeatureMaintainer)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Call the rest of the type defines, then perform the actual bindings + Callstack* callstack = (Callstack*) pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + // Actual bindings + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + // // Metadata / raw + // rawFeatureTrackerConfig + // .def(py::init<>()) + // .def_readwrite("cornerDetector", &FeatureTrackerConfigRvc4::cornerDetector, DOC(dai, FeatureTrackerConfigRvc4, cornerDetector)) + // .def_readwrite("motionEstimator", &FeatureTrackerConfigRvc4::motionEstimator, DOC(dai, FeatureTrackerConfigRvc4, motionEstimator)) + // .def_readwrite("featureMaintainer", &FeatureTrackerConfigRvc4::featureMaintainer, DOC(dai, FeatureTrackerConfigRvc4, featureMaintainer)) + // ; + + cornerDetectorType + .value("HARRIS", FeatureTrackerConfigRvc4::CornerDetector::Type::HARRIS) + ; + + cornerDetectorThresholds + .def(py::init<>()) + .def_readwrite("harrisScore", &FeatureTrackerConfigRvc4::CornerDetector::Thresholds::harrisScore, DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, Thresholds, harrisScore)) + .def_readwrite("robustness", &FeatureTrackerConfigRvc4::CornerDetector::Thresholds::robustness, DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, Thresholds, robustness)) + ; + + cornerDetector + .def(py::init<>()) + .def_readwrite("type", &FeatureTrackerConfigRvc4::CornerDetector::type, DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, type)) + .def_readwrite("numMaxFeatures", &FeatureTrackerConfigRvc4::CornerDetector::numMaxFeatures, DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, numMaxFeatures)) + .def_readwrite("gradientFilterShift", &FeatureTrackerConfigRvc4::CornerDetector::gradientFilterShift, DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, gradientFilterShift)) + .def_readwrite("descriptorLpf", &FeatureTrackerConfigRvc4::CornerDetector::descriptorLpf, DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, descriptorLpf)) + .def_readwrite("thresholds", &FeatureTrackerConfigRvc4::CornerDetector::thresholds, DOC(dai, FeatureTrackerConfigRvc4, CornerDetector, thresholds)) + ; + + motionEstimatorType + .value("HW_MOTION_ESTIMATION", FeatureTrackerConfigRvc4::MotionEstimator::Type::HW_MOTION_ESTIMATION) + ; + + motionEstimatorOpticalFlow + .def(py::init<>()) + .def_readwrite("pyramidLevels", &FeatureTrackerConfigRvc4::MotionEstimator::OpticalFlow::pyramidLevels, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, OpticalFlow, pyramidLevels)) + .def_readwrite("searchWindowWidth", &FeatureTrackerConfigRvc4::MotionEstimator::OpticalFlow::searchWindowWidth, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, OpticalFlow, searchWindowWidth)) + .def_readwrite("searchWindowHeight", &FeatureTrackerConfigRvc4::MotionEstimator::OpticalFlow::searchWindowHeight, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, OpticalFlow, searchWindowHeight)) + .def_readwrite("epsilon", &FeatureTrackerConfigRvc4::MotionEstimator::OpticalFlow::epsilon, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, OpticalFlow, epsilon)) + .def_readwrite("maxIterations", &FeatureTrackerConfigRvc4::MotionEstimator::OpticalFlow::maxIterations, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, OpticalFlow, maxIterations)) + ; + + motionEstimator + .def(py::init<>()) + .def_readwrite("enable", &FeatureTrackerConfigRvc4::MotionEstimator::enable, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, enable)) + .def_readwrite("type", &FeatureTrackerConfigRvc4::MotionEstimator::type, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, type)) + .def_readwrite("opticalFlow", &FeatureTrackerConfigRvc4::MotionEstimator::opticalFlow, DOC(dai, FeatureTrackerConfigRvc4, MotionEstimator, opticalFlow)) + ; + + featureMaintainer + .def(py::init<>()) + .def_readwrite("enable", &FeatureTrackerConfigRvc4::FeatureMaintainer::enable, DOC(dai, FeatureTrackerConfigRvc4, FeatureMaintainer, enable)) + .def_readwrite("minimumDistanceBetweenFeatures", &FeatureTrackerConfigRvc4::FeatureMaintainer::minimumDistanceBetweenFeatures, DOC(dai, FeatureTrackerConfigRvc4, FeatureMaintainer, minimumDistanceBetweenFeatures)) + .def_readwrite("lostFeatureErrorThreshold", &FeatureTrackerConfigRvc4::FeatureMaintainer::lostFeatureErrorThreshold, DOC(dai, FeatureTrackerConfigRvc4, FeatureMaintainer, lostFeatureErrorThreshold)) + .def_readwrite("trackedFeatureThreshold", &FeatureTrackerConfigRvc4::FeatureMaintainer::trackedFeatureThreshold, DOC(dai, FeatureTrackerConfigRvc4, FeatureMaintainer, trackedFeatureThreshold)) + .def_readwrite("confidenceThreshold", &FeatureTrackerConfigRvc4::FeatureMaintainer::confidenceThreshold, DOC(dai, FeatureTrackerConfigRvc4, FeatureMaintainer, confidenceThreshold)) + ; + + // Message + featureTrackerConfigRvc4 + .def(py::init<>()) + // .def(py::init>()) + + .def("setCornerDetector", static_cast(&FeatureTrackerConfigRvc4::setCornerDetector), py::arg("cornerDetector"), DOC(dai, FeatureTrackerConfigRvc4, setCornerDetector)) + .def("setCornerDetector", static_cast(&FeatureTrackerConfigRvc4::setCornerDetector), py::arg("config"), DOC(dai, FeatureTrackerConfigRvc4, setCornerDetector, 2)) + .def("setMotionEstimator", static_cast(&FeatureTrackerConfigRvc4::setMotionEstimator), py::arg("enable"), DOC(dai, FeatureTrackerConfigRvc4, setMotionEstimator)) + .def("setMotionEstimator", static_cast(&FeatureTrackerConfigRvc4::setMotionEstimator), py::arg("config"), DOC(dai, FeatureTrackerConfigRvc4, setMotionEstimator, 2)) + .def("setHwMotionEstimation", &FeatureTrackerConfigRvc4::setHwMotionEstimation, DOC(dai, FeatureTrackerConfigRvc4, setHwMotionEstimation)) + .def("setFeatureMaintainer", static_cast(&FeatureTrackerConfigRvc4::setFeatureMaintainer), py::arg("enable"), DOC(dai, FeatureTrackerConfigRvc4, setFeatureMaintainer)) + .def("setFeatureMaintainer", static_cast(&FeatureTrackerConfigRvc4::setFeatureMaintainer), py::arg("config"), DOC(dai, FeatureTrackerConfigRvc4, setFeatureMaintainer, 2)) + .def("setNumMaxFeatures", &FeatureTrackerConfigRvc4::setNumMaxFeatures, py::arg("numMaxFeatures"), DOC(dai, FeatureTrackerConfigRvc4, setNumMaxFeatures)) + .def("setHarrisCornerDetectorThreshold", &FeatureTrackerConfigRvc4::setHarrisCornerDetectorThreshold, py::arg("cornerDetectorThreshold"), DOC(dai, FeatureTrackerConfigRvc4, setHarrisCornerDetectorThreshold)) + .def("setHarrisCornerDetectorRobustness", &FeatureTrackerConfigRvc4::setHarrisCornerDetectorRobustness, py::arg("cornerDetectorRobustness"), DOC(dai, FeatureTrackerConfigRvc4, setHarrisCornerDetectorRobustness)) + .def("setConfidence", &FeatureTrackerConfigRvc4::setConfidence, py::arg("confidenceThreshold"), DOC(dai, FeatureTrackerConfigRvc4, setConfidence)) + .def("setFilterCoeffs", &FeatureTrackerConfigRvc4::setFilterCoeffs, py::arg("filterCoeffs"), DOC(dai, FeatureTrackerConfigRvc4, setFilterCoeffs)) + + + // .def("set", &FeatureTrackerConfigRvc4::set, py::arg("config"), DOC(dai, FeatureTrackerConfigRvc4, set)) + // .def("get", &FeatureTrackerConfigRvc4::get, DOC(dai, FeatureTrackerConfigRvc4, get)) + ; + + // add aliases + m.attr("FeatureTrackerConfigRvc4").attr("CornerDetector") = m.attr("FeatureTrackerConfigRvc4").attr("CornerDetector"); + m.attr("FeatureTrackerConfigRvc4").attr("MotionEstimator") = m.attr("FeatureTrackerConfigRvc4").attr("MotionEstimator"); + m.attr("FeatureTrackerConfigRvc4").attr("FeatureMaintainer") = m.attr("FeatureTrackerConfigRvc4").attr("FeatureMaintainer"); + +} diff --git a/bindings/python/src/pipeline/node/FeatureTrackerBindingsRvc4.cpp b/bindings/python/src/pipeline/node/FeatureTrackerBindingsRvc4.cpp new file mode 100644 index 000000000..cff7d5400 --- /dev/null +++ b/bindings/python/src/pipeline/node/FeatureTrackerBindingsRvc4.cpp @@ -0,0 +1,65 @@ +#include "NodeBindings.hpp" +#include "Common.hpp" + +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/node/FeatureTrackerRvc4.hpp" + + +void bind_featuretrackerRvc4(pybind11::module& m, void* pCallstack){ + + using namespace dai; + using namespace dai::node; + + // Node and Properties declare upfront + py::class_ featureTrackerProperties(m, "FeatureTrackerPropertiesRvc4", DOC(dai, FeatureTrackerPropertiesRvc4)); + auto featureTracker = ADD_NODE(FeatureTrackerRvc4); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Call the rest of the type defines, then perform the actual bindings + Callstack* callstack = (Callstack*) pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + // Actual bindings + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + // FeatureTrackerRvc4 properties + featureTrackerProperties + .def_readwrite("initialConfig", &FeatureTrackerPropertiesRvc4::initialConfig, DOC(dai, FeatureTrackerPropertiesRvc4, initialConfig)) + ; + + // FeatureTrackerRvc4 Node + featureTracker + .def_readonly("inputConfig", &FeatureTrackerRvc4::inputConfig, DOC(dai, node, FeatureTrackerRvc4, inputConfig)) + .def_readonly("inputImage", &FeatureTrackerRvc4::inputImage, DOC(dai, node, FeatureTrackerRvc4, inputImage)) + .def_readonly("outputFeatures", &FeatureTrackerRvc4::outputFeatures, DOC(dai, node, FeatureTrackerRvc4, outputFeatures)) + .def_readonly("passthroughInputImage", &FeatureTrackerRvc4::passthroughInputImage, DOC(dai, node, FeatureTrackerRvc4, passthroughInputImage)) + .def_readonly("initialConfig", &FeatureTrackerRvc4::initialConfig, DOC(dai, node, FeatureTrackerRvc4, initialConfig)) + + .def("setWaitForConfigInput", [](FeatureTrackerRvc4& obj, bool wait){ + // Issue a deprecation warning + PyErr_WarnEx(PyExc_DeprecationWarning, "Use 'inputConfig.setWaitForMessage()' instead", 1); + HEDLEY_DIAGNOSTIC_PUSH + HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED + obj.setWaitForConfigInput(wait); + HEDLEY_DIAGNOSTIC_POP + }, py::arg("wait"), DOC(dai, node, FeatureTrackerRvc4, setWaitForConfigInput)) + + .def("getWaitForConfigInput", [](FeatureTrackerRvc4& obj){ + // Issue a deprecation warning + PyErr_WarnEx(PyExc_DeprecationWarning, "Use 'inputConfig.setWaitForMessage()' instead", 1); + HEDLEY_DIAGNOSTIC_PUSH + HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED + return obj.getWaitForConfigInput(); + HEDLEY_DIAGNOSTIC_POP + }, DOC(dai, node, FeatureTrackerRvc4, getWaitForConfigInput)) + + ; + daiNodeModule.attr("FeatureTrackerRvc4").attr("Properties") = featureTrackerProperties; + +} diff --git a/bindings/python/src/pipeline/node/NodeBindings.cpp b/bindings/python/src/pipeline/node/NodeBindings.cpp index 237deab3b..37c2ddd56 100644 --- a/bindings/python/src/pipeline/node/NodeBindings.cpp +++ b/bindings/python/src/pipeline/node/NodeBindings.cpp @@ -143,6 +143,7 @@ void bind_objecttracker(pybind11::module& m, void* pCallstack); void bind_imu(pybind11::module& m, void* pCallstack); void bind_edgedetector(pybind11::module& m, void* pCallstack); void bind_featuretracker(pybind11::module& m, void* pCallstack); +void bind_featuretrackerRvc4(pybind11::module& m, void* pCallstack); void bind_apriltag(pybind11::module& m, void* pCallstack); void bind_detectionparser(pybind11::module& m, void* pCallstack); void bind_uvc(pybind11::module& m, void* pCallstack); @@ -181,6 +182,7 @@ void NodeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_imu); callstack.push_front(bind_edgedetector); callstack.push_front(bind_featuretracker); + callstack.push_front(bind_featuretrackerRvc4); callstack.push_front(bind_apriltag); callstack.push_front(bind_detectionparser); callstack.push_front(bind_uvc); diff --git a/bindings/python/tests/inherited_messages_test.py b/bindings/python/tests/inherited_messages_test.py index 668dbd38e..131d00f30 100644 --- a/bindings/python/tests/inherited_messages_test.py +++ b/bindings/python/tests/inherited_messages_test.py @@ -9,6 +9,7 @@ dai.CameraControl, dai.EdgeDetectorConfig, dai.FeatureTrackerConfig, + dai.FeatureTrackerConfigRvc4, dai.ImageManipConfig, dai.ImgDetections, dai.ImgFrame, diff --git a/include/depthai/pipeline/datatype/DatatypeEnum.hpp b/include/depthai/pipeline/datatype/DatatypeEnum.hpp index d5f6d0b92..96593ddbe 100644 --- a/include/depthai/pipeline/datatype/DatatypeEnum.hpp +++ b/include/depthai/pipeline/datatype/DatatypeEnum.hpp @@ -25,6 +25,7 @@ enum class DatatypeEnum : std::int32_t { IMUData, StereoDepthConfig, FeatureTrackerConfig, + FeatureTrackerConfigRvc4, ToFConfig, TrackedFeatures, BenchmarkReport, diff --git a/include/depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp b/include/depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp new file mode 100644 index 000000000..f011a44a6 --- /dev/null +++ b/include/depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp @@ -0,0 +1,293 @@ +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +/** + * FeatureTrackerConfigRvc4 message. Carries config for feature tracking algorithm + */ +class FeatureTrackerConfigRvc4 : public Buffer { + public: + /** + * Construct FeatureTrackerConfigRvc4 message. + */ + FeatureTrackerConfigRvc4() = default; + virtual ~FeatureTrackerConfigRvc4() = default; + + static constexpr const std::int32_t AUTO = 0; + + /** + * Corner detector configuration structure. + */ + struct CornerDetector { + enum class Type : std::int32_t { + /** + * HARRIS corner detector. + */ + HARRIS + }; + /** + * Corner detector algorithm type. + */ + Type type = Type::HARRIS; + + /** + * Hard limit for the maximum number of features that can be detected. + */ + std::int32_t numMaxFeatures = 1024; + + /** + * Gradient filter shift + * It is one of the HCD HW exposed configs + */ + std::int32_t gradientFilterShift = 1; + + /* + * Descriptor LPF + */ + bool descriptorLpf = true; + + std::array gradientFilterCoeffsX_3 = {1, 2, 1}; + std::array gradientFilterCoeffsY_3 = {1, 2, 1}; + std::array lowPassFilterCoeffs_3 = {1, 4, 6}; + + std::array, 3> filterCoeffs = {{{1, 2, 1}, {1, 2, 1}, {1, 4, 6}}}; + + /** + * Threshold settings structure for corner detector. + */ + struct Thresholds { + float harrisScore = 25000; + float robustness = 10; + + DEPTHAI_SERIALIZE(Thresholds, harrisScore, robustness); + }; + + /** + * Threshold settings. + * These are advanced settings, suitable for debugging/special cases. + */ + Thresholds thresholds; + + DEPTHAI_SERIALIZE(CornerDetector, type, numMaxFeatures, gradientFilterShift, thresholds, filterCoeffs, descriptorLpf); + }; + + /** + * Used for feature reidentification between current and previous features. + */ + struct MotionEstimator { + /** + * Enable motion estimation or not. + */ + bool enable = false; + + enum class Type : std::int32_t { + /** + * Using a dense motion estimation hardware block (Block matcher). + */ + HW_MOTION_ESTIMATION + }; + /** + * Motion estimator algorithm type. + */ + Type type = Type::HW_MOTION_ESTIMATION; + + /** + * Optical flow configuration structure. + */ + struct OpticalFlow { + /** + * Number of pyramid levels, only for optical flow. + * AUTO means it's decided based on input resolution: 3 if image width <= 640, else 4. + * Valid values are either 3/4 for VGA, 4 for 720p and above. + */ + std::int32_t pyramidLevels = AUTO; + + /** + * Image patch width used to track features. + * Must be an odd number, maximum 9. + * N means the algorithm will be able to track motion at most (N-1)/2 pixels in a direction per pyramid level. + * Increasing this number increases runtime + */ + std::int32_t searchWindowWidth = 5; + /** + * Image patch height used to track features. + * Must be an odd number, maximum 9. + * N means the algorithm will be able to track motion at most (N-1)/2 pixels in a direction per pyramid level. + * Increasing this number increases runtime + */ + std::int32_t searchWindowHeight = 5; + + /** + * Feature tracking termination criteria. + * Optical flow will refine the feature position on each pyramid level until + * the displacement between two refinements is smaller than this value. + * Decreasing this number increases runtime. + */ + float epsilon = 0.01f; + + /** + * Feature tracking termination criteria. Optical flow will refine the feature position maximum this many times + * on each pyramid level. If the Epsilon criteria described in the previous chapter is not met after this number + * of iterations, the algorithm will continue with the current calculated value. + * Increasing this number increases runtime. + */ + std::int32_t maxIterations = 9; + + DEPTHAI_SERIALIZE(OpticalFlow, pyramidLevels, searchWindowWidth, searchWindowHeight, epsilon, maxIterations); + }; + + /** + * Optical flow configuration. + * Takes effect only if MotionEstimator algorithm type set to LUCAS_KANADE_OPTICAL_FLOW. + */ + OpticalFlow opticalFlow; + + DEPTHAI_SERIALIZE(MotionEstimator, enable, type, opticalFlow); + }; + + /** + * FeatureMaintainer configuration structure. + */ + struct FeatureMaintainer { + /** + * Enable feature maintaining or not. + */ + bool enable = true; + + /** + * Used to filter out detected feature points that are too close. + * Requires sorting enabled in detector. + * Unit of measurement is squared euclidean distance in pixels. + */ + float minimumDistanceBetweenFeatures = 50; + + /** + * Optical flow measures the tracking error for every feature. + * If the point can’t be tracked or it’s out of the image it will set this error to a maximum value. + * This threshold defines the level where the tracking accuracy is considered too bad to keep the point. + */ + float lostFeatureErrorThreshold = 50000; + + /** + * Once a feature was detected and we started tracking it, we need to update its Harris score on each image. + * This is needed because a feature point can disappear, or it can become too weak to be tracked. This + * threshold defines the point where such a feature must be dropped. + * As the goal of the algorithm is to provide longer tracks, we try to add strong points and track them until + * they are absolutely untrackable. This is why, this value is usually smaller than the detection threshold. + */ + float trackedFeatureThreshold = 200000; + + int confidenceThreshold = 100; + + DEPTHAI_SERIALIZE(FeatureMaintainer, enable, minimumDistanceBetweenFeatures, lostFeatureErrorThreshold, trackedFeatureThreshold, confidenceThreshold); + }; + + /** + * Set corner detector algorithm type. + * @param cornerDetector Corner detector type, HARRIS + */ + FeatureTrackerConfigRvc4& setCornerDetector(CornerDetector::Type cornerDetector); + + /** + * Set corner detector full configuration. + * @param config Corner detector configuration + */ + FeatureTrackerConfigRvc4& setCornerDetector(CornerDetector config); + + /** + * Set hardware accelerated motion estimation using block matching. + * Faster than optical flow (software implementation) but might not be as accurate. + */ + FeatureTrackerConfigRvc4& setHwMotionEstimation(); + + /** + * Set initial threshold for HARRIS Corner Detector + */ + FeatureTrackerConfigRvc4& setHarrisCornerDetectorThreshold(std::int32_t cornerDetectorThreshold); + + /** + * Set initial robustness for Harris Corner Detector + */ + FeatureTrackerConfigRvc4& setHarrisCornerDetectorRobustness(std::int32_t cornerDetectorRobustness); + + + /** + * Set confidence for FeatureMaintainer + */ + FeatureTrackerConfigRvc4& setConfidence(std::int32_t confidenceThreshold); + + /** + * Set filter coeficients for Feature Tracker + */ + FeatureTrackerConfigRvc4& setFilterCoeffs(std::array, 3> filterCoeffs); + + /** + * Get filter coeficients for Feature Tracker + */ + std::array, 3> getFilterCoeffs(); // TODO: add this + + /** + * Set number of target features to detect. + * @param numTargetFeatures Number of features + */ + FeatureTrackerConfigRvc4& setNumMaxFeatures(std::int32_t numMaxFeatures); + + /** + * Enable or disable motion estimator. + * @param enable + */ + FeatureTrackerConfigRvc4& setMotionEstimator(bool enable); + + /** + * Set motion estimator full configuration. + * @param config Motion estimator configuration + */ + FeatureTrackerConfigRvc4& setMotionEstimator(MotionEstimator config); + + /** + * Enable or disable feature maintainer. + * @param enable + */ + FeatureTrackerConfigRvc4& setFeatureMaintainer(bool enable); + + /** + * Set feature maintainer full configuration. + * @param config feature maintainer configuration + */ + FeatureTrackerConfigRvc4& setFeatureMaintainer(FeatureMaintainer config); + + public: + /** + * Corner detector configuration. + * Used for feature detection. + */ + CornerDetector cornerDetector; + + /** + * Motion estimator configuration. + * Used for feature reidentification between current and previous features. + */ + MotionEstimator motionEstimator; + + /** + * FeatureMaintainer configuration. + * Used for feature maintaining. + */ + FeatureMaintainer featureMaintainer; + + public: + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override { + metadata = utility::serialize(*this); + datatype = DatatypeEnum::FeatureTrackerConfigRvc4; + }; + + DEPTHAI_SERIALIZE(FeatureTrackerConfigRvc4, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, cornerDetector, motionEstimator, featureMaintainer); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatypes.hpp b/include/depthai/pipeline/datatypes.hpp index c35d2ff1f..9e2c38de4 100644 --- a/include/depthai/pipeline/datatypes.hpp +++ b/include/depthai/pipeline/datatypes.hpp @@ -11,6 +11,7 @@ #include "datatype/EdgeDetectorConfig.hpp" #include "datatype/EncodedFrame.hpp" #include "datatype/FeatureTrackerConfig.hpp" +#include "datatype/FeatureTrackerConfigRvc4.hpp" #include "datatype/IMUData.hpp" #include "datatype/ImageManipConfig.hpp" #include "datatype/ImgDetections.hpp" diff --git a/include/depthai/pipeline/node/FeatureTrackerRvc4.hpp b/include/depthai/pipeline/node/FeatureTrackerRvc4.hpp new file mode 100644 index 000000000..83d1d2f47 --- /dev/null +++ b/include/depthai/pipeline/node/FeatureTrackerRvc4.hpp @@ -0,0 +1,77 @@ +#pragma once + +#include + +// standard +#include + +// shared +#include + +#include "depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp" + +namespace dai { +namespace node { + +/** + * @brief FeatureTrackerRvc4 node. + * Performs feature tracking and reidentification using motion estimation between 2 consecutive frames. + */ +class FeatureTrackerRvc4 : public DeviceNodeCRTP { + public: + constexpr static const char* NAME = "FeatureTrackerRvc4"; + using DeviceNodeCRTP::DeviceNodeCRTP; + + protected: + Properties& getProperties(); + + public: + FeatureTrackerRvc4() = default; + FeatureTrackerRvc4(std::unique_ptr props); + + std::shared_ptr build() { + return std::static_pointer_cast(shared_from_this()); + } + /** + * Initial config to use for feature tracking. + */ + FeatureTrackerConfigRvc4 initialConfig; + + /** + * Input FeatureTrackerConfigRvc4 message with ability to modify parameters in runtime. + * Default queue is non-blocking with size 4. + */ + Input inputConfig{*this, {.name = "inputConfig", .blocking = false, .queueSize = 4, .types = {{DatatypeEnum::FeatureTrackerConfigRvc4, false}}}}; + + /** + * Input message with frame data on which feature tracking is performed. + * Default queue is non-blocking with size 4. + */ + Input inputImage{*this, {.name = "inputImage", .blocking = false, .queueSize = 4, .types = {{DatatypeEnum::ImgFrame, false}}, .waitForMessage = true}}; + + /** + * Outputs TrackedFeatures message that carries tracked features results. + */ + Output outputFeatures{*this, {.name = "outputFeatures", .types = {{DatatypeEnum::TrackedFeatures, false}}}}; + + /** + * Passthrough message on which the calculation was performed. + * Suitable for when input queue is set to non-blocking behavior. + */ + Output passthroughInputImage{*this, {.name = "passthroughInputImage", .types = {{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. + */ + [[deprecated("Use 'inputConfig.setWaitForMessage()' instead")]] void setWaitForConfigInput(bool wait); + + /** + * @see setWaitForConfigInput + * @returns True if wait for inputConfig message, false otherwise + */ + [[deprecated("Use 'inputConfig.setWaitForMessage()' instead")]] bool getWaitForConfigInput() const; +}; + +} // namespace node +} // namespace dai diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index 61a472f85..bf0349b06 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -10,6 +10,7 @@ #include "node/DetectionParser.hpp" #include "node/EdgeDetector.hpp" #include "node/FeatureTracker.hpp" +#include "node/FeatureTrackerRvc4.hpp" #include "node/IMU.hpp" #include "node/ImageManip.hpp" #include "node/MessageDemux.hpp" diff --git a/include/depthai/properties/FeatureTrackerPropertiesRvc4.hpp b/include/depthai/properties/FeatureTrackerPropertiesRvc4.hpp new file mode 100644 index 000000000..ee054ce5b --- /dev/null +++ b/include/depthai/properties/FeatureTrackerPropertiesRvc4.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +#include "depthai/common/optional.hpp" +#include "depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp" +#include "depthai/properties/Properties.hpp" + +namespace dai { + +/** + * Specify properties for FeatureTracker + */ +struct FeatureTrackerPropertiesRvc4 : PropertiesSerializable { + /** + * Initial feature tracker config + */ + FeatureTrackerConfigRvc4 initialConfig; +}; + +DEPTHAI_SERIALIZE_EXT(FeatureTrackerPropertiesRvc4, initialConfig); + +} // namespace dai diff --git a/src/pipeline/datatype/DatatypeEnum.cpp b/src/pipeline/datatype/DatatypeEnum.cpp index ac98e53ac..710ce9a4d 100644 --- a/src/pipeline/datatype/DatatypeEnum.cpp +++ b/src/pipeline/datatype/DatatypeEnum.cpp @@ -27,6 +27,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::IMUData, DatatypeEnum::StereoDepthConfig, DatatypeEnum::FeatureTrackerConfig, + DatatypeEnum::FeatureTrackerConfigRvc4, DatatypeEnum::ToFConfig, DatatypeEnum::TrackedFeatures, DatatypeEnum::AprilTagConfig, @@ -52,6 +53,7 @@ const std::unordered_map> hierarchy = { {DatatypeEnum::IMUData, {}}, {DatatypeEnum::StereoDepthConfig, {}}, {DatatypeEnum::FeatureTrackerConfig, {}}, + {DatatypeEnum::FeatureTrackerConfigRvc4, {}}, {DatatypeEnum::ToFConfig, {}}, {DatatypeEnum::TrackedFeatures, {}}, {DatatypeEnum::AprilTagConfig, {}}, diff --git a/src/pipeline/datatype/FeatureTrackerConfigRvc4.cpp b/src/pipeline/datatype/FeatureTrackerConfigRvc4.cpp new file mode 100644 index 000000000..e5288a2b6 --- /dev/null +++ b/src/pipeline/datatype/FeatureTrackerConfigRvc4.cpp @@ -0,0 +1,78 @@ +#include "depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp" + +#include + +namespace dai { + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setCornerDetector(FeatureTrackerConfigRvc4::CornerDetector::Type cornerDetector) { + this->cornerDetector.type = cornerDetector; + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setCornerDetector(FeatureTrackerConfigRvc4::CornerDetector config) { + cornerDetector = config; + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setMotionEstimator(bool enable) { + motionEstimator.enable = enable; + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setMotionEstimator(FeatureTrackerConfigRvc4::MotionEstimator config) { + motionEstimator = config; + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setHwMotionEstimation() { + motionEstimator.type = FeatureTrackerConfigRvc4::MotionEstimator::Type::HW_MOTION_ESTIMATION; + + setMotionEstimator(true); + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setHarrisCornerDetectorThreshold(std::int32_t cornerDetectorThreshold) { + if(cornerDetectorThreshold >= 0 and cornerDetectorThreshold <= 25000) { + cornerDetector.thresholds.harrisScore = cornerDetectorThreshold; + } + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setHarrisCornerDetectorRobustness(std::int32_t cornerDetectorRobustness) { + if(cornerDetectorRobustness >= 0 and cornerDetectorRobustness <= 127) { + cornerDetector.thresholds.robustness = cornerDetectorRobustness; + } + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setConfidence(std::int32_t confidenceThreshold) { + if(confidenceThreshold >= 0 and confidenceThreshold <= 255) { + featureMaintainer.confidenceThreshold = confidenceThreshold; + } + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setFilterCoeffs(std::array, 3> filterCoeffs) { + cornerDetector.filterCoeffs = {{{filterCoeffs[0][0], filterCoeffs[0][1], filterCoeffs[0][2]}, + {filterCoeffs[1][0], filterCoeffs[1][1], filterCoeffs[1][2]}, + {filterCoeffs[2][0], filterCoeffs[2][1], filterCoeffs[2][2]}}}; + + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setFeatureMaintainer(bool enable) { + featureMaintainer.enable = enable; + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setFeatureMaintainer(FeatureTrackerConfigRvc4::FeatureMaintainer config) { + featureMaintainer = config; + return *this; +} + +FeatureTrackerConfigRvc4& FeatureTrackerConfigRvc4::setNumMaxFeatures(std::int32_t numMaxFeatures) { + cornerDetector.numMaxFeatures = numMaxFeatures; + return *this; +} + +} // namespace dai diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 8df56df94..06bc20ffa 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -18,6 +18,7 @@ #include "depthai/pipeline/datatype/EdgeDetectorConfig.hpp" #include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" +#include "depthai/pipeline/datatype/FeatureTrackerConfigRvc4.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" #include "depthai/pipeline/datatype/ImageManipConfig.hpp" #include "depthai/pipeline/datatype/ImgDetections.hpp" @@ -189,6 +190,11 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* case DatatypeEnum::FeatureTrackerConfig: return parseDatatype(metadataStart, serializedObjectSize, data); break; + + case DatatypeEnum::FeatureTrackerConfigRvc4: + return parseDatatype(metadataStart, serializedObjectSize, data); + break; + case DatatypeEnum::BenchmarkReport: return parseDatatype(metadataStart, serializedObjectSize, data); break; diff --git a/src/pipeline/node/FeatureTrackerRvc4.cpp b/src/pipeline/node/FeatureTrackerRvc4.cpp new file mode 100644 index 000000000..05c7ca767 --- /dev/null +++ b/src/pipeline/node/FeatureTrackerRvc4.cpp @@ -0,0 +1,25 @@ +#include "depthai/pipeline/node/FeatureTrackerRvc4.hpp" + +#include "spdlog/fmt/fmt.h" + +namespace dai { +namespace node { + +FeatureTrackerRvc4::FeatureTrackerRvc4(std::unique_ptr props) : DeviceNodeCRTP(std::move(props)) {} + +FeatureTrackerRvc4::Properties& FeatureTrackerRvc4::getProperties() { + properties.initialConfig = initialConfig; + return properties; +} + +// Node properties configuration +void FeatureTrackerRvc4::setWaitForConfigInput(bool wait) { + inputConfig.setWaitForMessage(wait); +} + +bool FeatureTrackerRvc4::getWaitForConfigInput() const { + return inputConfig.getWaitForMessage(); +} + +} // namespace node +} // namespace dai