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
