Basalt VIO

View as Markdown

1import signal
2import time
3import depthai as dai
4from rerun_node import RerunNode
5# Create pipeline
6
7with dai.Pipeline() as p:
8    fps = 60
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    odom = p.create(dai.node.BasaltVIO)
16
17    rerunViewer = p.create(RerunNode)
18    imu.enableIMUSensor([dai.IMUSensor.ACCELEROMETER_RAW, dai.IMUSensor.GYROSCOPE_RAW], 200)
19    imu.setBatchReportThreshold(1)
20    imu.setMaxBatchReports(10)
21
22    # Linking
23    left.requestOutput((width, height)).link(odom.left)
24    right.requestOutput((width, height)).link(odom.right)
25    imu.out.link(odom.imu)
26    odom.passthrough.link(rerunViewer.inputImg)
27    odom.transform.link(rerunViewer.inputTrans)
28    p.start()
29    while p.isRunning():
30        time.sleep(0.01)

1#include "depthai/depthai.hpp"
2#include "rerun_node.hpp"
3
4int main() {
5    using namespace std;
6
7    // Create pipeline
8    dai::Pipeline pipeline;
9    int fps = 60;
10    int width = 640;
11    int height = 400;
12    // Define sources and outputs
13    auto left = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_B, std::nullopt, fps);
14    auto right = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_C, std::nullopt, fps);
15    auto imu = pipeline.create<dai::node::IMU>();
16    auto odom = pipeline.create<dai::node::BasaltVIO>();
17
18    auto rerun = pipeline.create<RerunNode>();
19    imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
20    imu->setBatchReportThreshold(1);
21    imu->setMaxBatchReports(10);
22
23    // Linking
24    left->requestOutput(std::make_pair(width, height))->link(odom->left);
25    right->requestOutput(std::make_pair(width, height))->link(odom->right);
26    imu->out.link(odom->imu);
27    odom->transform.link(rerun->inputTrans);
28    odom->passthrough.link(rerun->inputImg);
29
30    pipeline.start();
31    pipeline.wait();
32}