Merge pull request #423 from luxonis/develop

Release v2.12.0.0

CMakeLists.txt

@@ -29,7 +29,7 @@ endif()
 
 # Pybindings project
 set(TARGET_NAME depthai)
-project(depthai VERSION "1") # revision of bindings [depthai-core].[rev]
+project(depthai VERSION "0") # revision of bindings [depthai-core].[rev]
 
 # Set default build type depending on context
 set(default_build_type "Release")

examples/CMakeLists.txt

@@ -109,6 +109,7 @@ add_python_example(edge_detector EdgeDetector/edge_detector.py)
 
 ## FeatureTracker
 add_python_example(feature_tracker FeatureTracker/feature_tracker.py)
+add_python_example(feature_tracker_color FeatureTracker/feature_tracker_color.py)
 add_python_example(feature_detector FeatureTracker/feature_detector.py)
 
 ## HostSide

examples/FeatureTracker/feature_tracker_color.py

@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+
+import cv2
+import depthai as dai
+from collections import deque
+
+class FeatureTrackerDrawer:
+
+    lineColor = (200, 0, 200)
+    pointColor = (0, 0, 255)
+    circleRadius = 2
+    maxTrackedFeaturesPathLength = 30
+    # for how many frames the feature is tracked
+    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()
+
+
+# Create pipeline
+pipeline = dai.Pipeline()
+
+# Define sources and outputs
+colorCam = pipeline.create(dai.node.ColorCamera)
+featureTrackerColor = pipeline.create(dai.node.FeatureTracker)
+
+xoutPassthroughFrameColor = pipeline.create(dai.node.XLinkOut)
+xoutTrackedFeaturesColor = pipeline.create(dai.node.XLinkOut)
+xinTrackedFeaturesConfig = pipeline.create(dai.node.XLinkIn)
+
+xoutPassthroughFrameColor.setStreamName("passthroughFrameColor")
+xoutTrackedFeaturesColor.setStreamName("trackedFeaturesColor")
+xinTrackedFeaturesConfig.setStreamName("trackedFeaturesConfig")
+
+# Properties
+colorCam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
+
+if 1:
+    colorCam.setIspScale(2,3)
+    colorCam.video.link(featureTrackerColor.inputImage)
+else:
+    colorCam.isp.link(featureTrackerColor.inputImage)
+
+# Linking
+featureTrackerColor.passthroughInputImage.link(xoutPassthroughFrameColor.input)
+featureTrackerColor.outputFeatures.link(xoutTrackedFeaturesColor.input)
+xinTrackedFeaturesConfig.out.link(featureTrackerColor.inputConfig)
+
+# By default the least mount of resources are allocated
+# increasing it improves performance
+numShaves = 2
+numMemorySlices = 2
+featureTrackerColor.setHardwareResources(numShaves, numMemorySlices)
+featureTrackerConfig = featureTrackerColor.initialConfig.get()
+
+print("Press 's' to switch between Lucas-Kanade optical flow and hardware accelerated motion estimation!")
+
+# Connect to device and start pipeline
+with dai.Device(pipeline) as device:
+
+    # Output queues used to receive the results
+    passthroughImageColorQueue = device.getOutputQueue("passthroughFrameColor", 8, False)
+    outputFeaturesColorQueue = device.getOutputQueue("trackedFeaturesColor", 8, False)
+
+    inputFeatureTrackerConfigQueue = device.getInputQueue("trackedFeaturesConfig")
+
+    colorWindowName = "color"
+    colorFeatureDrawer = FeatureTrackerDrawer("Feature tracking duration (frames)", colorWindowName)
+
+    while True:
+        inPassthroughFrameColor = passthroughImageColorQueue.get()
+        passthroughFrameColor = inPassthroughFrameColor.getCvFrame()
+        colorFrame = passthroughFrameColor
+
+        trackedFeaturesColor = outputFeaturesColorQueue.get().trackedFeatures
+        colorFeatureDrawer.trackFeaturePath(trackedFeaturesColor)
+        colorFeatureDrawer.drawFeatures(colorFrame)
+
+        # Show the frame
+        cv2.imshow(colorWindowName, colorFrame)
+
+        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)

examples/StereoDepth/stereo_depth_from_host.py

@@ -16,10 +16,6 @@
 parser.add_argument('-dumpdispcost', "--dumpdisparitycostvalues", action="store_true", help="Dumps the disparity cost values for each disparity range. 96 byte for each pixel.")
 args = parser.parse_args()
 
