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!

    koonpl

    I have not found any examples of reading the static calibration record or modifying the orientation record.

    Also, the data sheet also states there are tare (zero) functions built into the sh2 firmware.

    These options are not exposed at the moment.


    The default coordinate frame reference is used.

        GergelySzabolcs The default coordinate frame reference is used.

        That doesn't seem to be the case for the OAK-D-IOT-75 board. By monitoring accelerometer output and rotating the board, it seems to me that there is a 90 degrees rotation about the Z axis compared to the default which is also printed on the PCB. Can anyone confirm this or otherwise?

          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`