K

koonpl

I ran the following code with the OAK-D laying level on the heat sink. With the left (Starboard) camera pointed toward magnetic north the IMU message was all zeros. That makes me believe the BNO085 is mounted with the dot in the upper right hand corner so that it is physically rotated 90 degrees from its standard orientation that the SH-2 firmware assumes.

#!/usr/bin/env python3
import cv2
import depthai as dai
import time
import math

import numpy as np

def euler_from_quaternion(w, x, y, z):
        """
        Convert a quaternion into euler angles (roll, pitch, yaw)
        roll is rotation around x in radians (counterclockwise)
        pitch is rotation around y in radians (counterclockwise)
        yaw is rotation around z in radians (counterclockwise)
        """
        t0 = +2.0 * (w * x + y * z)
        t1 = +1.0 - 2.0 * (x * x + y * y)
        roll_x = math.degrees(math.atan2(t0, t1))
     

        t2 = +2.0 * (w * y - z * x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        pitch_y = math.degrees(math.asin(t2))
     

        t3 = +2.0 * (w * z + x * y)
        t4 = +1.0 - 2.0 * (y * y + z * z)
        yaw_z = math.degrees(math.atan2(t3, t4))
     

        return pitch_y, yaw_z, roll_x

def quaternion_to_euler_angle(qr, qi, qj, qk):
    jsqr = qj * qj
    isqr = qi * qi
    ksqr = qk * qk
    rsqr = qr * qr

    # Yaw  ypr->yaw = atan2(2.0 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));    t0 = 2.0 * (qi * qj + qk * qr)
    t0 = 2.0 * (qi * qj + qk * qr)
    t1 = isqr - jsqr - ksqr + rsqr
    yaw = np.degrees(np.arctan2(t0, t1))

    # Pitch  ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
    t2 = -2.0 * (qi * qk - qj * qr)/(isqr + jsqr + ksqr + rsqr)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch = np.degrees(np.arcsin(t2))  
    

    # Z  ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));   
    t3 = +2.0 * (qj * qk + qi * qr)
    t4 = 1 - 2 * (-isqr -jsqr + ksqr + rsqr)
    #Roll
    roll = np.degrees(np.arctan2(t3, t4))

    return pitch, yaw, roll 

# Create pipeline
pipeline = dai.Pipeline()

# Define sources and outputs
imu = pipeline.create(dai.node.IMU)
imuOut = pipeline.create(dai.node.XLinkOut)

imuOut.setStreamName("imu")

# enable ROTATION_VECTOR at 400 hz rate ARVR_STABILIZED_GAME_ROTATION_VECTOR
imu.enableIMUSensor(dai.IMUSensor.ROTATION_VECTOR, 10)
# above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
imu.setBatchReportThreshold(1)
# maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
# if lower or equal to batchReportThreshold then the sending is always blocking on device
# useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
imu.setMaxBatchReports(10)

# Link plugins IMU -> XLINK
imu.out.link(imuOut.input)

# Pipeline is defined, now we can connect to the device
with dai.Device(pipeline) as device:

    def timeDeltaToMilliS(delta) -> float:
        return delta.total_seconds()*1000

    # Output queue for imu bulk packets
    imuQueue = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
    baseTs = None
    while True:
        imuData = imuQueue.get()  # blocking call, will wait until a new data has arrived

        imuPackets = imuData.packets
        for imuPacket in imuPackets:
            rVvalues = imuPacket.rotationVector
            pitch, yaw, roll = euler_from_quaternion(
                rVvalues.real, rVvalues.i, rVvalues.j, rVvalues.k)
            # pitch, yaw, roll = quaternion_to_euler_angle(
            #     rVvalues.real, rVvalues.i, rVvalues.j, rVvalues.k)

            rvTs = rVvalues.timestamp.get()
            if baseTs is None:
                baseTs = rvTs
            rvTs = rvTs - baseTs

            imuF = "{:.06f}"
            tsF  = "{:.03f}"

            # print(f"Rotation vector timestamp: {tsF.format(timeDeltaToMilliS(rvTs))} ms")
            # print(f"Quaternion: i: {imuF.format(rVvalues.i)} j: {imuF.format(rVvalues.j)} "
            #     f"k: {imuF.format(rVvalues.k)} real: {imuF.format(rVvalues.real)}")
            print(f"Euler: pitch: {imuF.format(pitch)} yaw: {imuF.format(yaw)} "
                f"roll: {imuF.format(roll)} Accuracy (rad): {imuF.format(rVvalues.rotationVectorAccuracy)}")
            # print(f"Accuracy (rad): {imuF.format(rVvalues.rotationVectorAccuracy)}")
        if cv2.waitKey(1) == ord('q'):
            break`

I asked about setting IMU's static calibration record or the Sensor Orientation Record here, oak-d-bno085-imu-coordinate-frame. Unfortunately, this capabiility is not currently exposed in the IMU node. I was also told the static calibration record has not been set. Therefore, the BNO085 chip is still reporting the default configuration reference of East, North, Up, This means when it is level and the Y axis is pointing towards magnetic north the rotation vector will be ll zeros. The image of the OAK-D circuit board shows the chip is rotated 90 degrees clockwise from the default orientation so it is in physical South, East, Up configuration. I am basing this off of the dot in the image above. Is there a wish list for depthai features? Can adding the functions to set the configuration records on the BNO085 to the list?

Hi, I am a new user of the OAK-D. I am trying to understand the coordinate frame of the IMU. I read the datasheet for the BNO085 and i states "The BNO08X needs to be configured to understand what the orientation is relative to the device frame of reference. This is achieved by modifying either the static calibration record if the device is passed through a calibration step or by modifying the sensor orientation records." The datasheet also has the following image showing its default coordinate frame reference. I have not found any examples of reading the static calibration record or modifying the orientation record. Does anyone have example code for this? Also, the data sheet also states there are tare (zero) functions built into the sh2 firmware. Does anyone have examples calling the tares? Any help will be greatly appreciated, thanks!