-if args.debug and args.dumpdisparitycostvalues:
-    print("-debug and --dumpdisparitycostvalues are mutually exclusive!")
-    exit(1)
-
 if not Path(datasetDefault).exists():
     import sys
     raise FileNotFoundError(f'Required file/s not found, please run "{sys.executable} install_requirements.py"')
@@ -54,6 +50,8 @@ def set(self, value):
     trLineqAlpha = list()
     trLineqBeta = list()
     trLineqThreshold = list()
+    trCostAggregationP1 = list()
+    trCostAggregationP2 = list()
 
     def trackbarSigma(value):
         StereoConfigHandler.config.postProcessing.bilateralSigmaValue = value
@@ -96,6 +94,20 @@ def trackbarLineqThreshold(value):
         for tr in StereoConfigHandler.trLineqThreshold:
             tr.set(value)
 
+    def trackbarCostAggregationP1(value):
+        StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP1 = value
+        StereoConfigHandler.config.costAggregation.verticalPenaltyCostP1 = value
+        StereoConfigHandler.newConfig = True
+        for tr in StereoConfigHandler.trCostAggregationP1:
+            tr.set(value)
+
+    def trackbarCostAggregationP2(value):
+        StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP2 = value
+        StereoConfigHandler.config.costAggregation.verticalPenaltyCostP2 = value
+        StereoConfigHandler.newConfig = True
+        for tr in StereoConfigHandler.trCostAggregationP2:
+            tr.set(value)
+
     def handleKeypress(key, stereoDepthConfigInQueue):
         if key == ord('m'):
             StereoConfigHandler.newConfig = True
@@ -138,6 +150,11 @@ def handleKeypress(key, stereoDepthConfigInQueue):
             StereoConfigHandler.config.algorithmControl.enableSubpixel = not StereoConfigHandler.config.algorithmControl.enableSubpixel
             state = "on" if StereoConfigHandler.config.algorithmControl.enableSubpixel else "off"
             print(f"Subpixel {state}")
+        elif key == ord('3'):
+            StereoConfigHandler.newConfig = True
+            StereoConfigHandler.config.algorithmControl.enableExtended = not StereoConfigHandler.config.algorithmControl.enableExtended
+            state = "on" if StereoConfigHandler.config.algorithmControl.enableExtended else "off"
+            print(f"Extended {state}")
 
         StereoConfigHandler.sendConfig(stereoDepthConfigInQueue)
 
@@ -157,6 +174,8 @@ def registerWindow(stream):
         StereoConfigHandler.trLineqAlpha.append(StereoConfigHandler.Trackbar('Linear equation alpha', stream, 0, 15, StereoConfigHandler.config.costMatching.linearEquationParameters.alpha, StereoConfigHandler.trackbarLineqAlpha))
         StereoConfigHandler.trLineqBeta.append(StereoConfigHandler.Trackbar('Linear equation beta', stream, 0, 15, StereoConfigHandler.config.costMatching.linearEquationParameters.beta, StereoConfigHandler.trackbarLineqBeta))
         StereoConfigHandler.trLineqThreshold.append(StereoConfigHandler.Trackbar('Linear equation threshold', stream, 0, 255, StereoConfigHandler.config.costMatching.linearEquationParameters.threshold, StereoConfigHandler.trackbarLineqThreshold))
+        StereoConfigHandler.trCostAggregationP1.append(StereoConfigHandler.Trackbar('Cost aggregation P1', stream, 0, 500, StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP1, StereoConfigHandler.trackbarCostAggregationP1))
+        StereoConfigHandler.trCostAggregationP2.append(StereoConfigHandler.Trackbar('Cost aggregation P2', stream, 0, 500, StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP2, StereoConfigHandler.trackbarCostAggregationP2))
 
     def __init__(self, config):
         print("Control median filter using the 'm' key.")
@@ -166,6 +185,7 @@ def __init__(self, config):
         print("Control census transform mean mode using the 'v' key.")
         print("Control left-right check mode using the '1' key.")
         print("Control subpixel mode using the '2' key.")
+        print("Control extended mode using the '3' key.")
 
         StereoConfigHandler.config = config
 
@@ -204,6 +224,8 @@ def __init__(self, config):
 if args.debug:
     xoutDebugLrCheckIt1 = pipeline.create(dai.node.XLinkOut)
     xoutDebugLrCheckIt2 = pipeline.create(dai.node.XLinkOut)
+    xoutDebugExtLrCheckIt1 = pipeline.create(dai.node.XLinkOut)
+    xoutDebugExtLrCheckIt2 = pipeline.create(dai.node.XLinkOut)
 if args.dumpdisparitycostvalues:
     xoutDebugCostDump = pipeline.create(dai.node.XLinkOut)
 
