IMU Rotation Vector

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import time
6import math
7
8device = dai.Device()
9
10imuType = device.getConnectedIMU()
11imuFirmwareVersion = device.getIMUFirmwareVersion()
12print(f"IMU type: {imuType}, firmware version: {imuFirmwareVersion}")
13
14if imuType != "BNO086":
15    print("Rotation vector output is supported only by BNO086!")
16    exit(1)
17
18# Create pipeline
19pipeline = dai.Pipeline()
20
21# Define sources and outputs
22imu = pipeline.create(dai.node.IMU)
23xlinkOut = pipeline.create(dai.node.XLinkOut)
24
25xlinkOut.setStreamName("imu")
26
27# enable ROTATION_VECTOR at 400 hz rate
28imu.enableIMUSensor(dai.IMUSensor.ROTATION_VECTOR, 400)
29# it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
30# above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
31imu.setBatchReportThreshold(1)
32# maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
33# if lower or equal to batchReportThreshold then the sending is always blocking on device
34# useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
35imu.setMaxBatchReports(10)
36
37# Link plugins IMU -> XLINK
38imu.out.link(xlinkOut.input)
39
40# Pipeline is defined, now we can connect to the device
41with device:
42    device.startPipeline(pipeline)
43
44    def timeDeltaToMilliS(delta) -> float:
45        return delta.total_seconds()*1000
46
47    # Output queue for imu bulk packets
48    imuQueue = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
49    baseTs = None
50    while True:
51        imuData = imuQueue.get()  # blocking call, will wait until a new data has arrived
52
53        imuPackets = imuData.packets
54        for imuPacket in imuPackets:
55            rVvalues = imuPacket.rotationVector
56
57            rvTs = rVvalues.getTimestampDevice()
58            if baseTs is None:
59                baseTs = rvTs
60            rvTs = rvTs - baseTs
61
62            imuF = "{:.06f}"
63            tsF  = "{:.03f}"
64
65            print(f"Rotation vector timestamp: {tsF.format(timeDeltaToMilliS(rvTs))} ms")
66            print(f"Quaternion: i: {imuF.format(rVvalues.i)} j: {imuF.format(rVvalues.j)} "
67                f"k: {imuF.format(rVvalues.k)} real: {imuF.format(rVvalues.real)}")
68            print(f"Accuracy (rad): {imuF.format(rVvalues.rotationVectorAccuracy)}")
69
70
71        if cv2.waitKey(1) == ord('q'):
72            break

1#include <cstdio>
2#include <iostream>
3
4#include "utility.hpp"
5
6// Includes common necessary includes for development using depthai library
7#include "depthai/depthai.hpp"
8
9int main() {
10    using namespace std;
11    using namespace std::chrono;
12
13    // Create pipeline
14    dai::Pipeline pipeline;
15
16    // Define sources and outputs
17    auto imu = pipeline.create<dai::node::IMU>();
18    auto xlinkOut = pipeline.create<dai::node::XLinkOut>();
19
20    xlinkOut->setStreamName("imu");
21
22    // enable ROTATION_VECTOR at 400 hz rate
23    imu->enableIMUSensor(dai::IMUSensor::ROTATION_VECTOR, 400);
24    // it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
25    // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
26    imu->setBatchReportThreshold(1);
27    // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
28    // if lower or equal to batchReportThreshold then the sending is always blocking on device
29    // useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
30    imu->setMaxBatchReports(10);
31
32    // Link plugins IMU -> XLINK
33    imu->out.link(xlinkOut->input);
34
35    dai::Device d(pipeline);
36
37    auto imuQueue = d.getOutputQueue("imu", 50, false);
38    auto baseTs = steady_clock::now();
39
40    while(true) {
41        auto imuData = imuQueue->get<dai::IMUData>();
42
43        auto imuPackets = imuData->packets;
44        for(auto& imuPacket : imuPackets) {
45            auto& rVvalues = imuPacket.rotationVector;
46
47            auto rvTs = rVvalues.getTimestampDevice() - baseTs;
48            printf("Rotation vector timestamp: %ld ms\n", static_cast<long>(duration_cast<milliseconds>(rvTs).count()));
49
50            printf(
51                "Quaternion: i: %.3f j: %.3f k: %.3f real: %.3f\n"
52                "Accuracy (rad): %.3f \n",
53                rVvalues.i,
54                rVvalues.j,
55                rVvalues.k,
56                rVvalues.real,
57                rVvalues.rotationVectorAccuracy);
58        }
59
60        int key = cv::waitKey(1);
61        if(key == 'q') {
62            return 0;
63        }
64    }
65
66    return 0;
67}