# Feature Tracker with Motion Estimation This example demonstrates the capabilities of the [FeatureTracker](https://docs.luxonis.com/software-v3/depthai/examples/feature_tracker/feature_tracker.md) combined with motion estimation. It detects and tracks features between consecutive frames using optical flow. Each feature is assigned a unique ID. The motion of the camera is estimated based on the tracked features, and the estimated motion (e.g., Up, Down, Left, Right, Rotating) is displayed on screen. The [Feature Detector](https://docs.luxonis.com/software-v3/depthai/examples/feature_detector.md) example only detects features without estimating motion. ## Demo This example requires the DepthAI v3 API, see [installation instructions](https://docs.luxonis.com/software-v3/depthai.md). ## Source code #### Python ```python #!/usr/bin/env python3 import numpy as np import cv2 from collections import deque import depthai as dai class CameraMotionEstimator: def __init__(self, filter_weight=0.5, motion_threshold=0.01, rotation_threshold=0.05): self.last_avg_flow = np.array([0.0, 0.0]) self.filter_weight = filter_weight self.motion_threshold = motion_threshold self.rotation_threshold = rotation_threshold def estimate_motion(self, feature_paths): most_prominent_motion = "Camera Staying Still" max_magnitude = 0.0 avg_flow = np.array([0.0, 0.0]) total_rotation = 0.0 vanishing_point = np.array([0.0, 0.0]) num_features = len(feature_paths) print(f"Number of features: {num_features}") if num_features == 0: return most_prominent_motion, vanishing_point for path in feature_paths.values(): if len(path) >= 2: src = np.array([path[-2].x, path[-2].y]) dst = np.array([path[-1].x, path[-1].y]) avg_flow += dst - src motion_vector = dst + (dst - src) vanishing_point += motion_vector rotation = np.arctan2(dst[1] - src[1], dst[0] - src[0]) total_rotation += rotation avg_flow /= num_features avg_rotation = total_rotation / num_features vanishing_point /= num_features print(f"Average Flow: {avg_flow}") print(f"Average Rotation: {avg_rotation}") avg_flow = (self.filter_weight * self.last_avg_flow + (1 - self.filter_weight) * avg_flow) self.last_avg_flow = avg_flow flow_magnitude = np.linalg.norm(avg_flow) rotation_magnitude = abs(avg_rotation) if flow_magnitude > max_magnitude and flow_magnitude > self.motion_threshold: if abs(avg_flow[0]) > abs(avg_flow[1]): most_prominent_motion = 'Right' if avg_flow[0] < 0 else 'Left' else: most_prominent_motion = 'Down' if avg_flow[1] < 0 else 'Up' max_magnitude = flow_magnitude if rotation_magnitude > max_magnitude and rotation_magnitude > self.rotation_threshold: most_prominent_motion = 'Rotating' return most_prominent_motion, vanishing_point class FeatureTrackerDrawer: lineColor = (200, 0, 200) pointColor = (0, 0, 255) vanishingPointColor = (255, 0, 255) # Violet color for vanishing point circleRadius = 2 maxTrackedFeaturesPathLength = 30 trackedFeaturesPathLength = 10 trackedIDs = None trackedFeaturesPath = None 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 drawVanishingPoint(self, img, vanishing_point): cv2.circle(img, (int(vanishing_point[0]), int(vanishing_point[1])), self.circleRadius, self.vanishingPointColor, -1, cv2.LINE_AA, 0) # Define color mapping for directions direction_colors = { "Up": (0, 255, 255), # Yellow "Down": (0, 255, 0), # Green "Left": (255, 0, 0), # Blue "Right": (0, 0, 255), # Red } def drawFeatures(self, img, vanishing_point=None, prominent_motion=None): # Get the appropriate point color based on the prominent motion if prominent_motion in self.direction_colors: point_color = self.direction_colors[prominent_motion] else: point_color = self.pointColor 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, point_color, 1, cv2.LINE_AA, 0) j = len(path) - 1 cv2.circle(img, (int(path[j].x), int(path[j].y)), self.circleRadius, point_color, -1, cv2.LINE_AA, 0) # Draw the direction text on the image if prominent_motion: font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 1 font_thickness = 2 text_size = cv2.getTextSize(prominent_motion, font, font_scale, font_thickness)[0] text_x = (img.shape[1] - text_size[0]) // 2 text_y = text_size[1] + 20 # 20 pixels from the top # Get the appropriate color based on the prominent motion text_color = self.direction_colors.get(prominent_motion, (255, 255, 255)) # Default to white # Draw the text cv2.putText(img, prominent_motion, (text_x, text_y), font, font_scale, text_color, font_thickness, cv2.LINE_AA) # Draw vanishing point if provided if vanishing_point is not None: self.drawVanishingPoint(img, vanishing_point) def __init__(self, windowName): self.windowName = windowName cv2.namedWindow(windowName) self.trackedIDs = set() self.trackedFeaturesPath = dict() def create_pipeline(): pipeline = dai.Pipeline() # Create a MonoCamera node and set its properties mono_left = pipeline.create(dai.node.MonoCamera) mono_left.setCamera("left") mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) mono_left.setFps(15) # Create a FeatureTracker node feature_tracker_left = pipeline.create(dai.node.FeatureTracker) # Create XLinkOut nodes for output streams xout_tracked_features_left = pipeline.create(dai.node.XLinkOut) xout_passthrough_left = pipeline.create(dai.node.XLinkOut) # Set stream names xout_tracked_features_left.setStreamName("trackedFeaturesLeft") xout_passthrough_left.setStreamName("passthroughLeft") # Allocate resources for improved performance num_shaves = 2 num_memory_slices = 2 feature_tracker_left.setHardwareResources(num_shaves, num_memory_slices) # Link the nodes mono_left.out.link(feature_tracker_left.inputImage) feature_tracker_left.passthroughInputImage.link(xout_passthrough_left.input) feature_tracker_left.outputFeatures.link(xout_tracked_features_left.input) return pipeline if __name__ == '__main__': pipeline = create_pipeline() with dai.Device(pipeline) as device: output_features_left_queue = device.getOutputQueue( "trackedFeaturesLeft", maxSize=4, blocking=False) passthrough_image_left_queue = device.getOutputQueue( "passthroughLeft", maxSize=4, blocking=False) left_window_name = "Left" left_feature_drawer = FeatureTrackerDrawer(left_window_name) camera_estimator_left = CameraMotionEstimator( filter_weight=0.5, motion_threshold=0.3, rotation_threshold=0.5) while True: in_passthrough_frame_left = passthrough_image_left_queue.get() passthrough_frame_left = in_passthrough_frame_left.getFrame() left_frame = cv2.cvtColor(passthrough_frame_left, cv2.COLOR_GRAY2BGR) tracked_features_left = output_features_left_queue.get().trackedFeatures motions_left, vanishing_pt_left = camera_estimator_left.estimate_motion( left_feature_drawer.trackedFeaturesPath) left_feature_drawer.trackFeaturePath(tracked_features_left) left_feature_drawer.drawFeatures(left_frame, vanishing_pt_left, motions_left) print("Motions:", motions_left) cv2.imshow(left_window_name, left_frame) if cv2.waitKey(1) == ord('q'): break ``` ## Pipeline ### examples/feature_motion_estimation.pipeline.json ```json { "pipeline": { "connections": [ { "node1Id": 0, "node1Output": "out", "node1OutputGroup": "", "node2Id": 1, "node2Input": "inputImage", "node2InputGroup": "" }, { "node1Id": 1, "node1Output": "passthroughInputImage", "node1OutputGroup": "", "node2Id": 3, "node2Input": "in", "node2InputGroup": "" }, { "node1Id": 1, "node1Output": "outputFeatures", "node1OutputGroup": "", "node2Id": 2, "node2Input": "in", "node2InputGroup": "" } ], "globalProperties": { "calibData": null, "cameraTuningBlobSize": null, "cameraTuningBlobUri": "", "leonCssFrequencyHz": 700000000.0, "leonMssFrequencyHz": 700000000.0, "pipelineName": null, "pipelineVersion": null, "sippBufferSize": 18432, "sippDmaBufferSize": 16384, "xlinkChunkSize": -1 }, "nodes": [ [ 0, { "id": 0, "ioInfo": [ [ [ "", "inputControl" ], { "blocking": true, "group": "", "id": 1, "name": "inputControl", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "", "out" ], { "blocking": false, "group": "", "id": 2, "name": "out", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "raw" ], { "blocking": false, "group": "", "id": 3, "name": "raw", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "frameEvent" ], { "blocking": false, "group": "", "id": 4, "name": "frameEvent", "queueSize": 8, "type": 0, "waitForMessage": false } ] ], "name": "MonoCamera", "properties": { "boardSocket": -1, "cameraName": "left", "fps": 15.0, "imageOrientation": -1, "initialControl": { "aeLockMode": false, "aeMaxExposureTimeUs": 0, "aeRegion": { "height": 0, "priority": 0, "width": 0, "x": 0, "y": 0 }, "afRegion": { "height": 0, "priority": 0, "width": 0, "x": 0, "y": 0 }, "antiBandingMode": 0, "autoFocusMode": 3, "awbLockMode": false, "awbMode": 0, "brightness": 0, "captureIntent": 0, "chromaDenoise": 0, "cmdMask": 0, "contrast": 0, "controlMode": 0, "effectMode": 0, "expCompensation": 0, "expManual": { "exposureTimeUs": 0, "frameDurationUs": 0, "sensitivityIso": 0 }, "frameSyncMode": 0, "lensPosAutoInfinity": 0, "lensPosAutoMacro": 0, "lensPosition": 0, "lensPositionRaw": 0.0, "lowPowerNumFramesBurst": 0, "lowPowerNumFramesDiscard": 0, "lumaDenoise": 0, "saturation": 0, "sceneMode": 0, "sharpness": 0, "strobeConfig": { "activeLevel": 0, "enable": 0, "gpioNumber": 0 }, "strobeTimings": { "durationUs": 0, "exposureBeginOffsetUs": 0, "exposureEndOffsetUs": 0 }, "wbColorTemp": 0 }, "isp3aFps": 0, "numFramesPool": 3, "numFramesPoolRaw": 3, "rawPacked": null, "resolution": 0 } } ], [ 1, { "id": 1, "ioInfo": [ [ [ "", "inputConfig" ], { "blocking": false, "group": "", "id": 5, "name": "inputConfig", "queueSize": 4, "type": 3, "waitForMessage": false } ], [ [ "", "inputImage" ], { "blocking": false, "group": "", "id": 6, "name": "inputImage", "queueSize": 4, "type": 3, "waitForMessage": true } ], [ [ "", "outputFeatures" ], { "blocking": false, "group": "", "id": 7, "name": "outputFeatures", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "passthroughInputImage" ], { "blocking": false, "group": "", "id": 8, "name": "passthroughInputImage", "queueSize": 8, "type": 0, "waitForMessage": false } ] ], "name": "FeatureTracker", "properties": { "initialConfig": { "cornerDetector": { "cellGridDimension": 4, "enableSobel": true, "enableSorting": true, "numMaxFeatures": 0, "numTargetFeatures": 320, "thresholds": { "decreaseFactor": 0.8999999761581421, "increaseFactor": 1.100000023841858, "initialValue": 0.0, "max": 0.0, "min": 0.0 }, "type": 0 }, "featureMaintainer": { "enable": true, "lostFeatureErrorThreshold": 50000.0, "minimumDistanceBetweenFeatures": 50.0, "trackedFeatureThreshold": 200000.0 }, "motionEstimator": { "enable": true, "opticalFlow": { "epsilon": 0.009999999776482582, "maxIterations": 9, "pyramidLevels": 0, "searchWindowHeight": 5, "searchWindowWidth": 5 }, "type": 0 } }, "numMemorySlices": 2, "numShaves": 2 } } ], [ 2, { "id": 2, "ioInfo": [ [ [ "", "in" ], { "blocking": true, "group": "", "id": 9, "name": "in", "queueSize": 8, "type": 3, "waitForMessage": true } ] ], "name": "XLinkOut", "properties": { "maxFpsLimit": -1.0, "metadataOnly": false, "streamName": "trackedFeaturesLeft" } } ], [ 3, { "id": 3, "ioInfo": [ [ [ "", "in" ], { "blocking": true, "group": "", "id": 10, "name": "in", "queueSize": 8, "type": 3, "waitForMessage": true } ] ], "name": "XLinkOut", "properties": { "maxFpsLimit": -1.0, "metadataOnly": false, "streamName": "passthroughLeft" } } ] ] } } ``` ### Need assistance? Head over to [Discussion Forum](https://discuss.luxonis.com/) for technical support or any other questions you might have.