@@ -222,6 +244,8 @@ def __init__(self, config):
 if args.debug:
     xoutDebugLrCheckIt1.setStreamName('disparity_lr_check_iteration1')
     xoutDebugLrCheckIt2.setStreamName('disparity_lr_check_iteration2')
+    xoutDebugExtLrCheckIt1.setStreamName('disparity_ext_lr_check_iteration1')
+    xoutDebugExtLrCheckIt2.setStreamName('disparity_ext_lr_check_iteration2')
 if args.dumpdisparitycostvalues:
     xoutDebugCostDump.setStreamName('disparity_cost_dump')
 
@@ -253,6 +277,8 @@ def __init__(self, config):
 if args.debug:
     stereo.debugDispLrCheckIt1.link(xoutDebugLrCheckIt1.input)
     stereo.debugDispLrCheckIt2.link(xoutDebugLrCheckIt2.input)
+    stereo.debugExtDispLrCheckIt1.link(xoutDebugExtLrCheckIt1.input)
+    stereo.debugExtDispLrCheckIt2.link(xoutDebugExtLrCheckIt2.input)
 if args.dumpdisparitycostvalues:
     stereo.debugDispCostDump.link(xoutDebugCostDump.input)
 
@@ -278,6 +304,7 @@ def __init__(self, config):
 debugStreams = []
 if args.debug:
     debugStreams.extend(['disparity_lr_check_iteration1', 'disparity_lr_check_iteration2'])
+    debugStreams.extend(['disparity_ext_lr_check_iteration1', 'disparity_ext_lr_check_iteration2'])
 if args.dumpdisparitycostvalues:
     debugStreams.append('disparity_cost_dump')
 
@@ -327,22 +354,17 @@ def convertToCv2Frame(name, image, config):
 
     # Create a receive queue for each stream
     q_list = []
-    q_list_debug = []
     for s in streams:
         q = device.getOutputQueue(s, 8, blocking=False)
         q_list.append(q)
 
-    if args.debug or args.dumpdisparitycostvalues:
-        q_list_debug = q_list.copy()
-        for s in debugStreams:
-            q = device.getOutputQueue(s, 8, blocking=False)
-            q_list_debug.append(q)
 
     inCfg = device.getOutputQueue("stereo_cfg", 8, blocking=False)
 
     # Need to set a timestamp for input frames, for the sync stage in Stereo node
     timestamp_ms = 0
     index = 0
+    prevQueues = q_list.copy()
     while True:
         # Handle input streams, if any
         if in_q_list:
@@ -374,13 +396,38 @@ def convertToCv2Frame(name, image, config):
         currentConfig = inCfg.get()
 
         lrCheckEnabled = currentConfig.get().algorithmControl.enableLeftRightCheck
-        queues = q_list
-
-        if (args.debug and lrCheckEnabled) or args.dumpdisparitycostvalues:
-            queues = q_list_debug
-        else:
-            for s in debugStreams:
-                cv2.destroyWindow(s)
+        extendedEnabled = currentConfig.get().algorithmControl.enableExtended
+        queues = q_list.copy()
+
+        if args.dumpdisparitycostvalues:
+            q = device.getOutputQueue('disparity_cost_dump', 8, blocking=False)
+            queues.append(q)
+
+        if args.debug:
+            q_list_debug = []
+
+            activeDebugStreams = []
+            if lrCheckEnabled:
+                activeDebugStreams.extend(['disparity_lr_check_iteration1', 'disparity_lr_check_iteration2'])
+            if extendedEnabled:
+                activeDebugStreams.extend(['disparity_ext_lr_check_iteration1'])
+                if lrCheckEnabled:
+                    activeDebugStreams.extend(['disparity_ext_lr_check_iteration2'])
+
+            for s in activeDebugStreams:
+                q = device.getOutputQueue(s, 8, blocking=False)
+                q_list_debug.append(q)
+
+            queues.extend(q_list_debug)
+
+        def ListDiff(li1, li2):
+            return list(set(li1) - set(li2)) + list(set(li2) - set(li1))
+
+        diff = ListDiff(prevQueues, queues)
+        for s in diff:
+            name = s.getName()
+            cv2.destroyWindow(name)
+        prevQueues = queues.copy()
 
         for q in queues:
             if q.getName() in ['left', 'right']: continue

src/DatatypeBindings.cpp

