jakaskerl
Hello
This is the dephai v3 and for the script I want to test the example provided RVC2 --> RTab Map VIO SLAM
import time
import depthai as dai
from rerun_node import RerunNode
# Create pipeline
with dai.Pipeline() as p:
fps = 30
width = 640
height = 400
# Define sources and outputs
left = p.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B, sensorFps=fps)
right = p.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C, sensorFps=fps)
imu = p.create(dai.node.IMU)
stereo = p.create(dai.node.StereoDepth)
featureTracker = p.create(dai.node.FeatureTracker)
odom = p.create(dai.node.RTABMapVIO)
slam = p.create(dai.node.RTABMapSLAM)
params = {"RGBD/CreateOccupancyGrid": "true",
"Grid/3D": "true",
"Rtabmap/SaveWMState": "true"}
slam.setParams(params)
rerunViewer = RerunNode()
imu.enableIMUSensor([dai.IMUSensor.ACCELEROMETER_RAW, dai.IMUSensor.GYROSCOPE_RAW], 200)
imu.setBatchReportThreshold(1)
imu.setMaxBatchReports(10)
featureTracker.setHardwareResources(1,2)
featureTracker.initialConfig.setCornerDetector(dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS)
featureTracker.initialConfig.setNumTargetFeatures(1000)
featureTracker.initialConfig.setMotionEstimator(False)
featureTracker.initialConfig.FeatureMaintainer.minimumDistanceBetweenFeatures = 49
stereo.setExtendedDisparity(False)
stereo.setLeftRightCheck(True)
stereo.setRectifyEdgeFillColor(0)
stereo.enableDistortionCorrection(True)
stereo.initialConfig.setLeftRightCheckThreshold(10)
stereo.setDepthAlign(dai.CameraBoardSocket.CAM_B)
# Linking
left.requestOutput((width, height)).link(stereo.left)
right.requestOutput((width, height)).link(stereo.right)
featureTracker.passthroughInputImage.link(odom.rect)
stereo.rectifiedLeft.link(featureTracker.inputImage)
stereo.depth.link(odom.depth)
imu.out.link(odom.imu)
featureTracker.outputFeatures.link(odom.features)
odom.transform.link(slam.odom)
odom.passthroughRect.link(slam.rect)
odom.passthroughDepth.link(slam.depth)
slam.transform.link(rerunViewer.inputTrans)
slam.passthroughRect.link(rerunViewer.inputImg)
slam.occupancyGridMap.link(rerunViewer.inputGrid)
slam.obstaclePCL.link(rerunViewer.inputObstaclePCL)
slam.groundPCL.link(rerunViewer.inputGroundPCL)
p.start()
while p.isRunning():
time.sleep(1)