RTab Map VIO

1import time
2import depthai as dai
3from rerun_node import RerunNode
4
5# Create pipeline
6
7with dai.Pipeline() as p:
8    fps = 30
9    width = 640
10    height = 400
11    # Define sources and outputs
12    left = p.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B, sensorFps=fps)
13    right = p.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C, sensorFps=fps)
14    imu = p.create(dai.node.IMU)
15    stereo = p.create(dai.node.StereoDepth)
16    featureTracker = p.create(dai.node.FeatureTracker)
17    odom = p.create(dai.node.RTABMapVIO)
18
19    rerunViewer = RerunNode()
20    imu.enableIMUSensor([dai.IMUSensor.ACCELEROMETER_RAW, dai.IMUSensor.GYROSCOPE_RAW], 200)
21    imu.setBatchReportThreshold(1)
22    imu.setMaxBatchReports(10)
23
24    featureTracker.setHardwareResources(1,2)
25    featureTracker.initialConfig.setCornerDetector(dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS)
26    featureTracker.initialConfig.setNumTargetFeatures(1000)
27    featureTracker.initialConfig.setMotionEstimator(False)
28    featureTracker.initialConfig.FeatureMaintainer.minimumDistanceBetweenFeatures = 49
29
30    stereo.setExtendedDisparity(False)
31    stereo.setLeftRightCheck(True)
32    stereo.setRectifyEdgeFillColor(0)
33    stereo.enableDistortionCorrection(True)
34    stereo.initialConfig.setLeftRightCheckThreshold(10)
35    stereo.setDepthAlign(dai.CameraBoardSocket.CAM_B)
36
37
38    # Linking
39
40    left.requestOutput((width, height)).link(stereo.left)
41    right.requestOutput((width, height)).link(stereo.right)
42    stereo.rectifiedLeft.link(featureTracker.inputImage)
43    featureTracker.passthroughInputImage.link(odom.rect)
44    stereo.depth.link(odom.depth)
45    featureTracker.outputFeatures.link(odom.features)
46    imu.out.link(odom.imu)
47    odom.passthroughRect.link(rerunViewer.inputImg)
48    odom.transform.link(rerunViewer.inputTrans)
49    p.start()
50    while p.isRunning():
51        time.sleep(1)