@@ -1062,6 +1062,7 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){
     algorithmControl
         .def(py::init<>())
         .def_readwrite("enableLeftRightCheck", &RawStereoDepthConfig::AlgorithmControl::enableLeftRightCheck, DOC(dai, RawStereoDepthConfig, AlgorithmControl, enableLeftRightCheck))
+        .def_readwrite("enableExtended", &RawStereoDepthConfig::AlgorithmControl::enableExtended, DOC(dai, RawStereoDepthConfig, AlgorithmControl, enableExtended))
         .def_readwrite("enableSubpixel", &RawStereoDepthConfig::AlgorithmControl::enableSubpixel, DOC(dai, RawStereoDepthConfig, AlgorithmControl, enableSubpixel))
         .def_readwrite("leftRightCheckThreshold", &RawStereoDepthConfig::AlgorithmControl::leftRightCheckThreshold, DOC(dai, RawStereoDepthConfig, AlgorithmControl, leftRightCheckThreshold))
         .def_readwrite("subpixelFractionalBits", &RawStereoDepthConfig::AlgorithmControl::subpixelFractionalBits, DOC(dai, RawStereoDepthConfig, AlgorithmControl, subpixelFractionalBits))
@@ -1114,8 +1115,10 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){
     costAggregation
         .def(py::init<>())
         .def_readwrite("divisionFactor", &RawStereoDepthConfig::CostAggregation::divisionFactor, DOC(dai, RawStereoDepthConfig, CostAggregation, divisionFactor))
-        .def_readwrite("horizontalPenaltyCosts", &RawStereoDepthConfig::CostAggregation::horizontalPenaltyCosts, DOC(dai, RawStereoDepthConfig, CostAggregation, horizontalPenaltyCosts))
-        .def_readwrite("verticalPenaltyCosts", &RawStereoDepthConfig::CostAggregation::verticalPenaltyCosts, DOC(dai, RawStereoDepthConfig, CostAggregation, verticalPenaltyCosts))
+        .def_readwrite("horizontalPenaltyCostP1", &RawStereoDepthConfig::CostAggregation::horizontalPenaltyCostP1, DOC(dai, RawStereoDepthConfig, CostAggregation, horizontalPenaltyCostP1))
+        .def_readwrite("horizontalPenaltyCostP2", &RawStereoDepthConfig::CostAggregation::horizontalPenaltyCostP2, DOC(dai, RawStereoDepthConfig, CostAggregation, horizontalPenaltyCostP2))
+        .def_readwrite("verticalPenaltyCostP1", &RawStereoDepthConfig::CostAggregation::verticalPenaltyCostP1, DOC(dai, RawStereoDepthConfig, CostAggregation, verticalPenaltyCostP1))
+        .def_readwrite("verticalPenaltyCostP2", &RawStereoDepthConfig::CostAggregation::verticalPenaltyCostP2, DOC(dai, RawStereoDepthConfig, CostAggregation, verticalPenaltyCostP2))
         ;
 
     rawStereoDepthConfig
@@ -1141,8 +1144,9 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){
         .def("getMedianFilter",         &StereoDepthConfig::getMedianFilter, DOC(dai, StereoDepthConfig, getMedianFilter))
         .def("getBilateralFilterSigma", &StereoDepthConfig::getBilateralFilterSigma, DOC(dai, StereoDepthConfig, getBilateralFilterSigma))
         .def("getLeftRightCheckThreshold",         &StereoDepthConfig::getLeftRightCheckThreshold, DOC(dai, StereoDepthConfig, getLeftRightCheckThreshold))
-        .def("setLeftRightCheck",       &StereoDepthConfig::setLeftRightCheck, DOC(dai, StereoDepthConfig, setLeftRightCheck))
-        .def("setSubpixel",             &StereoDepthConfig::setSubpixel, DOC(dai, StereoDepthConfig, setSubpixel))
+        .def("setLeftRightCheck",       &StereoDepthConfig::setLeftRightCheck, py::arg("enable"), DOC(dai, StereoDepthConfig, setLeftRightCheck))
+        .def("setExtendedDisparity",    &StereoDepthConfig::setExtendedDisparity, py::arg("enable"), DOC(dai, StereoDepthConfig, setExtendedDisparity))
+        .def("setSubpixel",             &StereoDepthConfig::setSubpixel, py::arg("enable"), DOC(dai, StereoDepthConfig, setSubpixel))
         .def("getMaxDisparity",         &StereoDepthConfig::getMaxDisparity, DOC(dai, StereoDepthConfig, getMaxDisparity))
         .def("set",                     &StereoDepthConfig::set, py::arg("config"), DOC(dai, StereoDepthConfig, set))
         .def("get",                     &StereoDepthConfig::get, DOC(dai, StereoDepthConfig, get))