RTab Map VIO SLAM

View as Markdown

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    slam = p.create(dai.node.RTABMapSLAM)
19
20    params = {"RGBD/CreateOccupancyGrid": "true",
21              "Grid/3D": "true",
22              "Rtabmap/SaveWMState": "true"}
23    slam.setParams(params)
24
25    rerunViewer = p.create(RerunNode)
26    imu.enableIMUSensor([dai.IMUSensor.ACCELEROMETER_RAW, dai.IMUSensor.GYROSCOPE_RAW], 200)
27    imu.setBatchReportThreshold(1)
28    imu.setMaxBatchReports(10)
29
30    featureTracker.setHardwareResources(1,2)
31    featureTracker.initialConfig.setCornerDetector(dai.FeatureTrackerConfig.CornerDetector.Type.HARRIS)
32    featureTracker.initialConfig.setNumTargetFeatures(1000)
33    featureTracker.initialConfig.setMotionEstimator(False)
34    featureTracker.initialConfig.FeatureMaintainer.minimumDistanceBetweenFeatures = 49
35
36    stereo.setExtendedDisparity(False)
37    stereo.setLeftRightCheck(True)
38    stereo.setRectifyEdgeFillColor(0)
39    stereo.enableDistortionCorrection(True)
40    stereo.initialConfig.setLeftRightCheckThreshold(10)
41    stereo.setDepthAlign(dai.CameraBoardSocket.CAM_B)
42
43
44    # Linking
45
46    left.requestOutput((width, height)).link(stereo.left)
47    right.requestOutput((width, height)).link(stereo.right)
48    featureTracker.passthroughInputImage.link(odom.rect)
49    stereo.rectifiedLeft.link(featureTracker.inputImage)
50    stereo.depth.link(odom.depth)
51    imu.out.link(odom.imu)
52    featureTracker.outputFeatures.link(odom.features)
53
54    odom.transform.link(slam.odom)
55    odom.passthroughRect.link(slam.rect)
56    odom.passthroughDepth.link(slam.depth)
57
58    slam.transform.link(rerunViewer.inputTrans)
59    slam.passthroughRect.link(rerunViewer.inputImg)
60    slam.occupancyGridMap.link(rerunViewer.inputGrid)
61    slam.obstaclePCL.link(rerunViewer.inputObstaclePCL)
62    slam.groundPCL.link(rerunViewer.inputGroundPCL)
63    p.start()
64    while p.isRunning():
65        time.sleep(1)

1#include "depthai/depthai.hpp"
2#include "rerun_node.hpp"
3
4int main() {
5    using namespace std;
6    // Create pipeline
7    dai::Pipeline pipeline;
8    int fps = 30;
9    int width = 640;
10    int height = 400;
11    // Define sources and outputs
12    auto left = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_B, std::nullopt, fps);
13    auto right = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_C, std::nullopt, fps);
14    auto stereo = pipeline.create<dai::node::StereoDepth>();
15    auto imu = pipeline.create<dai::node::IMU>();
16    auto featureTracker = pipeline.create<dai::node::FeatureTracker>();
17    auto odom = pipeline.create<dai::node::RTABMapVIO>();
18    auto slam = pipeline.create<dai::node::RTABMapSLAM>();
19    auto rerun = pipeline.create<RerunNode>();
20    std::map<std::string, std::string> params{};
21    params.insert(std::make_pair<std::string, std::string>("Odom/ResetCountDown", "30"));
22
23    odom->setParams(params);
24
25    params.insert(std::make_pair<std::string, std::string>("RGBD/CreateOccupancyGrid", "true"));
26    params.insert(std::make_pair<std::string, std::string>("Grid/3D", "true"));
27    slam->setParams(params);
28    imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100);
29    imu->setBatchReportThreshold(1);
30    imu->setMaxBatchReports(10);
31
32    featureTracker->setHardwareResources(1, 2);
33    featureTracker->initialConfig->setCornerDetector(dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI);
34    featureTracker->initialConfig->setNumTargetFeatures(1000);
35    featureTracker->initialConfig->setMotionEstimator(false);
36    featureTracker->initialConfig->featureMaintainer.minimumDistanceBetweenFeatures = 49.0;
37    stereo->rectifiedLeft.link(featureTracker->inputImage);
38
39    stereo->setExtendedDisparity(false);
40    stereo->setSubpixel(true);
41    stereo->setLeftRightCheck(true);
42    stereo->setRectifyEdgeFillColor(0);  // black, to better see the cutout
43    stereo->enableDistortionCorrection(true);
44    stereo->initialConfig->setLeftRightCheckThreshold(10);
45    stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
46
47    // Linking
48    left->requestOutput(std::make_pair(width, height))->link(stereo->left);
49    right->requestOutput(std::make_pair(width, height))->link(stereo->right);
50    featureTracker->passthroughInputImage.link(odom->rect);
51    stereo->depth.link(odom->depth);
52    imu->out.link(odom->imu);
53    featureTracker->outputFeatures.link(odom->features);
54
55    odom->transform.link(slam->odom);
56    odom->passthroughRect.link(slam->rect);
57    odom->passthroughDepth.link(slam->depth);
58    // odom->passthroughFeatures.link(slam->inputFeatures);
59
60    slam->transform.link(rerun->inputTrans);
61    slam->passthroughRect.link(rerun->inputImg);
62    slam->occupancyGridMap.link(rerun->inputMap);
63    slam->obstaclePCL.link(rerun->inputObstaclePCL);
64    slam->groundPCL.link(rerun->inputGroundPCL);
65
66    pipeline.start();
67    pipeline.wait();
68}