• HardwareROS

  • BMO086 IMU Rotation Vector: How to calibrate when 90° pitched?

Hi,

i got a OAK-D PPOE (luckily an old one with BMO086). When the camera is put flat on the desk, camera looking up, everything is fine.
Running code from the demo with: imu.enableIMUSensor(dai.IMUSensor.ROTATION_VECTOR, 10)

https://docs.luxonis.com/projects/api/en/latest/samples/IMU/imu_rotation_vector/#imu-rotation-vector

BUT: on my robot the camera is pitched ~100°. I tried to configure the camera pose in the URDF in ROS2, but it looks like it gets into a gimbal lock (a 1-5° motion flips the robot). So i need to either a) tell the camera its pose, or/and b) get the quarternion and rotate it accordingly. But this would be great to get done automatically (gravity down, north…)

My question is: How and where do i configure the global pose of the camera, so that
Z- = Image direction = roll,
Y+ = up (- Gravity) = yaw,
X+ = left = pitch

oh, and another question: is there a marketplace/trading forum, where i can trade my 600€ Oak-d-pro-W POE with a 6-axis IMU against a 9-axis version?

thx,
Walter

    Hi WalterWohlkinger
    There currently isn't an easy way to do it.
    Here is some code that hopes to achieve what you are looking for:

    from depthai_sdk import OakCamera
from depthai_sdk.classes.packets import IMUPacket, FramePacket
import depthai as dai
from ahrs.filters import Mahony
import numpy as np

def rotation_matrix(axis, angle_degree):
    angle = np.deg2rad(angle_degree)
    if axis == 'roll': # x
        return np.array([
            [1, 0, 0],
            [0, np.cos(angle), -np.sin(angle)],
            [0, np.sin(angle), np.cos(angle)]
        ])
    elif axis == 'pitch': # y
        return np.array([
            [np.cos(angle), 0, np.sin(angle)],
            [0, 1, 0],
            [-np.sin(angle), 0, np.cos(angle)]
        ])
    elif axis == 'yaw': # x
        return np.array([
            [np.cos(angle), -np.sin(angle), 0],
            [np.sin(angle), np.cos(angle), 0],
            [0, 0, 1]
        ])

mahony = Mahony(frequency=100)
mahony.Q = np.array([1, 0, 0, 0], dtype=np.float64)



BNO_OAK_D_POE = np.dot(
    rotation_matrix('roll', 270),
    rotation_matrix('pitch', 270)
)


R_combined = BNO_OAK_D_POE

with OakCamera() as oak:
    imu = oak.create_imu()
    print(oak.device.getConnectedIMU())
    imu.config_imu(report_rate=400, batch_report_threshold=1)
    color = oak.create_camera("color", dai.ColorCameraProperties.SensorResolution.THE_1080_P)
    intrinsics = oak.device.readCalibration().getCameraIntrinsics(dai.CameraBoardSocket.LEFT, dai.Size2f(1920, 1080))

    def imu_callback(packet: IMUPacket):
        for d in packet.data:
            gyro: dai.IMUReportGyroscope = d.gyroscope
            accel: dai.IMUReportAccelerometer = d.acceleroMeter
            gryo_rotated = np.dot(R_combined, np.array([gyro.x, gyro.y, gyro.z]))
            accel_rotated = np.dot(R_combined, np.array([accel.x, accel.y, accel.z]))
            mahony.Q = mahony.updateIMU(mahony.Q, gryo_rotated, accel_rotated)
            print(mahony.Q)

   
    oak.callback(imu.out.main, callback=imu_callback)
    oak.start(blocking=True)

    Make sure the rotation matrix -> R_combined makes sense for your IMU setup.

    Hope it helps,
    Jaka

      Thank you Jaka,

      I will give this a try.

      I am just wondering how such a great feature of a global pose (gravity + north) is used by others.
      @erik: There is a "tare" function in the manual of the BNO086 which does exactly that. any chance this will be implemented?

      "This document describes the Tare function of the BNO080/BNO085/BNO086 that redefines the orientation of the sensor. This allows the outputs of the BNO080/BNO085/BNO086 to be in line with the orientation with which it was mounted into the main device.
      These commands are described in more detail in the SH-2 Reference Manual [1] and this document assumes that the reader has this manual available for reference. Tare Overview The BNO080/BNO085/BNO086 uses two commands and a configuration record to redefine the sensor’s orientation, or “Tare”: - Commands:

      o Tare Now (Section 6.4.3.1 of SH-2 Reference Manual) ▪ Allows for a temporary Tare. Useful at the start of a game to define the current orientation.
      o Persist Tare (Section 6.4.3.2 of SH-2 Reference Manual) ▪ Saves the current Tare orientation into the Sensor Orientation configuration record. This is a persistent setting that now becomes the default orientation. - Configuration Record o Sensor Orientation ▪ Stores the orientation of the device. Once you know the standard orientation of your BNO080/BNO085/BNO086 in your device, you can use the same starting orientation for all devices using this record as a convenient starting point. More accurate per-device taring can be done using the commands above.
      "

      found here: https://device.report/m/2617273e2d1c2265e9b3d7349ecf3af9e247b6357aceb8936508077561cf4e9c

      • erik replied to this.